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