/**
* Process an byte from the telemetry stream.
* \param[in] connection UAVLinkConnection to be used
* \param[in] rxbyte Received byte
* \return UAVLinkRxState
*/
UAVLinkRxState UAVLinkProcessInputStreamQuiet(UAVLinkConnection connectionHandle, uint8_t rxbyte)
{
UAVLinkConnectionData *connection;
CHECKCONHANDLE(connectionHandle,connection,return -1);
UAVLinkInputProcessor *iproc = &connection->iproc;
++connection->stats.rxBytes;
if (iproc->state == UAVLINK_STATE_ERROR || iproc->state == UAVLINK_STATE_COMPLETE || iproc->state == UAVLINK_STATE_STREAM_COMPLETE)
iproc->state = UAVLINK_STATE_SYNC;
if (iproc->rxPacketLength < 0xffff)
iproc->rxPacketLength++; // update packet byte count
// Receive state machine
switch (iproc->state)
{
case UAVLINK_STATE_SYNC:
if (rxbyte != UAVLINK_SYNC_VAL)
break;
// Initialize and update the CRC
iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
iproc->rxPacketLength = 1;
iproc->state = UAVLINK_STATE_TYPE;
break;
case UAVLINK_STATE_TYPE:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
if ((rxbyte & UAVLINK_TYPE_MASK) != UAVLINK_TYPE_VER)
{
iproc->state = UAVLINK_STATE_ERROR;
break;
}
iproc->type = rxbyte;
iproc->packet_size = 0;
iproc->state = UAVLINK_STATE_SIZE;
iproc->rxCount = 0;
break;
case UAVLINK_STATE_SIZE:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
if (iproc->rxCount == 0)
{
iproc->packet_size += rxbyte;
iproc->rxCount++;
break;
}
iproc->packet_size += rxbyte << 8;
if (iproc->packet_size < UAVLINK_MIN_PACKET_SIZE || iproc->packet_size > UAVLINK_MAX_PACKET_LENGTH)
{ // incorrect packet size
iproc->state = UAVLINK_STATE_ERROR;
break;
}
iproc->rxCount = 0;
iproc->rxId = 0;
if (iproc->type == UAVLINK_TYPE_STREAM)
{
iproc->state = UAVLINK_STATE_STREAMID;
} else {
iproc->state = UAVLINK_STATE_OBJID;
}
break;
case UAVLINK_STATE_STREAMID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->rxId = rxbyte;
iproc->length = iproc->packet_size - iproc->rxPacketLength;
iproc->state = UAVLINK_STATE_DATA;
break;
case UAVLINK_STATE_OBJID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->rxId += rxbyte << (8*(iproc->rxCount++));
if (iproc->rxCount < 4)
break;
// Search for object.
iproc->obj = UAVObjGetByID(iproc->rxId);
// Determine data length
if (iproc->type == UAVLINK_TYPE_OBJ_REQ || iproc->type == UAVLINK_TYPE_ACK || iproc->type == UAVLINK_TYPE_NACK)
{
iproc->length = 0;
iproc->instanceLength = 0;
}
else
{
if (iproc->obj)
{
iproc->length = UAVObjGetNumBytes(iproc->obj);
iproc->instanceLength = (UAVObjIsSingleInstance(iproc->obj) ? 0 : 2);
}
else
{
// We don't know if it's a multi-instance object, so just assume it's 0.
iproc->instanceLength = 0;
iproc->length = iproc->packet_size - iproc->rxPacketLength;
}
}
// Check length and determine next state
if (iproc->length >= UAVLINK_MAX_PAYLOAD_LENGTH)
{
connection->stats.rxErrors++;
iproc->state = UAVLINK_STATE_ERROR;
break;
}
// Check the lengths match
if ((iproc->rxPacketLength + iproc->instanceLength + iproc->length) != iproc->packet_size)
{ // packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVLINK_STATE_ERROR;
break;
}
iproc->instId = 0;
if (iproc->type == UAVLINK_TYPE_NACK)
{
// If this is a NACK, we skip to Checksum
iproc->state = UAVLINK_STATE_CS;
}
// Check if this is a single instance object (i.e. if the instance ID field is coming next)
else if ((iproc->obj != 0) && !UAVObjIsSingleInstance(iproc->obj))
{
iproc->state = UAVLINK_STATE_INSTID;
}
else
{
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0)
iproc->state = UAVLINK_STATE_DATA;
else
iproc->state = UAVLINK_STATE_CS;
}
iproc->rxCount = 0;
break;
case UAVLINK_STATE_INSTID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->instId += rxbyte << (8*(iproc->rxCount++));
if (iproc->rxCount < 2)
break;
iproc->rxCount = 0;
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0)
iproc->state = UAVLINK_STATE_DATA;
else
iproc->state = UAVLINK_STATE_CS;
break;
case UAVLINK_STATE_DATA:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
connection->rxBuffer[iproc->rxCount++] = rxbyte;
if (iproc->rxCount < iproc->length)
break;
iproc->state = UAVLINK_STATE_CS;
iproc->rxCount = 0;
break;
case UAVLINK_STATE_CS:
// the CRC byte
if (rxbyte != iproc->cs)
{ // packet error - faulty CRC
connection->stats.rxErrors++;
iproc->state = UAVLINK_STATE_ERROR;
break;
}
if (iproc->type == UAVLINK_TYPE_STREAM)
{
connection->stats.rxStreamBytes += iproc->length;
connection->stats.rxStreamPackets++;
iproc->state = UAVLINK_STATE_COMPLETE;
} else {
if (iproc->rxPacketLength != (iproc->packet_size + 1))
{ // packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVLINK_STATE_ERROR;
break;
} else {
connection->stats.rxObjectBytes += iproc->length;
connection->stats.rxObjects++;
iproc->state = UAVLINK_STATE_COMPLETE;
}
}
break;
default:
connection->stats.rxErrors++;
iproc->state = UAVLINK_STATE_ERROR;
}
// Done
return iproc->state;
}
/**
* Process an byte from the telemetry stream.
* \param[in] connectionHandle UAVTalkConnection to be used
* \param[in] rxbyte Received byte
* \return UAVTalkRxState
*/
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t rxbyte)
{
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle, connection, return -1);
UAVTalkInputProcessor *iproc = &connection->iproc;
++connection->stats.rxBytes;
if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) {
iproc->state = UAVTALK_STATE_SYNC;
}
if (iproc->rxPacketLength < 0xffff) {
// update packet byte count
iproc->rxPacketLength++;
}
// Receive state machine
switch (iproc->state) {
case UAVTALK_STATE_SYNC:
if (rxbyte != UAVTALK_SYNC_VAL) {
connection->stats.rxSyncErrors++;
break;
}
// Initialize and update the CRC
iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
iproc->rxPacketLength = 1;
iproc->rxCount = 0;
iproc->type = 0;
iproc->state = UAVTALK_STATE_TYPE;
break;
case UAVTALK_STATE_TYPE:
if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) {
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC;
break;
}
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->type = rxbyte;
iproc->packet_size = 0;
iproc->state = UAVTALK_STATE_SIZE;
break;
case UAVTALK_STATE_SIZE:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
if (iproc->rxCount == 0) {
iproc->packet_size += rxbyte;
iproc->rxCount++;
break;
}
iproc->packet_size += rxbyte << 8;
iproc->rxCount = 0;
if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) {
// incorrect packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
iproc->objId = 0;
iproc->state = UAVTALK_STATE_OBJID;
break;
case UAVTALK_STATE_OBJID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->objId += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 4) {
break;
}
iproc->rxCount = 0;
iproc->instId = 0;
iproc->state = UAVTALK_STATE_INSTID;
break;
case UAVTALK_STATE_INSTID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->instId += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 2) {
break;
}
iproc->rxCount = 0;
UAVObjHandle obj = UAVObjGetByID(iproc->objId);
// Determine data length
if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) {
iproc->length = 0;
iproc->timestampLength = 0;
} else {
iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
if (obj) {
iproc->length = UAVObjGetNumBytes(obj);
} else {
iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength;
}
}
// Check length
if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
// packet error - exceeded payload max length
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
// Check the lengths match
if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
// Determine next state
if (iproc->type & UAVTALK_TIMESTAMPED) {
// If there is a timestamp get it
iproc->timestamp = 0;
iproc->state = UAVTALK_STATE_TIMESTAMP;
} else {
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0) {
iproc->state = UAVTALK_STATE_DATA;
} else {
iproc->state = UAVTALK_STATE_CS;
}
}
break;
case UAVTALK_STATE_TIMESTAMP:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->timestamp += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 2) {
break;
}
iproc->rxCount = 0;
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0) {
iproc->state = UAVTALK_STATE_DATA;
} else {
iproc->state = UAVTALK_STATE_CS;
}
break;
case UAVTALK_STATE_DATA:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
connection->rxBuffer[iproc->rxCount++] = rxbyte;
if (iproc->rxCount < iproc->length) {
break;
}
iproc->rxCount = 0;
iproc->state = UAVTALK_STATE_CS;
break;
case UAVTALK_STATE_CS:
// Check the CRC byte
if (rxbyte != iproc->cs) {
// packet error - faulty CRC
UAVT_DEBUGLOG_PRINTF("BAD CRC");
connection->stats.rxCrcErrors++;
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
connection->stats.rxObjects++;
connection->stats.rxObjectBytes += iproc->length;
iproc->state = UAVTALK_STATE_COMPLETE;
break;
default:
iproc->state = UAVTALK_STATE_ERROR;
break;
}
// Done
return iproc->state;
}
