mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
OP-1122 OP-1125 uavtalk - minor OPReview-593 related cleanups
This commit is contained in:
Philippe Renon
parent
2f0974fda9
commit
50a0a4f512
@ -63,7 +63,6 @@ typedef struct {
|
||||
uint32_t objId;
|
||||
uint16_t instId;
|
||||
uint32_t length;
|
||||
uint8_t instanceLength;
|
||||
uint8_t timestampLength;
|
||||
uint8_t cs;
|
||||
uint16_t timestamp;
|
||||
|
||||
@ -32,6 +32,8 @@
|
||||
#include "openpilot.h"
|
||||
#include "uavtalk_priv.h"
|
||||
|
||||
// Size of instance ID (2 bytes)
|
||||
#define UAVTALK_INSTANCE_LENGTH 2
|
||||
|
||||
// Private functions
|
||||
static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeout);
|
||||
@ -422,23 +424,23 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
|
||||
iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
|
||||
}
|
||||
|
||||
// Message always contain an instance ID
|
||||
iproc->instanceLength = 2;
|
||||
|
||||
// Check length and determine next state
|
||||
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->instanceLength + iproc->timestampLength + iproc->length) != iproc->packet_size) { // packet error - mismatched packet size
|
||||
if ((iproc->rxPacketLength + UAVTALK_INSTANCE_LENGTH + iproc->timestampLength + iproc->length) != iproc->packet_size) {
|
||||
// packet error - mismatched packet size
|
||||
connection->stats.rxErrors++;
|
||||
iproc->state = UAVTALK_STATE_ERROR;
|
||||
break;
|
||||
}
|
||||
|
||||
// Message always contain an instance ID
|
||||
iproc->rxCount = 0;
|
||||
iproc->instId = 0;
|
||||
iproc->state = UAVTALK_STATE_INSTID;
|
||||
@ -585,25 +587,23 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(outConnection->lock, portMAX_DELAY);
|
||||
|
||||
// Setup type and object id fields
|
||||
outConnection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
|
||||
// Setup sync byte
|
||||
outConnection->txBuffer[0] = UAVTALK_SYNC_VAL;
|
||||
// Setup type
|
||||
outConnection->txBuffer[1] = inIproc->type;
|
||||
// data length inserted here below
|
||||
int32_t dataOffset = 8;
|
||||
// next 2 bytes are reserved for data length (inserted here later)
|
||||
int32_t dataOffset = 4;
|
||||
if (inIproc->objId != 0) {
|
||||
// Setup object ID
|
||||
outConnection->txBuffer[4] = (uint8_t)(inIproc->objId & 0xFF);
|
||||
outConnection->txBuffer[5] = (uint8_t)((inIproc->objId >> 8) & 0xFF);
|
||||
outConnection->txBuffer[6] = (uint8_t)((inIproc->objId >> 16) & 0xFF);
|
||||
outConnection->txBuffer[7] = (uint8_t)((inIproc->objId >> 24) & 0xFF);
|
||||
|
||||
// Setup instance ID if one is required
|
||||
if (inIproc->instanceLength > 0) {
|
||||
outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF);
|
||||
outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF);
|
||||
dataOffset = 10;
|
||||
}
|
||||
} else {
|
||||
dataOffset = 4;
|
||||
// Setup instance ID
|
||||
outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF);
|
||||
outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF);
|
||||
dataOffset = 10;
|
||||
}
|
||||
|
||||
// Add timestamp when the transaction type is appropriate
|
||||
@ -860,26 +860,26 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type,
|
||||
int32_t length;
|
||||
int32_t dataOffset;
|
||||
|
||||
// Important note : obj can be null (when type is NACK for example) so protect all obj dereferences.
|
||||
// IMPORTANT : obj can be null (when type is NACK for example)
|
||||
|
||||
if (!connection->outStream) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Setup type and object id fields
|
||||
connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
|
||||
// Setup sync byte
|
||||
connection->txBuffer[0] = UAVTALK_SYNC_VAL;
|
||||
// Setup type
|
||||
connection->txBuffer[1] = type;
|
||||
// data length inserted here below
|
||||
// next 2 bytes are reserved for data length (inserted here later)
|
||||
// Setup object ID
|
||||
connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
|
||||
connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
|
||||
connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
|
||||
connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
|
||||
dataOffset = 8;
|
||||
|
||||
// The instance ID is always sent
|
||||
// Setup instance ID
|
||||
connection->txBuffer[8] = (uint8_t)(instId & 0xFF);
|
||||
connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF);
|
||||
dataOffset += 2;
|
||||
dataOffset = 10;
|
||||
|
||||
// Add timestamp when the transaction type is appropriate
|
||||
if (type & UAVTALK_TIMESTAMPED) {
|
||||
|
||||
@ -248,6 +248,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
|
||||
case STATE_SYNC:
|
||||
|
||||
if (rxbyte != SYNC_VAL) {
|
||||
// continue until synch byte is matched
|
||||
UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Sync (" + QString::number(rxbyte) + " " + QString("0x%1").arg(rxbyte, 2, 16) + ")");
|
||||
break;
|
||||
}
|
||||
@ -337,14 +338,16 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
|
||||
if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK || rxType == TYPE_NACK) {
|
||||
rxLength = 0;
|
||||
} else {
|
||||
rxLength = rxObj->getNumBytes();
|
||||
if (rxObj) {
|
||||
rxLength = rxObj->getNumBytes();
|
||||
} else {
|
||||
rxLength = packetSize - rxPacketLength;
|
||||
}
|
||||
}
|
||||
|
||||
// The instance ID is always sent
|
||||
rxInstanceLength = 2;
|
||||
|
||||
// Check length and determine next state
|
||||
if (rxLength >= MAX_PAYLOAD_LENGTH) {
|
||||
// packet error - exceeded payload max length
|
||||
stats.rxErrors++;
|
||||
rxState = STATE_SYNC;
|
||||
UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (oversize)");
|
||||
@ -352,7 +355,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
|
||||
}
|
||||
|
||||
// Check the lengths match
|
||||
if ((rxPacketLength + rxInstanceLength + rxLength) != packetSize) {
|
||||
if ((rxPacketLength + INSTANCE_LENGTH + rxLength) != packetSize) {
|
||||
// packet error - mismatched packet size
|
||||
stats.rxErrors++;
|
||||
rxState = STATE_SYNC;
|
||||
@ -360,6 +363,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
|
||||
break;
|
||||
}
|
||||
|
||||
// Message always contain an instance ID
|
||||
rxCount = 0;
|
||||
rxInstId = 0;
|
||||
rxState = STATE_INSTID;
|
||||
@ -707,22 +711,25 @@ bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObje
|
||||
*/
|
||||
bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
|
||||
{
|
||||
#ifdef VERBOSE_UAVTALK
|
||||
qDebug() << "UAVTalk - transmitting object" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
|
||||
#endif
|
||||
qint32 length;
|
||||
qint32 dataOffset;
|
||||
|
||||
// Setup type and object id fields
|
||||
txBuffer[0] = SYNC_VAL;
|
||||
txBuffer[1] = type;
|
||||
// data length inserted here below
|
||||
qToLittleEndian<quint32>(objId, &txBuffer[4]);
|
||||
dataOffset = 8;
|
||||
#ifdef VERBOSE_UAVTALK
|
||||
qDebug() << "UAVTalk - transmitting object" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
|
||||
#endif
|
||||
|
||||
// The instance ID is always sent
|
||||
// IMPORTANT : obj can be null (when type is NACK for example)
|
||||
|
||||
// Setup sync byte
|
||||
txBuffer[0] = SYNC_VAL;
|
||||
// Setup type
|
||||
txBuffer[1] = type;
|
||||
// next 2 bytes are reserved for data length (inserted here later)
|
||||
// Setup object ID
|
||||
qToLittleEndian<quint32>(objId, &txBuffer[4]);
|
||||
// Setup instance ID
|
||||
qToLittleEndian<quint16>(instId, &txBuffer[8]);
|
||||
dataOffset += 2;
|
||||
dataOffset = 10;
|
||||
|
||||
// Determine data length
|
||||
if (type == TYPE_OBJ_REQ || type == TYPE_ACK || type == TYPE_NACK) {
|
||||
@ -743,6 +750,7 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U
|
||||
}
|
||||
}
|
||||
|
||||
// Store the packet length
|
||||
qToLittleEndian<quint16>(dataOffset + length, &txBuffer[2]);
|
||||
|
||||
// Calculate checksum
|
||||
|
||||
@ -94,6 +94,8 @@ private:
|
||||
|
||||
static const quint16 ALL_INSTANCES = 0xFFFF;
|
||||
|
||||
static const quint8 INSTANCE_LENGTH = 2;
|
||||
|
||||
static const int TX_BUFFER_SIZE = 2 * 1024;
|
||||
static const quint8 crc_table[256];
|
||||
|
||||
@ -114,7 +116,6 @@ private:
|
||||
quint16 rxInstId;
|
||||
quint16 rxLength;
|
||||
quint16 rxPacketLength;
|
||||
quint8 rxInstanceLength;
|
||||
|
||||
quint8 rxCSPacket, rxCS;
|
||||
qint32 rxCount;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user