rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

OP-1122 OP-1125 reworked uavtalk encoding/decoding to fix issues found during OPLM testing

Browse Source 
Packet sizes are now again properly calculated and sent by GCS
Made the ground and flight side more symmetric and robust
Fixed few error handling issues
Replaced UAVTALK_QXTLOG_DEBUG define with qWarning() calls
Known issue : GCS sometimes reports bad CRC on messages received over USB connection
This commit is contained in:
Philippe Renon 2013-12-12 00:05:58 +01:00
parent df7db8a01a
commit 625d7d53fc
7 changed files with 196 additions and 187 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
flight/uavtalk/inc/uavtalk_priv.h
View File

@ -32,32 +32,21 @@
#include "uavobjectsinit.h"
// Private types and constants
typedef struct {
uint8_t sync;
uint8_t type;
uint16_t size;
uint32_t objId;
} uavtalk_min_header;
#define UAVTALK_MIN_HEADER_LENGTH sizeof(uavtalk_min_header)
typedef struct {
uint8_t sync;
uint8_t type;
uint16_t size;
uint32_t objId;
uint16_t instId;
uint16_t timestamp;
} uavtalk_max_header;
#define UAVTALK_MAX_HEADER_LENGTH sizeof(uavtalk_max_header)
// min header : sync(1), type (1), size(2), object ID(4), instance ID(2)
#define UAVTALK_MIN_HEADER_LENGTH 10
// max header : sync(1), type (1), size(2), object ID(4), instance ID(2), timestamp(2)
#define UAVTALK_MAX_HEADER_LENGTH 12
#define UAVTALK_CHECKSUM_LENGTH 1
typedef uint8_t uavtalk_checksum;
#define UAVTALK_CHECKSUM_LENGTH sizeof(uavtalk_checksum)
#define UAVTALK_MAX_PAYLOAD_LENGTH (UAVOBJECTS_LARGEST + 1)
#define UAVTALK_MIN_PACKET_LENGTH UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH
#define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH
typedef struct {
UAVObjHandle obj;
uint8_t type;
uint16_t packet_size;
uint32_t objId;

119
flight/uavtalk/uavtalk.c
View File

@ -32,9 +32,6 @@
#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);
static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj);
@ -351,6 +348,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
case UAVTALK_STATE_SYNC:
if (rxbyte != UAVTALK_SYNC_VAL) {
// TODO connection->stats.rxSyncErrors++;
break;
}
@ -359,6 +357,8 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
iproc->rxPacketLength = 1;
iproc->rxCount = 0;
iproc->state = UAVTALK_STATE_TYPE;
break;
@ -368,6 +368,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) {
// TODO connection->stats.rxSyncErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
@ -377,7 +378,6 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
iproc->packet_size = 0;
iproc->state = UAVTALK_STATE_SIZE;
iproc->rxCount = 0;
break;
case UAVTALK_STATE_SIZE:
@ -390,15 +390,16 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
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
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->rxCount = 0;
iproc->objId = 0;
iproc->state = UAVTALK_STATE_OBJID;
break;
@ -409,44 +410,12 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->objId += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 4) {
break;
}
// Search for object.
iproc->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;
} else {
if (iproc->obj) {
iproc->length = UAVObjGetNumBytes(iproc->obj);
} else {
iproc->length = iproc->packet_size - iproc->rxPacketLength;
}
iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
}
// 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 + UAVTALK_INSTANCE_LENGTH + iproc->timestampLength + iproc->length) != iproc->packet_size) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
iproc->rxCount = 0;
// Message always contain an instance ID
iproc->rxCount = 0;
iproc->instId = 0;
iproc->state = UAVTALK_STATE_INSTID;
break;
@ -457,23 +426,54 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
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 timestamp, get it
if ((iproc->length > 0) && (iproc->type & UAVTALK_TIMESTAMPED)) {
iproc->timestamp = 0;
iproc->state = UAVTALK_STATE_TIMESTAMP;
}
// If there is a payload get it, otherwise receive checksum
else if (iproc->length > 0) {
iproc->state = UAVTALK_STATE_DATA;
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->state = UAVTALK_STATE_CS;
if (obj) {
iproc->length = UAVObjGetNumBytes(obj);
} else {
iproc->length = iproc->packet_size - iproc->rxPacketLength;
}
iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
}
// 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;
@ -482,11 +482,9 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
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
@ -506,28 +504,30 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
if (iproc->rxCount < iproc->length) {
break;
}
iproc->rxCount = 0;
iproc->state = UAVTALK_STATE_CS;
iproc->rxCount = 0;
break;
case UAVTALK_STATE_CS:
// the CRC byte
if (rxbyte != iproc->cs) { // packet error - faulty CRC
if (rxbyte != iproc->cs) {
// packet error - faulty CRC
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
if (iproc->rxPacketLength != (iproc->packet_size + 1)) { // packet error - mismatched packet size
if (iproc->rxPacketLength != (iproc->packet_size + 1)) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
connection->stats.rxObjectBytes += iproc->length;
connection->stats.rxObjects++;
connection->stats.rxObjectBytes += iproc->length;
iproc->state = UAVTALK_STATE_COMPLETE;
break;
@ -852,9 +852,8 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3
// Send all instances in reverse order
// This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received)
for (n = 0; n < numInst; ++n) {
sendSingleObject(connection, type, objId, numInst - n - 1, obj);
if (UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS) {
// TODO check semaphore and bail out if necessary
if (sendSingleObject(connection, type, objId, numInst - n - 1, obj) < 0) {
return -1;
}
}
return 0;

3
ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp
View File

@ -52,6 +52,7 @@ Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr) : objMngr(objMng
connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *)));
// Listen to transaction completions
// TODO should send a status (SUCCESS, FAILED, TIMEOUT)
connect(utalk, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool)));
// Get GCS stats object
@ -392,7 +393,7 @@ void Telemetry::processObjectQueue()
// If a single instance transaction is running, then starting an "all instance" transaction is not allowed
// TODO make the above logic a reality...
if (findTransaction(objInfo.obj)) {
qWarning() << "Telemetry - !!! Making request for a object " << objInfo.obj->toStringBrief() << " for which a request is already in progress";
qWarning().nospace() << "Telemetry - !!! Making request for an object " << objInfo.obj->toStringBrief() << " for which a request is already in progress";
//objInfo.obj->emitTransactionCompleted(false);
return;
}

8
ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp
View File

@ -59,10 +59,7 @@ void TelemetryManager::start(QIODevice *dev)
void TelemetryManager::onStart()
{
utalk = new UAVTalk(device, objMngr);
telemetry = new Telemetry(utalk, objMngr);
telemetryMon = new TelemetryMonitor(objMngr, telemetry);
utalk = new UAVTalk(device, objMngr);
if (false) {
// UAVTalk must be thread safe and for that:
// 1- all public methods must lock a mutex
@ -85,6 +82,9 @@ void TelemetryManager::onStart()
connect(device, SIGNAL(readyRead()), utalk, SLOT(processInputStream()));
}
telemetry = new Telemetry(utalk, objMngr);
telemetryMon = new TelemetryMonitor(objMngr, telemetry);
connect(telemetryMon, SIGNAL(connected()), this, SLOT(onConnect()));
connect(telemetryMon, SIGNAL(disconnected()), this, SLOT(onDisconnect()));
connect(telemetryMon, SIGNAL(telemetryUpdated(double, double)), this, SLOT(onTelemetryUpdate(double, double)));

209
ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp
View File

@ -32,14 +32,9 @@
#include <QDebug>
#include <QEventLoop>
// #define UAVTALK_DEBUG
#ifdef UAVTALK_DEBUG
#define UAVTALK_QXTLOG_DEBUG(args ...)
#else // UAVTALK_DEBUG
#define UAVTALK_QXTLOG_DEBUG(args ...)
#endif // UAVTALK_DEBUG
#define VERBOSE_FILTER(objId) //if (objId == 0xD23852DC)
#ifdef VERBOSE_UAVTALK
#define VERBOSE_FILTER(objId) if (objId == 0xD23852DC || objId == 0x2472A0AE)
#endif
#define SYNC_VAL 0x3C
@ -62,7 +57,6 @@ const quint8 UAVTalk::crc_table[256] = {
0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3
};
/**
* Constructor
*/
@ -261,7 +255,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
if (rxbyte != SYNC_VAL) {
// continue until sync byte is matched
UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Sync (" + QString::number(rxbyte) + " " + QString("0x%1").arg(rxbyte, 2, 16) + ")");
// TODO stats.rxSyncErrror++;
break;
}
@ -270,13 +264,15 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
rxPacketLength = 1;
// case local byte counter, don't forget to zero it after use.
rxCount = 0;
if (useUDPMirror) {
rxDataArray.clear();
rxDataArray.append(rxbyte);
}
rxState = STATE_TYPE;
UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Type");
break;
case STATE_TYPE:
@ -285,8 +281,9 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
rxCS = updateCRC(rxCS, rxbyte);
if ((rxbyte & TYPE_MASK) != TYPE_VER) {
qWarning() << "UAVTalk - error : bad type";
// TODO stats.rxSyncErrror++;
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Sync");
break;
}
@ -295,8 +292,6 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
packetSize = 0;
rxState = STATE_SIZE;
UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Size");
rxCount = 0;
break;
case STATE_SIZE:
@ -307,21 +302,21 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
if (rxCount == 0) {
packetSize += rxbyte;
rxCount++;
UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Size");
break;
}
packetSize += (quint32)rxbyte << 8;
rxCount = 0;
if (packetSize < MIN_HEADER_LENGTH || packetSize > MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH) { // incorrect packet size
if (packetSize < HEADER_LENGTH || packetSize > HEADER_LENGTH + MAX_PAYLOAD_LENGTH) {
// incorrect packet size
qWarning() << "UAVTalk - error : incorrect packet size";
stats.rxErrors++;
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Sync");
break;
}
rxCount = 0;
rxState = STATE_OBJID;
UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->ObjID");
break;
case STATE_OBJID:
@ -331,18 +326,37 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
rxTmpBuffer[rxCount++] = rxbyte;
if (rxCount < 4) {
UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->ObjID");
break;
}
rxCount = 0;
rxObjId = (qint32)qFromLittleEndian<quint32>(rxTmpBuffer);
// Message always contain an instance ID
rxInstId = 0;
rxState = STATE_INSTID;
break;
case STATE_INSTID:
// Update CRC
rxCS = updateCRC(rxCS, rxbyte);
rxTmpBuffer[rxCount++] = rxbyte;
if (rxCount < 2) {
break;
}
rxCount = 0;
rxInstId = (qint16)qFromLittleEndian<quint16>(rxTmpBuffer);
// Search for object, if not found reset state machine
rxObjId = (qint32)qFromLittleEndian<quint32>(rxTmpBuffer);
{
UAVObject *rxObj = objMngr->getObject(rxObjId);
if (rxObj == NULL && rxType != TYPE_OBJ_REQ) {
qWarning() << "UAVTalk - error : missing object" << rxObjId;
stats.rxErrors++;
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (badtype)");
break;
}
@ -360,50 +374,28 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
// Check length and determine next state
if (rxLength >= MAX_PAYLOAD_LENGTH) {
// packet error - exceeded payload max length
qWarning() << "UAVTalk - error : exceeded payload max length" << rxObjId;
stats.rxErrors++;
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (oversize)");
break;
}
// Check the lengths match
if ((rxPacketLength + INSTANCE_LENGTH + rxLength) != packetSize) {
if ((rxPacketLength + rxLength) != packetSize) {
// packet error - mismatched packet size
qWarning() << "UAVTalk - error : mismatched packet size" << rxObjId;
stats.rxErrors++;
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (length mismatch)");
break;
}
// Message always contain an instance ID
rxCount = 0;
rxInstId = 0;
rxState = STATE_INSTID;
}
break;
case STATE_INSTID:
// Update CRC
rxCS = updateCRC(rxCS, rxbyte);
rxTmpBuffer[rxCount++] = rxbyte;
if (rxCount < 2) {
UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->InstID");
break;
}
rxInstId = (qint16)qFromLittleEndian<quint16>(rxTmpBuffer);
rxCount = 0;
// If there is a payload get it, otherwise receive checksum
if (rxLength > 0) {
rxState = STATE_DATA;
UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->Data");
} else {
rxState = STATE_CS;
UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->CSum");
}
break;
@ -414,13 +406,11 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
rxBuffer[rxCount++] = rxbyte;
if (rxCount < rxLength) {
UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->Data");
break;
}
rxCount = 0;
rxState = STATE_CS;
UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->CSum");
rxCount = 0;
break;
case STATE_CS:
@ -430,17 +420,17 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
if (rxCS != rxCSPacket) {
// packet error - faulty CRC
qWarning() << "UAVTalk - error : faulty CRC" << rxObjId;
stats.rxErrors++;
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (badcrc)");
break;
}
if (rxPacketLength != packetSize + 1) {
// packet error - mismatched packet size
qWarning() << "UAVTalk - error : mismatched packet size" << rxObjId;
stats.rxErrors++;
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (length mismatch)");
break;
}
@ -458,14 +448,13 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
}
rxState = STATE_SYNC;
UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (OK)");
break;
default:
rxState = STATE_SYNC;
qWarning() << "UAVTalk - error : bad state";
stats.rxErrors++;
UAVTALK_QXTLOG_DEBUG("UAVTalk: \?\?\?->Sync"); // Use the escape character for '?' so that the tripgraph isn't triggered.
rxState = STATE_SYNC;
}
// Done
@ -491,6 +480,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length)
{
Q_UNUSED(length);
UAVObject *obj = NULL;
bool error = false;
bool allInstances = (instId == ALL_INSTANCES);
@ -603,7 +593,7 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
error = true;
}
if (error) {
qWarning() << "UAVTalk - !!! error receiving object" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
qWarning() << "UAVTalk - !!! error receiving" << typeToString(type) << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
}
// Done
return !error;
@ -624,19 +614,18 @@ UAVObject *UAVTalk::updateObject(quint32 objId, quint16 instId, quint8 *data)
// Get the object type
UAVObject *typeObj = objMngr->getObject(objId);
if (typeObj == NULL) {
qWarning() << "UAVTalk - Failed to get object, object ID :" << objId;
qWarning() << "UAVTalk - failed to get object, object ID :" << objId;
return NULL;
}
// Make sure this is a data object
UAVDataObject *dataObj = dynamic_cast<UAVDataObject *>(typeObj);
if (dataObj == NULL) {
qWarning() << "UAVTalk - Object is not a data object, object ID :" << objId;
return NULL;
}
// Create a new instance, unpack and register
UAVDataObject *instObj = dataObj->clone(instId);
if (!objMngr->registerObject(instObj)) {
qWarning() << "UAVTalk - Failed to register object " << instObj->toStringBrief();
qWarning() << "UAVTalk - failed to register object " << instObj->toStringBrief();
return NULL;
}
instObj->unpack(data);
@ -710,46 +699,44 @@ bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObje
}
bool allInstances = (instId == ALL_INSTANCES);
#ifdef VERBOSE_UAVTALK
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - transmitting" << typeToString(type) << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
// Process message type
bool ret = false;
if (type == TYPE_OBJ || type == TYPE_OBJ_ACK) {
if (allInstances) {
// Send all instances in reverse order
// This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received)
ret = true;
quint32 numInst = objMngr->getNumInstances(objId);
for (quint32 n = 0; n < numInst; ++n) {
quint32 i = numInst - n - 1;
UAVObject *o = objMngr->getObject(objId, i);
if (!transmitSingleObject(type, objId, i, o)) {
return false;
}
if (false) {
// yield
mutex.unlock();
// going back to the event loop is necessary to allow timeout events to be fired
// but don't allow user events as the event can cause the main thread to reenter UAVTalk
// the timer event suffers from the same issue but this case is handled
//QCoreApplication::processEvents(QEventLoop::ExcludeUserInputEvents, 10);
QThread::msleep(1);
mutex.lock();
ret = false;
break;
}
}
return true;
} else {
return transmitSingleObject(type, objId, instId, obj);
ret = transmitSingleObject(type, objId, instId, obj);
}
} else if (type == TYPE_OBJ_REQ) {
return transmitSingleObject(TYPE_OBJ_REQ, objId, instId, obj);
ret = transmitSingleObject(TYPE_OBJ_REQ, objId, instId, NULL);
} else if (type == TYPE_ACK || type == TYPE_NACK) {
if (!allInstances) {
return transmitSingleObject(type, objId, instId, obj);
} else {
return false;
ret = transmitSingleObject(type, objId, instId, NULL);
}
} else {
return false;
}
#ifdef VERBOSE_UAVTALK
if (!ret) {
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - failed to transmit" << typeToString(type) << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
}
#endif
return ret;
}
/**
@ -763,11 +750,6 @@ bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObje
bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
{
qint32 length;
qint32 dataOffset;
#ifdef VERBOSE_UAVTALK
VERBOSE_FILTER(objId) qDebug() << "UAVTalk - transmitting object" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
// IMPORTANT : obj can be null (when type is NACK for example)
@ -780,7 +762,6 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U
qToLittleEndian<quint32>(objId, &txBuffer[4]);
// Setup instance ID
qToLittleEndian<quint16>(instId, &txBuffer[8]);
dataOffset = 10;
// Determine data length
if (type == TYPE_OBJ_REQ || type == TYPE_ACK || type == TYPE_NACK) {
@ -791,37 +772,48 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U
// Check length
if (length >= MAX_PAYLOAD_LENGTH) {
qWarning() << "UAVTalk - error transmitting : object exceeds max payload length" << obj->toStringBrief();
return false;
}
// Copy data (if any)
if (length > 0) {
if (!obj->pack(&txBuffer[dataOffset])) {
if (!obj->pack(&txBuffer[HEADER_LENGTH])) {
qWarning() << "UAVTalk - error transmitting : failed to pack object" << obj->toStringBrief();
return false;
}
}
// Store the packet length
qToLittleEndian<quint16>(dataOffset + length, &txBuffer[2]);
qToLittleEndian<quint16>(HEADER_LENGTH + length, &txBuffer[2]);
// Calculate checksum
txBuffer[dataOffset + length] = updateCRC(0, txBuffer, dataOffset + length);
txBuffer[HEADER_LENGTH + length] = updateCRC(0, txBuffer, HEADER_LENGTH + length);
// Send buffer, check that the transmit backlog does not grow above limit
if (!io.isNull() && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE) {
io->write((const char *)txBuffer, dataOffset + length + CHECKSUM_LENGTH);
if (useUDPMirror) {
udpSocketRx->writeDatagram((const char *)txBuffer, dataOffset + length + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort());
if (!io.isNull() && io->isWritable()) {
if (io->bytesToWrite() < TX_BUFFER_SIZE) {
io->write((const char *)txBuffer, HEADER_LENGTH + length + CHECKSUM_LENGTH);
if (useUDPMirror) {
udpSocketRx->writeDatagram((const char *)txBuffer, HEADER_LENGTH + length + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort());
}
}
else {
qWarning() << "UAVTalk - error transmitting : io device full";
++stats.txErrors;
return false;
}
} else {
qWarning() << "UAVTalk - error transmitting : io device not writable";
++stats.txErrors;
return false;
}
// Update stats
++stats.txObjects;
stats.txBytes += dataOffset + length + CHECKSUM_LENGTH;
stats.txObjectBytes += length;
stats.txBytes += HEADER_LENGTH + length + CHECKSUM_LENGTH;
// Done
return true;
@ -913,3 +905,30 @@ void UAVTalk::closeAllTransactions()
delete objTransactions;
}
}
const char* UAVTalk::typeToString(quint8 type)
{
switch (type) {
case TYPE_OBJ:
return "object";
break;
case TYPE_OBJ_ACK:
return "object (acked)";
break;
case TYPE_OBJ_REQ:
return "object request";
break;
case TYPE_ACK:
return "ack";
break;
case TYPE_NACK:
return "nack";
break;
}
return "<error>";
}

13
ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h
View File

@ -91,18 +91,17 @@ private:
static const int TYPE_ACK = (TYPE_VER | 0x03);
static const int TYPE_NACK = (TYPE_VER | 0x04);
static const int MIN_HEADER_LENGTH = 8; // sync(1), type (1), size(2), object ID(4)
static const int MAX_HEADER_LENGTH = 10; // sync(1), type (1), size(2), object ID (4), instance ID(2, not used in single objects)
static const int CHECKSUM_LENGTH = 1;
// header : sync(1), type (1), size(2), object ID(4), instance ID(2)
static const int HEADER_LENGTH = 10;
static const int MAX_PAYLOAD_LENGTH = 256;
static const int MAX_PACKET_LENGTH = (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH);
static const int CHECKSUM_LENGTH = 1;
static const quint8 INSTANCE_LENGTH = 2;
static const int MAX_PACKET_LENGTH = (HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH);
static const int TX_BUFFER_SIZE = 2 * 1024;
static const quint8 crc_table[256];
// Types
@ -150,6 +149,8 @@ private:
void openTransaction(quint8 type, quint32 objId, quint16 instId);
void closeTransaction(Transaction *trans);
void closeAllTransactions();
const char *typeToString(quint8 type);
};
#endif // UAVTALK_H

4
ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro
View File

@ -5,8 +5,8 @@ QT += network
DEFINES += UAVTALK_LIBRARY
#DEFINES += VERBOSE_TELEMETRY
#DEFINES += VERBOSE_UAVTALK
DEFINES += VERBOSE_TELEMETRY
DEFINES += VERBOSE_UAVTALK
include(../../openpilotgcsplugin.pri)
include(uavtalk_dependencies.pri)