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-1145 flight side uavtalk and telemetry enhancements

Browse Source 
- added more stats (bytes, sync errors, crc errors)
- made error handling more robust
- added some optional PIOS_DEBUGLOG calls
- fixed a timestamp handling bug
This commit is contained in:
Philippe Renon 2013-12-19 00:17:08 +01:00
parent c2e8d25319
commit 5f33fadb1b
5 changed files with 118 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
flight/modules/Telemetry/telemetry.c
View File

@ -179,7 +179,6 @@ static void registerObject(UAVObjHandle obj)
if (UAVObjIsMetaobject(obj)) {
/* Only connect change notifications for meta objects. No periodic updates */
UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES);
return;
} else {
// Setup object for periodic updates
UAVObjEvent ev = {
@ -191,7 +190,6 @@ static void registerObject(UAVObjHandle obj)
ev.event = EV_LOGGING_PERIODIC;
EventPeriodicQueueCreate(&ev, queue, 0);
// Setup object for telemetry updates
updateObject(obj, EV_NONE);
}
@ -208,9 +206,8 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
int32_t eventMask;
if (UAVObjIsMetaobject(obj)) {
/* This function updates the periodic updates for the object.
* Meta Objects cannot have periodic updates.
*/
// This function updates the periodic updates for the object.
// Meta Objects cannot have periodic updates.
PIOS_Assert(false);
return;
}
@ -318,29 +315,36 @@ static void processObjEvent(UAVObjEvent *ev)
if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) {
// Send update to GCS (with retries)
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
++retries;
// call blocks until ack is received or timeout
success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS);
if (success == -1) {
++retries;
}
}
// Update stats
txRetries += (retries - 1);
txRetries += retries;
if (success == -1) {
++txErrors;
}
} else if (ev->event == EV_UPDATE_REQ) {
// Request object update from GCS (with retries)
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
++retries;
// call blocks until update is received or timeout
success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS);
if (success == -1) {
++retries;
}
}
// Update stats
txRetries += (retries - 1);
txRetries += retries;
if (success == -1) {
++txErrors;
}
}
// If this is a metaobject then make necessary telemetry updates
if (UAVObjIsMetaobject(ev->obj)) {
updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); // linked object will be the actual object the metadata are for
// linked object will be the actual object the metadata are for
updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE);
} else {
if (updateMode == UPDATEMODE_THROTTLED) {
// If this is UPDATEMODE_THROTTLED, the event mask changes on every event.
@ -553,22 +557,30 @@ static void updateTelemetryStats()
// Update stats object
if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
flightStats.RxFailures += utalkStats.rxErrors;
flightStats.TxBytes += utalkStats.txBytes;
flightStats.TxFailures += txErrors;
flightStats.TxRetries += txRetries;
txErrors = 0;
txRetries = 0;
flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
flightStats.RxBytes += utalkStats.rxBytes;
flightStats.RxFailures += utalkStats.rxErrors;
flightStats.RxSyncErrors += utalkStats.rxSyncErrors;
flightStats.RxCrcErrors += utalkStats.rxCrcErrors;
} else {
flightStats.RxDataRate = 0;
flightStats.TxDataRate = 0;
flightStats.RxFailures = 0;
flightStats.TxBytes = 0;
flightStats.TxFailures = 0;
flightStats.TxRetries = 0;
txErrors = 0;
txRetries = 0;
flightStats.RxDataRate = 0;
flightStats.RxBytes = 0;
flightStats.RxFailures = 0;
flightStats.RxSyncErrors = 0;
flightStats.RxCrcErrors = 0;
}
txErrors = 0;
txRetries = 0;
// Check for connection timeout
timeNow = xTaskGetTickCount() * portTICK_RATE_MS;

10
flight/uavtalk/inc/uavtalk.h
View File

@ -34,13 +34,17 @@ typedef int32_t (*UAVTalkOutputStream)(uint8_t *data, int32_t length);
typedef struct {
uint32_t txBytes;
uint32_t rxBytes;
uint32_t txObjectBytes;
uint32_t rxObjectBytes;
uint32_t rxObjects;
uint32_t txObjects;
uint32_t txErrors;
uint32_t rxBytes;
uint32_t rxObjectBytes;
uint32_t rxObjects;
uint32_t rxErrors;
uint32_t rxSyncErrors;
uint32_t rxCrcErrors;
} UAVTalkStats;
typedef void *UAVTalkConnection;

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

@ -76,9 +76,8 @@ typedef struct {
} UAVTalkConnectionData;
#define UAVTALK_CANARI 0xCA
#define UAVTALK_WAITFOREVER -1
#define UAVTALK_NOWAIT 0
#define UAVTALK_SYNC_VAL 0x3C
#define UAVTALK_TYPE_MASK 0x78
#define UAVTALK_TYPE_VER 0x20
#define UAVTALK_TIMESTAMPED 0x80

119
flight/uavtalk/uavtalk.c
View File

@ -32,11 +32,21 @@
#include "openpilot.h"
#include "uavtalk_priv.h"
//#define UAV_DEBUGLOG 1
#if defined UAV_DEBUGLOG && defined(FLASH_FREETOS)
#define UAVT_DEBUGLOG_PRINTF(...) PIOS_DEBUGLOG_Printf(__VA_ARGS__)
#define UAVT_DEBUGLOG_CPRINTF(objId, ...) if (objId == 0x5E5903CC) { UAVT_DEBUGLOG_PRINTF(__VA_ARGS__); }
#else
#define UAVT_DEBUGLOG_PRINTF(...)
#define UAVT_DEBUGLOG_CPRINTF(objId, ...)
#endif
// 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);
static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj);
static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data, int32_t length);
static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data);
static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId);
/**
@ -150,13 +160,15 @@ void UAVTalkAddStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut)
// Copy stats
statsOut->txBytes += connection->stats.txBytes;
statsOut->rxBytes += connection->stats.rxBytes;
statsOut->txObjectBytes += connection->stats.txObjectBytes;
statsOut->rxObjectBytes += connection->stats.rxObjectBytes;
statsOut->txObjects += connection->stats.txObjects;
statsOut->rxObjects += connection->stats.rxObjects;
statsOut->txErrors += connection->stats.txErrors;
statsOut->rxBytes += connection->stats.rxBytes;
statsOut->rxObjectBytes += connection->stats.rxObjectBytes;
statsOut->rxObjects += connection->stats.rxObjects;
statsOut->rxErrors += connection->stats.rxErrors;
statsOut->rxSyncErrors += connection->stats.rxSyncErrors;
statsOut->rxCrcErrors += connection->stats.rxCrcErrors;
// Release lock
xSemaphoreGiveRecursive(connection->lock);
@ -278,7 +290,7 @@ int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjH
static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs)
{
int32_t respReceived;
int32_t ret = -1;
// Send object depending on if a response is needed
if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS || type == UAVTALK_TYPE_OBJ_REQ) {
// Get transaction lock (will block if a transaction is pending)
@ -289,12 +301,19 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type
connection->respType = (type == UAVTALK_TYPE_OBJ_REQ) ? UAVTALK_TYPE_OBJ : UAVTALK_TYPE_ACK;
connection->respObjId = UAVObjGetID(obj);
connection->respInstId = instId;
sendObject(connection, type, UAVObjGetID(obj), instId, obj);
ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj);
xSemaphoreGiveRecursive(connection->lock);
// Wait for response (or timeout)
respReceived = xSemaphoreTake(connection->respSema, timeoutMs / portTICK_RATE_MS);
// Wait for response (or timeout) if sending the object succeeded
respReceived = pdFALSE;
if (ret == 0) {
respReceived = xSemaphoreTake(connection->respSema, timeoutMs / portTICK_RATE_MS);
}
// Check if a response was received
if (respReceived == pdFALSE) {
if (respReceived == pdTRUE) {
// We are done successfully
xSemaphoreGiveRecursive(connection->transLock);
ret = 0;
} else {
// Cancel transaction
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
// non blocking call to make sure the value is reset to zero (binary sema)
@ -303,18 +322,13 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type
xSemaphoreGiveRecursive(connection->lock);
xSemaphoreGiveRecursive(connection->transLock);
return -1;
} else {
xSemaphoreGiveRecursive(connection->transLock);
return 0;
}
} else if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS) {
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
sendObject(connection, type, UAVObjGetID(obj), instId, obj);
ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj);
xSemaphoreGiveRecursive(connection->lock);
return 0;
} else {
return -1;
}
return ret;
}
/**
@ -347,7 +361,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
case UAVTALK_STATE_SYNC:
if (rxbyte != UAVTALK_SYNC_VAL) {
// TODO connection->stats.rxSyncErrors++;
connection->stats.rxSyncErrors++;
break;
}
@ -355,27 +369,26 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
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:
// update the CRC
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;
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;
@ -414,7 +427,6 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
}
iproc->rxCount = 0;
// Message always contain an instance ID
iproc->instId = 0;
iproc->state = UAVTALK_STATE_INSTID;
break;
@ -437,12 +449,12 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
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->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength;
}
iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
}
// Check length
@ -477,6 +489,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
break;
case UAVTALK_STATE_TIMESTAMP:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
@ -510,9 +523,11 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
case UAVTALK_STATE_CS:
// the CRC byte
// 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;
@ -532,8 +547,9 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
break;
default:
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
// Done
@ -576,6 +592,7 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC
// The input packet must be completely parsed.
if (inIproc->state != UAVTALK_STATE_COMPLETE) {
inConnection->stats.rxErrors++;
return -1;
}
@ -583,13 +600,13 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC
CHECKCONHANDLE(outConnectionHandle, outConnection, return -1);
if (!outConnection->outStream) {
outConnection->stats.txErrors++;
return -1;
}
// Lock
xSemaphoreTakeRecursive(outConnection->lock, portMAX_DELAY);
// Setup sync byte
outConnection->txBuffer[0] = UAVTALK_SYNC_VAL;
// Setup type
outConnection->txBuffer[1] = inIproc->type;
@ -628,11 +645,12 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC
int32_t rc = (*outConnection->outStream)(outConnection->txBuffer, headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH);
// Update stats
outConnection->stats.txBytes += rc;
outConnection->stats.txBytes += (rc > 0) ? rc : 0;
// evaluate return value before releasing the lock
UAVTalkRxState rxState = 0;
if (rc != (int32_t) (headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH)) {
outConnection->stats.txErrors++;
rxState = -1;
}
@ -660,7 +678,7 @@ int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle)
return -1;
}
receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length);
receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer);
return 0;
}
@ -668,7 +686,6 @@ int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle)
/**
* Get the object ID of the current packet.
* \param[in] connectionHandle UAVTalkConnection to be used
* \param[in] objId Object ID to send a NACK for
* \return The object ID, or 0 on error.
*/
uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle)
@ -698,13 +715,7 @@ uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle)
* \return 0 Success
* \return -1 Failure
*/
static int32_t receiveObject(UAVTalkConnectionData *connection,
uint8_t type,
uint32_t objId,
uint16_t instId,
uint8_t *data,
__attribute__((unused)) int32_t length)
{
static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data) {
UAVObjHandle obj;
int32_t ret = 0;
@ -741,10 +752,12 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
case UAVTALK_TYPE_OBJ_ACK:
case UAVTALK_TYPE_OBJ_ACK_TS:
UAVT_DEBUGLOG_CPRINTF(objId, "OBJ_ACK %X %d", objId, instId);
// All instances not allowed for OBJ_ACK messages
if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
// Unpack object, if the instance does not exist it will be created!
if (UAVObjUnpack(obj, instId, data) == 0) {
UAVT_DEBUGLOG_CPRINTF(objId, "OBJ ACK %X %d", objId, instId);
// Object updated or created, transmit ACK
sendObject(connection, UAVTALK_TYPE_ACK, objId, instId, NULL);
} else {
@ -755,21 +768,24 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
}
if (ret == -1) {
// failed to update object, transmit NACK
UAVT_DEBUGLOG_PRINTF("OBJ NACK %X %d", objId, instId);
sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
}
break;
case UAVTALK_TYPE_OBJ_REQ:
// Check if requested object exists
if (obj && ((instId == UAVOBJ_ALL_INSTANCES) || instId < UAVObjGetNumInstances(obj))) {
UAVT_DEBUGLOG_CPRINTF(objId, "REQ %X %d", objId, instId);
if (obj) {
// Object found, transmit it
// The sent object will ack the object request on the receiver side
sendObject(connection, UAVTALK_TYPE_OBJ, objId, instId, obj);
ret = sendObject(connection, UAVTALK_TYPE_OBJ, objId, instId, obj);
} else {
ret = -1;
}
if (ret == -1) {
// failed to send object, transmit NACK
UAVT_DEBUGLOG_PRINTF("REQ NACK %X %d", objId, instId);
sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
}
break;
@ -841,6 +857,7 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3
{
uint32_t numInst;
uint32_t n;
uint32_t ret = -1;
// Important note : obj can be null (when type is NACK for example) so protect all obj dereferences.
@ -856,26 +873,25 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3
numInst = UAVObjGetNumInstances(obj);
// 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 = 0;
for (n = 0; n < numInst; ++n) {
if (sendSingleObject(connection, type, objId, numInst - n - 1, obj) == -1) {
return -1;
ret = -1;
break;
}
}
return 0;
} else {
return sendSingleObject(connection, type, objId, instId, obj);
ret = sendSingleObject(connection, type, objId, instId, obj);
}
} else if (type == UAVTALK_TYPE_OBJ_REQ) {
return sendSingleObject(connection, UAVTALK_TYPE_OBJ_REQ, objId, instId, obj);
ret = sendSingleObject(connection, type, objId, instId, obj);
} else if (type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) {
if (instId != UAVOBJ_ALL_INSTANCES) {
return sendSingleObject(connection, type, objId, instId, obj);
} else {
return -1;
ret = sendSingleObject(connection, type, objId, instId, obj);
}
} else {
return -1;
}
return ret;
}
/**
@ -894,6 +910,7 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type,
// IMPORTANT : obj can be null (when type is NACK for example)
if (!connection->outStream) {
connection->stats.txErrors++;
return -1;
}

12
shared/uavobjectdefinition/flighttelemetrystats.xml
View File

@ -1,12 +1,20 @@
<xml>
<object name="FlightTelemetryStats" singleinstance="true" settings="false" category="System">
<description>Maintains the telemetry statistics from the OpenPilot flight computer.</description>
<field name="Status" units="" type="enum" elements="1" options="Disconnected,HandshakeReq,HandshakeAck,Connected"/>
<field name="TxDataRate" units="bytes/sec" type="float" elements="1"/>
<field name="RxDataRate" units="bytes/sec" type="float" elements="1"/>
<field name="TxBytes" units="bytes" type="uint32" elements="1"/>
<field name="TxFailures" units="count" type="uint32" elements="1"/>
<field name="RxFailures" units="count" type="uint32" elements="1"/>
<field name="TxRetries" units="count" type="uint32" elements="1"/>
<field name="RxDataRate" units="bytes/sec" type="float" elements="1"/>
<field name="RxBytes" units="bytes" type="uint32" elements="1"/>
<field name="RxFailures" units="count" type="uint32" elements="1"/>
<field name="RxSyncErrors" units="count" type="uint32" elements="1"/>
<field name="RxCrcErrors" units="count" type="uint32" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="5000"/>