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, bool create);
  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, true);
 561 }
 562
 563 /**
 564  * Complete receiving a UAVTalk packet.  This will cause the packet to be unpacked, acked, etc.
 565  * This version will not create/unpack an object if it does not already exist.
 566  * \param[in] connectionHandle UAVTalkConnection to be used
 567  * \return 0 Success
 568  * \return -1 Failure
 569  */
 570 int32_t UAVTalkReceiveObjectNoCreate(UAVTalkConnection connectionHandle)
 571 {
 572     UAVTalkConnectionData *connection;
 573
 574     CHECKCONHANDLE(connectionHandle, connection, return -1);
 575
 576     UAVTalkInputProcessor *iproc = &connection->iproc;
 577     if (iproc->state != UAVTALK_STATE_COMPLETE) {
 578         return -1;
 579     }
 580
 581     return receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, false);
 582 }
 583
 584 /**
 585  * Get the object ID of the current packet.
 586  * \param[in] connectionHandle UAVTalkConnection to be used
 587  * \return The object ID, or 0 on error.
 588  */
 589 uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle)
 590 {
 591     UAVTalkConnectionData *connection;
 592
 593     CHECKCONHANDLE(connectionHandle, connection, return 0);
 594
 595     return connection->iproc.objId;
 596 }
 597
 598 /**
 599  * Receive an object. This function process objects received through the telemetry stream.
 600  *
 601  * Parser errors are considered as transmission errors and are not NACKed.
 602  * Some senders (GCS) can timeout and retry if the message is not answered by an ack or nack.
 603  *
 604  * Object handling errors are considered as application errors and are NACked.
 605  * In that case we want to nack as there is no point in the sender retrying to send invalid objects.
 606  *
 607  * \param[in] connection UAVTalkConnection to be used
 608  * \param[in] type Type of received message (UAVTALK_TYPE_OBJ, UAVTALK_TYPE_OBJ_REQ, UAVTALK_TYPE_OBJ_ACK, UAVTALK_TYPE_ACK, UAVTALK_TYPE_NACK)
 609  * \param[in] objId ID of the object to work on
 610  * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
 611  * \param[in] data Data buffer
 612  * \param[in] length Buffer length
 613  * \return 0 Success
 614  * \return -1 Failure
 615  */
 616 static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data, bool create)
 617 {
 618     UAVObjHandle obj;
 619     int32_t ret = 0;
 620
 621     // Lock
 622     xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
 623
 624     // Get the handle to the object. Will be null if object does not exist.
 625     // Warning :
 626     // Here we ask for instance ID 0 without taking into account the provided instId
 627     // The provided instId will be used later when packing, unpacking, etc...
 628     // TODO the above should be fixed as it is cumbersome and error prone
 629     obj = UAVObjGetByID(objId);
 630
 631     // Process message type
 632     switch (type) {
 633     case UAVTALK_TYPE_OBJ:
 634     case UAVTALK_TYPE_OBJ_TS:
 635         // All instances not allowed for OBJ messages
 636         if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
 637             // Unpack object, if create is true and the instance does not exist it will be created!
 638             if (UAVObjUnpack(obj, instId, data, create) == 0) {
 639                 // Check if this object acks a pending OBJ_REQ message
 640                 // any OBJ message can ack a pending OBJ_REQ message
 641                 // even one that was not sent in response to the OBJ_REQ message
 642                 updateAck(connection, type, objId, instId);
 643             } else {
 644                 ret = -1;
 645             }
 646         } else {
 647             ret = -1;
 648         }
 649         break;
 650
 651     case UAVTALK_TYPE_OBJ_ACK:
 652     case UAVTALK_TYPE_OBJ_ACK_TS:
 653         UAVT_DEBUGLOG_CPRINTF(objId, "OBJ_ACK %X %d", objId, instId);
 654         // All instances not allowed for OBJ_ACK messages
 655         if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
 656             // Unpack object, if create is true and the instance does not exist it will be created!
 657             if (UAVObjUnpack(obj, instId, data, create) == 0) {
 658                 UAVT_DEBUGLOG_CPRINTF(objId, "OBJ ACK %X %d", objId, instId);
 659                 // Object updated or created, transmit ACK
 660                 sendObject(connection, UAVTALK_TYPE_ACK, objId, instId, NULL);
 661             } else {
 662                 ret = -1;
 663             }
 664         } else {
 665             ret = -1;
 666         }
 667         if (ret == -1) {
 668             // failed to update object, transmit NACK
 669             UAVT_DEBUGLOG_PRINTF("OBJ NACK %X %d", objId, instId);
 670             sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
 671         }
 672         break;
 673
 674     case UAVTALK_TYPE_OBJ_REQ:
 675         // Check if requested object exists
 676         UAVT_DEBUGLOG_CPRINTF(objId, "REQ %X %d", objId, instId);
 677         if (obj) {
 678             // Object found, transmit it
 679             // The sent object will ack the object request on the receiver side
 680             ret = sendObject(connection, UAVTALK_TYPE_OBJ, objId, instId, obj);
 681         } else {
 682             ret = -1;
 683         }
 684         if (ret == -1) {
 685             // failed to send object, transmit NACK
 686             UAVT_DEBUGLOG_PRINTF("REQ NACK %X %d", objId, instId);
 687             sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
 688         }
 689         break;
 690
 691     case UAVTALK_TYPE_NACK:
 692         // Do nothing on flight side, let it time out.
 693         // TODO:
 694         // The transaction takes the result code of the "semaphore taking operation" into account to determine success.
 695         // If we give that semaphore in time, its "success" (ack received)
 696         // If we do not give that semaphore before the timeout it will return failure.
 697         // What would have to be done here is give the semaphore, but set a flag (for example connection->respFail=true)
 698         // that indicates failure and then above where it checks for the result code, have it behave as if it failed
 699         // if the explicit failure is set.
 700         break;
 701
 702     case UAVTALK_TYPE_ACK:
 703         // All instances not allowed for ACK messages
 704         if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
 705             // Check if an ACK is pending
 706             updateAck(connection, type, objId, instId);
 707         } else {
 708             ret = -1;
 709         }
 710         break;
 711
 712     default:
 713         ret = -1;
 714     }
 715
 716     // Unlock
 717     xSemaphoreGiveRecursive(connection->lock);
 718
 719     // Done
 720     return ret;
 721 }
 722
 723 /**
 724  * Check if an ack is pending on an object and give response semaphore
 725  * \param[in] connection UAVTalkConnection to be used
 726  * \param[in] obj Object
 727  * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
 728  */
 729 static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId)
 730 {
 731     if ((connection->respObjId == objId) && (connection->respType == type)) {
 732         if ((connection->respInstId == UAVOBJ_ALL_INSTANCES) && (instId == 0)) {
 733             // last instance received, complete transaction
 734             xSemaphoreGive(connection->respSema);
 735             connection->respObjId = 0;
 736         } else if (connection->respInstId == instId) {
 737             xSemaphoreGive(connection->respSema);
 738             connection->respObjId = 0;
 739         }
 740     }
 741 }
 742
 743 /**
 744  * Send an object through the telemetry link.
 745  * \param[in] connection UAVTalkConnection to be used
 746  * \param[in] type Transaction type
 747  * \param[in] objId The object ID
 748  * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances
 749  * \param[in] obj Object handle to send (null when type is NACK)
 750  * \return 0 Success
 751  * \return -1 Failure
 752  */
 753 static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj)
 754 {
 755     uint32_t numInst;
 756     uint32_t n;
 757     int32_t ret = -1;
 758
 759     // Important note : obj can be null (when type is NACK for example) so protect all obj dereferences.
 760
 761     // If all instances are requested and this is a single instance object, force instance ID to zero
 762     if ((obj != NULL) && (instId == UAVOBJ_ALL_INSTANCES) && UAVObjIsSingleInstance(obj)) {
 763         instId = 0;
 764     }
 765
 766     // Process message type
 767     if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS || type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS) {
 768         if (instId == UAVOBJ_ALL_INSTANCES) {
 769             // Get number of instances
 770             numInst = UAVObjGetNumInstances(obj);
 771             // Send all instances in reverse order
 772             // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received)
 773             ret     = 0;
 774             for (n = 0; n < numInst; ++n) {
 775                 ret = sendSingleObject(connection, type, objId, numInst - n - 1, obj);
 776                 if (ret == -1) {
 777                     break;
 778                 }
 779             }
 780         } else {
 781             ret = sendSingleObject(connection, type, objId, instId, obj);
 782         }
 783     } else if (type == UAVTALK_TYPE_OBJ_REQ) {
 784         ret = sendSingleObject(connection, type, objId, instId, obj);
 785     } else if (type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) {
 786         if (instId != UAVOBJ_ALL_INSTANCES) {
 787             ret = sendSingleObject(connection, type, objId, instId, obj);
 788         }
 789     }
 790
 791     return ret;
 792 }
 793
 794 /**
 795  * Send an object through the telemetry link.
 796  * \param[in] connection UAVTalkConnection to be used
 797  * \param[in] type Transaction type
 798  * \param[in] objId The object ID
 799  * \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use () instead)
 800  * \param[in] obj Object handle to send (null when type is NACK)
 801  * \return 0 Success
 802  * \return -1 Failure
 803  */
 804 static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj)
 805 {
 806     // IMPORTANT : obj can be null (when type is NACK for example)
 807
 808     if (!connection->outStream) {
 809         connection->stats.txErrors++;
 810         return -1;
 811     }
 812
 813     // Setup sync byte
 814     connection->txBuffer[0] = UAVTALK_SYNC_VAL;
 815     // Setup type
 816     connection->txBuffer[1] = type;
 817     // next 2 bytes are reserved for data length (inserted here later)
 818     // Setup object ID
 819     connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
 820     connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
 821     connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
 822     connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
 823     // Setup instance ID
 824     connection->txBuffer[8] = (uint8_t)(instId & 0xFF);
 825     connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF);
 826     int32_t headerLength = 10;
 827
 828     // Add timestamp when the transaction type is appropriate
 829     if (type & UAVTALK_TIMESTAMPED) {
 830         portTickType time = xTaskGetTickCount();
 831         connection->txBuffer[10] = (uint8_t)(time & 0xFF);
 832         connection->txBuffer[11] = (uint8_t)((time >> 8) & 0xFF);
 833         headerLength += 2;
 834     }
 835
 836     // Determine data length
 837     int32_t length;
 838     if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) {
 839         length = 0;
 840     } else {
 841         length = UAVObjGetNumBytes(obj);
 842     }
 843
 844     // Check length
 845     if (length > UAVOBJECTS_LARGEST) {
 846         connection->stats.txErrors++;
 847         return -1;
 848     }
 849
 850     // Copy data (if any)
 851     if (length > 0) {
 852         if (UAVObjPack(obj, instId, &connection->txBuffer[headerLength]) == -1) {
 853             connection->stats.txErrors++;
 854             return -1;
 855         }
 856     }
 857
 858     // Store the packet length
 859     connection->txBuffer[2] = (uint8_t)((headerLength + length) & 0xFF);
 860     connection->txBuffer[3] = (uint8_t)(((headerLength + length) >> 8) & 0xFF);
 861
 862     // Calculate and store checksum
 863     connection->txBuffer[headerLength + length] = PIOS_CRC_updateCRC(0, connection->txBuffer, headerLength + length);
 864
 865     // Send object
 866     uint16_t tx_msg_len = headerLength + length + UAVTALK_CHECKSUM_LENGTH;
 867     int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len);
 868
 869     // Update stats
 870     if (rc == tx_msg_len) {
 871         ++connection->stats.txObjects;
 872         connection->stats.txObjectBytes += length;
 873         connection->stats.txBytes += tx_msg_len;
 874     } else {
 875         connection->stats.txErrors++;
 876         // TODO rc == -1 connection not open, -2 buffer full should retry
 877         connection->stats.txBytes += (rc > 0) ? rc : 0;
 878         return -1;
 879     }
 880
 881     // Done
 882     return 0;
 883 }
 884
 885 /*
 886  * Functions that implements the UAVTalk Process FSM. return false to break out of current cycle
 887  */
 888
 889 static bool UAVTalkProcess_SYNC(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, __attribute__((unused)) uint8_t length, uint8_t *position)
 890 {
 891     uint8_t rxbyte = rxbuffer[(*position)++];
 892
 893     if (rxbyte != UAVTALK_SYNC_VAL) {
 894         connection->stats.rxSyncErrors++;
 895         return false;
 896     }
 897
 898     // Initialize and update the CRC
 899     iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
 900
 901     iproc->rxPacketLength = 1;
 902     iproc->rxCount = 0;
 903
 904     iproc->type    = 0;
 905     iproc->state   = UAVTALK_STATE_TYPE;
 906     return true;
 907 }
 908
 909 static bool UAVTalkProcess_TYPE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, __attribute__((unused)) uint8_t length, uint8_t *position)
 910 {
 911     uint8_t rxbyte = rxbuffer[(*position)++];
 912
 913     if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) {
 914         connection->stats.rxErrors++;
 915         iproc->state = UAVTALK_STATE_SYNC;
 916         return false;
 917     }
 918
 919     // update the CRC
 920     iproc->cs    = PIOS_CRC_updateByte(iproc->cs, rxbyte);
 921
 922     iproc->type  = rxbyte;
 923     iproc->rxPacketLength++;
 924     iproc->packet_size = 0;
 925     iproc->state = UAVTALK_STATE_SIZE;
 926     return true;
 927 }
 928
 929 static bool UAVTalkProcess_SIZE(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
 930 {
 931     while (iproc->rxCount < 2 && length > (*position)) {
 932         uint8_t rxbyte = rxbuffer[(*position)++];
 933         // update the CRC
 934         iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
 935         iproc->packet_size += rxbyte << 8 * iproc->rxCount;
 936         iproc->rxCount++;
 937     }
 938
 939     if (iproc->rxCount < 2) {
 940         return false;;
 941     }
 942
 943     iproc->rxCount = 0;
 944
 945     if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) {
 946         // incorrect packet size
 947         connection->stats.rxErrors++;
 948         iproc->state = UAVTALK_STATE_ERROR;
 949         return false;
 950     }
 951     iproc->rxPacketLength += 2;
 952     iproc->objId = 0;
 953     iproc->state = UAVTALK_STATE_OBJID;
 954     return true;
 955 }
 956
 957 static bool UAVTalkProcess_OBJID(__attribute__((unused)) UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
 958 {
 959     while (iproc->rxCount < 4 && length > (*position)) {
 960         uint8_t rxbyte = rxbuffer[(*position)++];
 961         iproc->cs     = PIOS_CRC_updateByte(iproc->cs, rxbyte);
 962         iproc->objId += rxbyte << (8 * (iproc->rxCount++));
 963     }
 964
 965     if (iproc->rxCount < 4) {
 966         return false;
 967     }
 968     iproc->rxCount = 0;
 969     iproc->rxPacketLength += 4;
 970     iproc->instId  = 0;
 971     iproc->state   = UAVTALK_STATE_INSTID;
 972     return true;
 973 }
 974
 975 static bool UAVTalkProcess_INSTID(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
 976 {
 977     while (iproc->rxCount < 2 && length > (*position)) {
 978         uint8_t rxbyte = rxbuffer[(*position)++];
 979         iproc->cs      = PIOS_CRC_updateByte(iproc->cs, rxbyte);
 980         iproc->instId += rxbyte << (8 * (iproc->rxCount++));
 981     }
 982
 983     if (iproc->rxCount < 2) {
 984         return false;
 985     }
 986     iproc->rxPacketLength += 2;
 987     iproc->rxCount = 0;
 988
 989     UAVObjHandle obj = UAVObjGetByID(iproc->objId);
 990
 991     // Determine data length
 992     if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) {
 993         iproc->length = 0;
 994         iproc->timestampLength = 0;
 995     } else {
 996         iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
 997         if (obj) {
 998             iproc->length = UAVObjGetNumBytes(obj);
 999         } else {
1000             iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength;
1001         }
1002     }
1003
1004     // Check length
1005     if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
1006         // packet error - exceeded payload max length
1007         connection->stats.rxErrors++;
1008         iproc->state = UAVTALK_STATE_ERROR;
1009         return false;
1010     }
1011
1012     // Check the lengths match
1013     if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) {
1014         // packet error - mismatched packet size
1015         connection->stats.rxErrors++;
1016         iproc->state = UAVTALK_STATE_ERROR;
1017         return false;
1018     }
1019
1020     // Determine next state
1021     if (iproc->type & UAVTALK_TIMESTAMPED) {
1022         // If there is a timestamp get it
1023         iproc->timestamp = 0;
1024         iproc->state     = UAVTALK_STATE_TIMESTAMP;
1025     } else {
1026         // If there is a payload get it, otherwise receive checksum
1027         if (iproc->length > 0) {
1028             iproc->state = UAVTALK_STATE_DATA;
1029         } else {
1030             iproc->state = UAVTALK_STATE_CS;
1031         }
1032     }
1033     return true;
1034 }
1035
1036 static bool UAVTalkProcess_TIMESTAMP(__attribute__((unused)) UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
1037 {
1038     while (iproc->rxCount < 2 && length > (*position)) {
1039         uint8_t rxbyte = rxbuffer[(*position)++];
1040         iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
1041         iproc->timestamp += rxbyte << (8 * (iproc->rxCount++));
1042     }
1043
1044     if (iproc->rxCount < 2) {
1045         return false;;
1046     }
1047
1048     iproc->rxCount = 0;
1049     iproc->rxPacketLength += 2;
1050     // If there is a payload get it, otherwise receive checksum
1051     if (iproc->length > 0) {
1052         iproc->state = UAVTALK_STATE_DATA;
1053     } else {
1054         iproc->state = UAVTALK_STATE_CS;
1055     }
1056     return true;
1057 }
1058
1059 static bool UAVTalkProcess_DATA(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, uint8_t length, uint8_t *position)
1060 {
1061     uint8_t toCopy = iproc->length - iproc->rxCount;
1062
1063     if (toCopy > length - (*position)) {
1064         toCopy = length - (*position);
1065     }
1066
1067     memcpy(&connection->rxBuffer[iproc->rxCount], &rxbuffer[(*position)], toCopy);
1068     (*position)    += toCopy;
1069
1070     // update the CRC
1071     iproc->cs       = PIOS_CRC_updateCRC(iproc->cs, &connection->rxBuffer[iproc->rxCount], toCopy);
1072     iproc->rxCount += toCopy;
1073
1074     iproc->rxPacketLength += toCopy;
1075
1076     if (iproc->rxCount < iproc->length) {
1077         return false;
1078     }
1079
1080     iproc->rxCount = 0;
1081     iproc->state   = UAVTALK_STATE_CS;
1082     return true;
1083 }
1084
1085 static bool UAVTalkProcess_CS(UAVTalkConnectionData *connection, UAVTalkInputProcessor *iproc, uint8_t *rxbuffer, __attribute__((unused)) uint8_t length, uint8_t *position)
1086 {
1087     // Check the CRC byte
1088     uint8_t rxbyte = rxbuffer[(*position)++];
1089
1090     if (rxbyte != iproc->cs) {
1091         // packet error - faulty CRC
1092         UAVT_DEBUGLOG_PRINTF("BAD CRC");
1093         connection->stats.rxCrcErrors++;
1094         connection->stats.rxErrors++;
1095         iproc->state = UAVTALK_STATE_ERROR;
1096         return false;;
1097     }
1098     iproc->rxPacketLength++;
1099
1100     if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) {
1101         // packet error - mismatched packet size
1102         connection->stats.rxErrors++;
1103         iproc->state = UAVTALK_STATE_ERROR;
1104         return false;;
1105     }
1106
1107     connection->stats.rxObjects++;
1108     connection->stats.rxObjectBytes += iproc->length;
1109
1110     iproc->state = UAVTALK_STATE_COMPLETE;
1111     return true;
1112 }
1113
1114
1115 /**
1116  * @}
1117  * @}
1118  */