flight/uavtalk/uavtalk.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotSystem OpenPilot System
   4  * @{
   5  * @addtogroup OpenPilotLibraries OpenPilot System Libraries
   6  * @{
   7  *
   8  * @file       uavtalk.c
   9  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  10  * @brief      UAVTalk library, implements to telemetry protocol. See the wiki for more details.
  11  *             This library should not be called directly by the application, it is only used by the
  12  *             Telemetry module.
  13  * @see        The GNU Public License (GPL) Version 3
  14  *
  15  *****************************************************************************/
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 #include "openpilot.h"
  33 #include "uavtalk_priv.h"
  34
  35 // #define UAV_DEBUGLOG 1
  36
  37 #if defined UAV_DEBUGLOG && defined FLASH_FREERTOS
  38 #define UAVT_DEBUGLOG_PRINTF(...) PIOS_DEBUGLOG_Printf(__VA_ARGS__)
  39 // uncomment and adapt the following lines to filter verbose logging to include specific object(s) only
  40 // #include "flighttelemetrystats.h"
  41 // #define UAVT_DEBUGLOG_CPRINTF(objId, ...) if (objId == FLIGHTTELEMETRYSTATS_OBJID) { UAVT_DEBUGLOG_PRINTF(__VA_ARGS__); }
  42 #endif
  43 #ifndef UAVT_DEBUGLOG_PRINTF
  44 #define UAVT_DEBUGLOG_PRINTF(...)
  45 #endif
  46 #ifndef UAVT_DEBUGLOG_CPRINTF
  47 #define UAVT_DEBUGLOG_CPRINTF(objId, ...)
  48 #endif
  49
  50 // Private functions
  51 static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeout);
  52 static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj);
  53 static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj);
  54 static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data);
  55 static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId);
  56
  57 /**
  58  * Initialize the UAVTalk library
  59  * \param[in] connection UAVTalkConnection to be used
  60  * \param[in] outputStream Function pointer that is called to send a data buffer
  61  * \return 0 Success
  62  * \return -1 Failure
  63  */
  64 UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream)
  65 {
  66     // allocate object
  67     UAVTalkConnectionData *connection = pios_malloc(sizeof(UAVTalkConnectionData));
  68
  69     if (!connection) {
  70         return 0;
  71     }
  72     connection->canari      = UAVTALK_CANARI;
  73     connection->iproc.rxPacketLength = 0;
  74     connection->iproc.state = UAVTALK_STATE_SYNC;
  75     connection->outStream   = outputStream;
  76     connection->lock = xSemaphoreCreateRecursiveMutex();
  77     connection->transLock   = xSemaphoreCreateRecursiveMutex();
  78     // allocate buffers
  79     connection->rxBuffer    = pios_malloc(UAVTALK_MAX_PACKET_LENGTH);
  80     if (!connection->rxBuffer) {
  81         return 0;
  82     }
  83     connection->txBuffer = pios_malloc(UAVTALK_MAX_PACKET_LENGTH);
  84     if (!connection->txBuffer) {
  85         return 0;
  86     }
  87     vSemaphoreCreateBinary(connection->respSema);
  88     xSemaphoreTake(connection->respSema, 0); // reset to zero
  89     UAVTalkResetStats((UAVTalkConnection)connection);
  90     return (UAVTalkConnection)connection;
  91 }
  92
  93 /**
  94  * Set the communication output stream
  95  * \param[in] connection UAVTalkConnection to be used
  96  * \param[in] outputStream Function pointer that is called to send a data buffer
  97  * \return 0 Success
  98  * \return -1 Failure
  99  */
 100 int32_t UAVTalkSetOutputStream(UAVTalkConnection connectionHandle, UAVTalkOutputStream outputStream)
 101 {
 102     UAVTalkConnectionData *connection;
 103
 104     CHECKCONHANDLE(connectionHandle, connection, return -1);
 105
 106     // Lock
 107     xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
 108
 109     // set output stream
 110     connection->outStream = outputStream;
 111
 112     // Release lock
 113     xSemaphoreGiveRecursive(connection->lock);
 114
 115     return 0;
 116 }
 117
 118 /**
 119  * Get current output stream
 120  * \param[in] connection UAVTalkConnection to be used
 121  * @return UAVTarlkOutputStream the output stream used
 122  */
 123 UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connectionHandle)
 124 {
 125     UAVTalkConnectionData *connection;
 126
 127     CHECKCONHANDLE(connectionHandle, connection, return NULL);
 128     return connection->outStream;
 129 }
 130
 131 /**
 132  * Get communication statistics counters
 133  * \param[in] connection UAVTalkConnection to be used
 134  * @param[out] statsOut Statistics counters
 135  */
 136 void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut, bool reset)
 137 {
 138     UAVTalkConnectionData *connection;
 139
 140     CHECKCONHANDLE(connectionHandle, connection, return );
 141
 142     // Lock
 143     xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
 144
 145     // Copy stats
 146     memcpy(statsOut, &connection->stats, sizeof(UAVTalkStats));
 147
 148     if (reset) {
 149         // Clear stats
 150         memset(&connection->stats, 0, sizeof(UAVTalkStats));
 151     }
 152
 153     // Release lock
 154     xSemaphoreGiveRecursive(connection->lock);
 155 }
 156
 157 /**
 158  * Get communication statistics counters
 159  * \param[in] connection UAVTalkConnection to be used
 160  * @param[out] statsOut Statistics counters
 161  */
 162 void UAVTalkAddStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut, bool reset)
 163 {
 164     UAVTalkConnectionData *connection;
 165
 166     CHECKCONHANDLE(connectionHandle, connection, return );
 167
 168     // Lock
 169     xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
 170
 171     // Copy stats
 172     statsOut->txBytes       += connection->stats.txBytes;
 173     statsOut->txObjectBytes += connection->stats.txObjectBytes;
 174     statsOut->txObjects     += connection->stats.txObjects;
 175     statsOut->txErrors      += connection->stats.txErrors;
 176     statsOut->rxBytes       += connection->stats.rxBytes;
 177     statsOut->rxObjectBytes += connection->stats.rxObjectBytes;
 178     statsOut->rxObjects     += connection->stats.rxObjects;
 179     statsOut->rxErrors      += connection->stats.rxErrors;
 180     statsOut->rxSyncErrors  += connection->stats.rxSyncErrors;
 181     statsOut->rxCrcErrors   += connection->stats.rxCrcErrors;
 182
 183     if (reset) {
 184         // Clear stats
 185         memset(&connection->stats, 0, sizeof(UAVTalkStats));
 186     }
 187
 188     // Release lock
 189     xSemaphoreGiveRecursive(connection->lock);
 190 }
 191
 192 /**
 193  * Reset the statistics counters.
 194  * \param[in] connection UAVTalkConnection to be used
 195  */
 196 void UAVTalkResetStats(UAVTalkConnection connectionHandle)
 197 {
 198     UAVTalkConnectionData *connection;
 199
 200     CHECKCONHANDLE(connectionHandle, connection, return );
 201
 202     // Lock
 203     xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
 204
 205     // Clear stats
 206     memset(&connection->stats, 0, sizeof(UAVTalkStats));
 207
 208     // Release lock
 209     xSemaphoreGiveRecursive(connection->lock);
 210 }
 211
 212 /**
 213  * Accessor method to get the timestamp from the last UAVTalk message
 214  */
 215 void UAVTalkGetLastTimestamp(UAVTalkConnection connectionHandle, uint16_t *timestamp)
 216 {
 217     UAVTalkConnectionData *connection;
 218
 219     CHECKCONHANDLE(connectionHandle, connection, return );
 220
 221     UAVTalkInputProcessor *iproc = &connection->iproc;
 222     *timestamp = iproc->timestamp;
 223 }
 224
 225 /**
 226  * Request an update for the specified object, on success the object data would have been
 227  * updated by the GCS.
 228  * \param[in] connection UAVTalkConnection to be used
 229  * \param[in] obj Object to update
 230  * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances.
 231  * \param[in] timeout Time to wait for the response, when zero it will return immediately
 232  * \return 0 Success
 233  * \return -1 Failure
 234  */
 235 int32_t UAVTalkSendObjectRequest(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, int32_t timeout)
 236 {
 237     UAVTalkConnectionData *connection;
 238
 239     CHECKCONHANDLE(connectionHandle, connection, return -1);
 240
 241     return objectTransaction(connection, UAVTALK_TYPE_OBJ_REQ, obj, instId, timeout);
 242 }
 243
 244 /**
 245  * Send the specified object through the telemetry link.
 246  * \param[in] connection UAVTalkConnection to be used
 247  * \param[in] obj Object to send
 248  * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances.
 249  * \param[in] acked Selects if an ack is required (1:ack required, 0: ack not required)
 250  * \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately
 251  * \return 0 Success
 252  * \return -1 Failure
 253  */
 254 int32_t UAVTalkSendObject(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs)
 255 {
 256     UAVTalkConnectionData *connection;
 257
 258     CHECKCONHANDLE(connectionHandle, connection, return -1);
 259
 260     // Send object
 261     if (acked == 1) {
 262         return objectTransaction(connection, UAVTALK_TYPE_OBJ_ACK, obj, instId, timeoutMs);
 263     } else {
 264         return objectTransaction(connection, UAVTALK_TYPE_OBJ, obj, instId, timeoutMs);
 265     }
 266 }
 267
 268 /**
 269  * Send the specified object through the telemetry link with a timestamp.
 270  * \param[in] connection UAVTalkConnection to be used
 271  * \param[in] obj Object to send
 272  * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances.
 273  * \param[in] acked Selects if an ack is required (1:ack required, 0: ack not required)
 274  * \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately
 275  * \return 0 Success
 276  * \return -1 Failure
 277  */
 278 int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs)
 279 {
 280     UAVTalkConnectionData *connection;
 281
 282     CHECKCONHANDLE(connectionHandle, connection, return -1);
 283
 284     // Send object
 285     if (acked == 1) {
 286         return objectTransaction(connection, UAVTALK_TYPE_OBJ_ACK_TS, obj, instId, timeoutMs);
 287     } else {
 288         return objectTransaction(connection, UAVTALK_TYPE_OBJ_TS, obj, instId, timeoutMs);
 289     }
 290 }
 291
 292 /**
 293  * Execute the requested transaction on an object.
 294  * \param[in] connection UAVTalkConnection to be used
 295  * \param[in] type Transaction type
 296  *                        UAVTALK_TYPE_OBJ: send object,
 297  *                        UAVTALK_TYPE_OBJ_REQ: request object update
 298  *                        UAVTALK_TYPE_OBJ_ACK: send object with an ack
 299  * \param[in] obj Object
 300  * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
 301  * \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately
 302  * \return 0 Success
 303  * \return -1 Failure
 304  */
 305 static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs)
 306 {
 307     int32_t respReceived;
 308     int32_t ret = -1;
 309
 310     // Send object depending on if a response is needed
 311     if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS || type == UAVTALK_TYPE_OBJ_REQ) {
 312         // Get transaction lock (will block if a transaction is pending)
 313         xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY);
 314         // Send object
 315         xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
 316         // expected response type
 317         connection->respType   = (type == UAVTALK_TYPE_OBJ_REQ) ? UAVTALK_TYPE_OBJ : UAVTALK_TYPE_ACK;
 318         connection->respObjId  = UAVObjGetID(obj);
 319         connection->respInstId = instId;
 320         ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj);
 321         xSemaphoreGiveRecursive(connection->lock);
 322         // Wait for response (or timeout) if sending the object succeeded
 323         respReceived = pdFALSE;
 324         if (ret == 0) {
 325             respReceived = xSemaphoreTake(connection->respSema, timeoutMs / portTICK_RATE_MS);
 326         }
 327         // Check if a response was received
 328         if (respReceived == pdTRUE) {
 329             // We are done successfully
 330             xSemaphoreGiveRecursive(connection->transLock);
 331             ret = 0;
 332         } else {
 333             // Cancel transaction
 334             xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
 335             // non blocking call to make sure the value is reset to zero (binary sema)
 336             xSemaphoreTake(connection->respSema, 0);
 337             connection->respObjId = 0;
 338             xSemaphoreGiveRecursive(connection->lock);
 339             xSemaphoreGiveRecursive(connection->transLock);
 340             return -1;
 341         }
 342     } else if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS) {
 343         xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
 344         ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj);
 345         xSemaphoreGiveRecursive(connection->lock);
 346     }
 347     return ret;
 348 }
 349
 350 /**
 351  * Process an byte from the telemetry stream.
 352  * \param[in] connectionHandle UAVTalkConnection to be used
 353  * \param[in] rxbyte Received byte
 354  * \return UAVTalkRxState
 355  */
 356 UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t rxbyte)
 357 {
 358     UAVTalkConnectionData *connection;
 359
 360     CHECKCONHANDLE(connectionHandle, connection, return -1);
 361
 362     UAVTalkInputProcessor *iproc = &connection->iproc;
 363
 364     ++connection->stats.rxBytes;
 365
 366     if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) {
 367         iproc->state = UAVTALK_STATE_SYNC;
 368     }
 369
 370     if (iproc->rxPacketLength < 0xffff) {
 371         // update packet byte count
 372         iproc->rxPacketLength++;
 373     }
 374
 375     // Receive state machine
 376     switch (iproc->state) {
 377     case UAVTALK_STATE_SYNC:
 378
 379         if (rxbyte != UAVTALK_SYNC_VAL) {
 380             connection->stats.rxSyncErrors++;
 381             break;
 382         }
 383
 384         // Initialize and update the CRC
 385         iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
 386
 387         iproc->rxPacketLength = 1;
 388         iproc->rxCount = 0;
 389
 390         iproc->type    = 0;
 391         iproc->state   = UAVTALK_STATE_TYPE;
 392         break;
 393
 394     case UAVTALK_STATE_TYPE:
 395
 396         if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) {
 397             connection->stats.rxErrors++;
 398             iproc->state = UAVTALK_STATE_SYNC;
 399             break;
 400         }
 401
 402         // update the CRC
 403         iproc->cs    = PIOS_CRC_updateByte(iproc->cs, rxbyte);
 404
 405         iproc->type  = rxbyte;
 406
 407         iproc->packet_size = 0;
 408         iproc->state = UAVTALK_STATE_SIZE;
 409         break;
 410
 411     case UAVTALK_STATE_SIZE:
 412
 413         // update the CRC
 414         iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
 415
 416         if (iproc->rxCount == 0) {
 417             iproc->packet_size += rxbyte;
 418             iproc->rxCount++;
 419             break;
 420         }
 421         iproc->packet_size += rxbyte << 8;
 422         iproc->rxCount      = 0;
 423
 424         if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) {
 425             // incorrect packet size
 426             connection->stats.rxErrors++;
 427             iproc->state = UAVTALK_STATE_ERROR;
 428             break;
 429         }
 430
 431         iproc->objId = 0;
 432         iproc->state = UAVTALK_STATE_OBJID;
 433         break;
 434
 435     case UAVTALK_STATE_OBJID:
 436
 437         // update the CRC
 438         iproc->cs     = PIOS_CRC_updateByte(iproc->cs, rxbyte);
 439
 440         iproc->objId += rxbyte << (8 * (iproc->rxCount++));
 441         if (iproc->rxCount < 4) {
 442             break;
 443         }
 444         iproc->rxCount = 0;
 445
 446         iproc->instId  = 0;
 447         iproc->state   = UAVTALK_STATE_INSTID;
 448         break;
 449
 450     case UAVTALK_STATE_INSTID:
 451
 452         // update the CRC
 453         iproc->cs      = PIOS_CRC_updateByte(iproc->cs, rxbyte);
 454
 455         iproc->instId += rxbyte << (8 * (iproc->rxCount++));
 456         if (iproc->rxCount < 2) {
 457             break;
 458         }
 459         iproc->rxCount = 0;
 460
 461         UAVObjHandle obj = UAVObjGetByID(iproc->objId);
 462
 463         // Determine data length
 464         if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) {
 465             iproc->length = 0;
 466             iproc->timestampLength = 0;
 467         } else {
 468             iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
 469             if (obj) {
 470                 iproc->length = UAVObjGetNumBytes(obj);
 471             } else {
 472                 iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength;
 473             }
 474         }
 475
 476         // Check length
 477         if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
 478             // packet error - exceeded payload max length
 479             connection->stats.rxErrors++;
 480             iproc->state = UAVTALK_STATE_ERROR;
 481             break;
 482         }
 483
 484         // Check the lengths match
 485         if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) {
 486             // packet error - mismatched packet size
 487             connection->stats.rxErrors++;
 488             iproc->state = UAVTALK_STATE_ERROR;
 489             break;
 490         }
 491
 492         // Determine next state
 493         if (iproc->type & UAVTALK_TIMESTAMPED) {
 494             // If there is a timestamp get it
 495             iproc->timestamp = 0;
 496             iproc->state     = UAVTALK_STATE_TIMESTAMP;
 497         } else {
 498             // If there is a payload get it, otherwise receive checksum
 499             if (iproc->length > 0) {
 500                 iproc->state = UAVTALK_STATE_DATA;
 501             } else {
 502                 iproc->state = UAVTALK_STATE_CS;
 503             }
 504         }
 505         break;
 506
 507     case UAVTALK_STATE_TIMESTAMP:
 508
 509         // update the CRC
 510         iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
 511
 512         iproc->timestamp += rxbyte << (8 * (iproc->rxCount++));
 513         if (iproc->rxCount < 2) {
 514             break;
 515         }
 516         iproc->rxCount = 0;
 517
 518         // If there is a payload get it, otherwise receive checksum
 519         if (iproc->length > 0) {
 520             iproc->state = UAVTALK_STATE_DATA;
 521         } else {
 522             iproc->state = UAVTALK_STATE_CS;
 523         }
 524         break;
 525
 526     case UAVTALK_STATE_DATA:
 527
 528         // update the CRC
 529         iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
 530
 531         connection->rxBuffer[iproc->rxCount++] = rxbyte;
 532         if (iproc->rxCount < iproc->length) {
 533             break;
 534         }
 535         iproc->rxCount = 0;
 536
 537         iproc->state   = UAVTALK_STATE_CS;
 538         break;
 539
 540     case UAVTALK_STATE_CS:
 541
 542         // Check the CRC byte
 543         if (rxbyte != iproc->cs) {
 544             // packet error - faulty CRC
 545             UAVT_DEBUGLOG_PRINTF("BAD CRC");
 546             connection->stats.rxCrcErrors++;
 547             connection->stats.rxErrors++;
 548             iproc->state = UAVTALK_STATE_ERROR;
 549             break;
 550         }
 551
 552         if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) {
 553             // packet error - mismatched packet size
 554             connection->stats.rxErrors++;
 555             iproc->state = UAVTALK_STATE_ERROR;
 556             break;
 557         }
 558
 559         connection->stats.rxObjects++;
 560         connection->stats.rxObjectBytes += iproc->length;
 561
 562         iproc->state = UAVTALK_STATE_COMPLETE;
 563         break;
 564
 565     default:
 566
 567         iproc->state = UAVTALK_STATE_ERROR;
 568         break;
 569     }
 570
 571     // Done
 572     return iproc->state;
 573 }
 574
 575 /**
 576  * Process an byte from the telemetry stream.
 577  * \param[in] connection UAVTalkConnection to be used
 578  * \param[in] rxbyte Received byte
 579  * \return UAVTalkRxState
 580  */
 581 UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte)
 582 {
 583     UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte);
 584
 585     if (state == UAVTALK_STATE_COMPLETE) {
 586         UAVTalkReceiveObject(connectionHandle);
 587     }
 588
 589     return state;
 590 }
 591
 592 /**
 593  * Send a parsed packet received on one connection handle out on a different connection handle.
 594  * The packet must be in a complete state, meaning it is completed parsing.
 595  * The packet is re-assembled from the component parts into a complete message and sent.
 596  * This can be used to relay packets from one UAVTalk connection to another.
 597  * \param[in] connection UAVTalkConnection to be used
 598  * \param[in] rxbyte Received byte
 599  * \return 0 Success
 600  * \return -1 Failure
 601  */
 602 int32_t UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle)
 603 {
 604     UAVTalkConnectionData *inConnection;
 605
 606     CHECKCONHANDLE(inConnectionHandle, inConnection, return -1);
 607     UAVTalkInputProcessor *inIproc = &inConnection->iproc;
 608
 609     // The input packet must be completely parsed.
 610     if (inIproc->state != UAVTALK_STATE_COMPLETE) {
 611         inConnection->stats.rxErrors++;
 612
 613         return -1;
 614     }
 615
 616     UAVTalkConnectionData *outConnection;
 617     CHECKCONHANDLE(outConnectionHandle, outConnection, return -1);
 618
 619     if (!outConnection->outStream) {
 620         outConnection->stats.txErrors++;
 621
 622         return -1;
 623     }
 624
 625     // Lock
 626     xSemaphoreTakeRecursive(outConnection->lock, portMAX_DELAY);
 627
 628     outConnection->txBuffer[0] = UAVTALK_SYNC_VAL;
 629     // Setup type
 630     outConnection->txBuffer[1] = inIproc->type;
 631     // next 2 bytes are reserved for data length (inserted here later)
 632     // Setup object ID
 633     outConnection->txBuffer[4] = (uint8_t)(inIproc->objId & 0xFF);
 634     outConnection->txBuffer[5] = (uint8_t)((inIproc->objId >> 8) & 0xFF);
 635     outConnection->txBuffer[6] = (uint8_t)((inIproc->objId >> 16) & 0xFF);
 636     outConnection->txBuffer[7] = (uint8_t)((inIproc->objId >> 24) & 0xFF);
 637     // Setup instance ID
 638     outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF);
 639     outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF);
 640     int32_t headerLength = 10;
 641
 642     // Add timestamp when the transaction type is appropriate
 643     if (inIproc->type & UAVTALK_TIMESTAMPED) {
 644         portTickType time = xTaskGetTickCount();
 645         outConnection->txBuffer[10] = (uint8_t)(time & 0xFF);
 646         outConnection->txBuffer[11] = (uint8_t)((time >> 8) & 0xFF);
 647         headerLength += 2;
 648     }
 649
 650     // Copy data (if any)
 651     if (inIproc->length > 0) {
 652         memcpy(&outConnection->txBuffer[headerLength], inConnection->rxBuffer, inIproc->length);
 653     }
 654
 655     // Store the packet length
 656     outConnection->txBuffer[2] = (uint8_t)((headerLength + inIproc->length) & 0xFF);
 657     outConnection->txBuffer[3] = (uint8_t)(((headerLength + inIproc->length) >> 8) & 0xFF);
 658
 659     // Copy the checksum
 660     outConnection->txBuffer[headerLength + inIproc->length] = inIproc->cs;
 661
 662     // Send the buffer.
 663     int32_t rc = (*outConnection->outStream)(outConnection->txBuffer, headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH);
 664
 665     // Update stats
 666     outConnection->stats.txBytes += (rc > 0) ? rc : 0;
 667
 668     // evaluate return value before releasing the lock
 669     int32_t ret = 0;
 670     if (rc != (int32_t)(headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH)) {
 671         outConnection->stats.txErrors++;
 672         ret = -1;
 673     }
 674
 675     // Release lock
 676     xSemaphoreGiveRecursive(outConnection->lock);
 677
 678     // Done
 679     return ret;
 680 }
 681
 682 /**
 683  * Complete receiving a UAVTalk packet.  This will cause the packet to be unpacked, acked, etc.
 684  * \param[in] connectionHandle UAVTalkConnection to be used
 685  * \return 0 Success
 686  * \return -1 Failure
 687  */
 688 int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle)
 689 {
 690     UAVTalkConnectionData *connection;
 691
 692     CHECKCONHANDLE(connectionHandle, connection, return -1);
 693
 694     UAVTalkInputProcessor *iproc = &connection->iproc;
 695     if (iproc->state != UAVTALK_STATE_COMPLETE) {
 696         return -1;
 697     }
 698
 699     return receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer);
 700 }
 701
 702 /**
 703  * Get the object ID of the current packet.
 704  * \param[in] connectionHandle UAVTalkConnection to be used
 705  * \return The object ID, or 0 on error.
 706  */
 707 uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle)
 708 {
 709     UAVTalkConnectionData *connection;
 710
 711     CHECKCONHANDLE(connectionHandle, connection, return 0);
 712
 713     return connection->iproc.objId;
 714 }
 715
 716 /**
 717  * Receive an object. This function process objects received through the telemetry stream.
 718  *
 719  * Parser errors are considered as transmission errors and are not NACKed.
 720  * Some senders (GCS) can timeout and retry if the message is not answered by an ack or nack.
 721  *
 722  * Object handling errors are considered as application errors and are NACked.
 723  * In that case we want to nack as there is no point in the sender retrying to send invalid objects.
 724  *
 725  * \param[in] connection UAVTalkConnection to be used
 726  * \param[in] type Type of received message (UAVTALK_TYPE_OBJ, UAVTALK_TYPE_OBJ_REQ, UAVTALK_TYPE_OBJ_ACK, UAVTALK_TYPE_ACK, UAVTALK_TYPE_NACK)
 727  * \param[in] objId ID of the object to work on
 728  * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
 729  * \param[in] data Data buffer
 730  * \param[in] length Buffer length
 731  * \return 0 Success
 732  * \return -1 Failure
 733  */
 734 static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data)
 735 {
 736     UAVObjHandle obj;
 737     int32_t ret = 0;
 738
 739     // Lock
 740     xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
 741
 742     // Get the handle to the object. Will be null if object does not exist.
 743     // Warning :
 744     // Here we ask for instance ID 0 without taking into account the provided instId
 745     // The provided instId will be used later when packing, unpacking, etc...
 746     // TODO the above should be fixed as it is cumbersome and error prone
 747     obj = UAVObjGetByID(objId);
 748
 749     // Process message type
 750     switch (type) {
 751     case UAVTALK_TYPE_OBJ:
 752     case UAVTALK_TYPE_OBJ_TS:
 753         // All instances not allowed for OBJ messages
 754         if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
 755             // Unpack object, if the instance does not exist it will be created!
 756             if (UAVObjUnpack(obj, instId, data) == 0) {
 757                 // Check if this object acks a pending OBJ_REQ message
 758                 // any OBJ message can ack a pending OBJ_REQ message
 759                 // even one that was not sent in response to the OBJ_REQ message
 760                 updateAck(connection, type, objId, instId);
 761             } else {
 762                 ret = -1;
 763             }
 764         } else {
 765             ret = -1;
 766         }
 767         break;
 768
 769     case UAVTALK_TYPE_OBJ_ACK:
 770     case UAVTALK_TYPE_OBJ_ACK_TS:
 771         UAVT_DEBUGLOG_CPRINTF(objId, "OBJ_ACK %X %d", objId, instId);
 772         // All instances not allowed for OBJ_ACK messages
 773         if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
 774             // Unpack object, if the instance does not exist it will be created!
 775             if (UAVObjUnpack(obj, instId, data) == 0) {
 776                 UAVT_DEBUGLOG_CPRINTF(objId, "OBJ ACK %X %d", objId, instId);
 777                 // Object updated or created, transmit ACK
 778                 sendObject(connection, UAVTALK_TYPE_ACK, objId, instId, NULL);
 779             } else {
 780                 ret = -1;
 781             }
 782         } else {
 783             ret = -1;
 784         }
 785         if (ret == -1) {
 786             // failed to update object, transmit NACK
 787             UAVT_DEBUGLOG_PRINTF("OBJ NACK %X %d", objId, instId);
 788             sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
 789         }
 790         break;
 791
 792     case UAVTALK_TYPE_OBJ_REQ:
 793         // Check if requested object exists
 794         UAVT_DEBUGLOG_CPRINTF(objId, "REQ %X %d", objId, instId);
 795         if (obj) {
 796             // Object found, transmit it
 797             // The sent object will ack the object request on the receiver side
 798             ret = sendObject(connection, UAVTALK_TYPE_OBJ, objId, instId, obj);
 799         } else {
 800             ret = -1;
 801         }
 802         if (ret == -1) {
 803             // failed to send object, transmit NACK
 804             UAVT_DEBUGLOG_PRINTF("REQ NACK %X %d", objId, instId);
 805             sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
 806         }
 807         break;
 808
 809     case UAVTALK_TYPE_NACK:
 810         // Do nothing on flight side, let it time out.
 811         // TODO:
 812         // The transaction takes the result code of the "semaphore taking operation" into account to determine success.
 813         // If we give that semaphore in time, its "success" (ack received)
 814         // If we do not give that semaphore before the timeout it will return failure.
 815         // What would have to be done here is give the semaphore, but set a flag (for example connection->respFail=true)
 816         // that indicates failure and then above where it checks for the result code, have it behave as if it failed
 817         // if the explicit failure is set.
 818         break;
 819
 820     case UAVTALK_TYPE_ACK:
 821         // All instances not allowed for ACK messages
 822         if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
 823             // Check if an ACK is pending
 824             updateAck(connection, type, objId, instId);
 825         } else {
 826             ret = -1;
 827         }
 828         break;
 829
 830     default:
 831         ret = -1;
 832     }
 833
 834     // Unlock
 835     xSemaphoreGiveRecursive(connection->lock);
 836
 837     // Done
 838     return ret;
 839 }
 840
 841 /**
 842  * Check if an ack is pending on an object and give response semaphore
 843  * \param[in] connection UAVTalkConnection to be used
 844  * \param[in] obj Object
 845  * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
 846  */
 847 static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId)
 848 {
 849     if ((connection->respObjId == objId) && (connection->respType == type)) {
 850         if ((connection->respInstId == UAVOBJ_ALL_INSTANCES) && (instId == 0)) {
 851             // last instance received, complete transaction
 852             xSemaphoreGive(connection->respSema);
 853             connection->respObjId = 0;
 854         } else if (connection->respInstId == instId) {
 855             xSemaphoreGive(connection->respSema);
 856             connection->respObjId = 0;
 857         }
 858     }
 859 }
 860
 861 /**
 862  * Send an object through the telemetry link.
 863  * \param[in] connection UAVTalkConnection to be used
 864  * \param[in] type Transaction type
 865  * \param[in] objId The object ID
 866  * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances
 867  * \param[in] obj Object handle to send (null when type is NACK)
 868  * \return 0 Success
 869  * \return -1 Failure
 870  */
 871 static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj)
 872 {
 873     uint32_t numInst;
 874     uint32_t n;
 875     int32_t ret = -1;
 876
 877     // Important note : obj can be null (when type is NACK for example) so protect all obj dereferences.
 878
 879     // If all instances are requested and this is a single instance object, force instance ID to zero
 880     if ((obj != NULL) && (instId == UAVOBJ_ALL_INSTANCES) && UAVObjIsSingleInstance(obj)) {
 881         instId = 0;
 882     }
 883
 884     // Process message type
 885     if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS || type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS) {
 886         if (instId == UAVOBJ_ALL_INSTANCES) {
 887             // Get number of instances
 888             numInst = UAVObjGetNumInstances(obj);
 889             // Send all instances in reverse order
 890             // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received)
 891             ret     = 0;
 892             for (n = 0; n < numInst; ++n) {
 893                 ret = sendSingleObject(connection, type, objId, numInst - n - 1, obj);
 894                 if (ret == -1) {
 895                     break;
 896                 }
 897             }
 898         } else {
 899             ret = sendSingleObject(connection, type, objId, instId, obj);
 900         }
 901     } else if (type == UAVTALK_TYPE_OBJ_REQ) {
 902         ret = sendSingleObject(connection, type, objId, instId, obj);
 903     } else if (type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) {
 904         if (instId != UAVOBJ_ALL_INSTANCES) {
 905             ret = sendSingleObject(connection, type, objId, instId, obj);
 906         }
 907     }
 908
 909     return ret;
 910 }
 911
 912 /**
 913  * Send an object through the telemetry link.
 914  * \param[in] connection UAVTalkConnection to be used
 915  * \param[in] type Transaction type
 916  * \param[in] objId The object ID
 917  * \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use () instead)
 918  * \param[in] obj Object handle to send (null when type is NACK)
 919  * \return 0 Success
 920  * \return -1 Failure
 921  */
 922 static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj)
 923 {
 924     // IMPORTANT : obj can be null (when type is NACK for example)
 925
 926     if (!connection->outStream) {
 927         connection->stats.txErrors++;
 928         return -1;
 929     }
 930
 931     // Setup sync byte
 932     connection->txBuffer[0] = UAVTALK_SYNC_VAL;
 933     // Setup type
 934     connection->txBuffer[1] = type;
 935     // next 2 bytes are reserved for data length (inserted here later)
 936     // Setup object ID
 937     connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
 938     connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
 939     connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
 940     connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
 941     // Setup instance ID
 942     connection->txBuffer[8] = (uint8_t)(instId & 0xFF);
 943     connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF);
 944     int32_t headerLength = 10;
 945
 946     // Add timestamp when the transaction type is appropriate
 947     if (type & UAVTALK_TIMESTAMPED) {
 948         portTickType time = xTaskGetTickCount();
 949         connection->txBuffer[10] = (uint8_t)(time & 0xFF);
 950         connection->txBuffer[11] = (uint8_t)((time >> 8) & 0xFF);
 951         headerLength += 2;
 952     }
 953
 954     // Determine data length
 955     int32_t length;
 956     if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) {
 957         length = 0;
 958     } else {
 959         length = UAVObjGetNumBytes(obj);
 960     }
 961
 962     // Check length
 963     if (length > UAVOBJECTS_LARGEST) {
 964         connection->stats.txErrors++;
 965         return -1;
 966     }
 967
 968     // Copy data (if any)
 969     if (length > 0) {
 970         if (UAVObjPack(obj, instId, &connection->txBuffer[headerLength]) == -1) {
 971             connection->stats.txErrors++;
 972             return -1;
 973         }
 974     }
 975
 976     // Store the packet length
 977     connection->txBuffer[2] = (uint8_t)((headerLength + length) & 0xFF);
 978     connection->txBuffer[3] = (uint8_t)(((headerLength + length) >> 8) & 0xFF);
 979
 980     // Calculate and store checksum
 981     connection->txBuffer[headerLength + length] = PIOS_CRC_updateCRC(0, connection->txBuffer, headerLength + length);
 982
 983     // Send object
 984     uint16_t tx_msg_len = headerLength + length + UAVTALK_CHECKSUM_LENGTH;
 985     int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len);
 986
 987     // Update stats
 988     if (rc == tx_msg_len) {
 989         ++connection->stats.txObjects;
 990         connection->stats.txObjectBytes += length;
 991         connection->stats.txBytes += tx_msg_len;
 992     } else {
 993         connection->stats.txErrors++;
 994         // TODO rc == -1 connection not open, -2 buffer full should retry
 995         connection->stats.txBytes += (rc > 0) ? rc : 0;
 996         return -1;
 997     }
 998
 999     // Done
1000     return 0;
1001 }
1002
1003 /**
1004  * @}
1005  * @}
1006  */