From 772c395e67994bbefd462905a9392b15ad1fa4b1 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Mon, 6 Jan 2014 21:35:00 +0100 Subject: [PATCH] OP-1122 OP-1145 fixed handling of ground messages done by OPLM - OPLM used to receive and relay all messages - OPLM will now reveice only specific messages and relay others as required (needs to be reviewed...) - add RadioComBridgeStats uavobject to collect and report tx/rx statistics - made error handling more robust - added a few FIXMEs --- .../modules/RadioComBridge/RadioComBridge.c | 202 ++++++++++++++---- .../boards/oplinkmini/firmware/Makefile | 1 + .../src/plugins/uavobjects/uavobjects.pro | 2 + .../radiocombridgestats.xml | 26 +++ 4 files changed, 193 insertions(+), 38 deletions(-) create mode 100644 shared/uavobjectdefinition/radiocombridgestats.xml diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index 88b2bb9d9..f913ab672 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -57,6 +58,7 @@ #define SERIAL_RX_BUF_LEN 100 #define PPM_INPUT_TIMEOUT 100 + // **************** // Private types @@ -81,10 +83,11 @@ typedef struct { uint8_t serialRxBuf[SERIAL_RX_BUF_LEN]; // Error statistics. - uint32_t comTxErrors; - uint32_t comTxRetries; - uint32_t UAVTalkErrors; - uint32_t droppedPackets; + uint32_t telemetryTxRetries; + uint32_t radioTxRetries; + + // Is this modem the coordinator + bool isCoordinator; // Should we parse UAVTalk? bool parseUAVTalk; @@ -107,6 +110,7 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length); static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte); static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte); static void objectPersistenceUpdatedCb(UAVObjEvent *objEv); +static void registerObject(UAVObjHandle obj); // **************** // Private variables @@ -124,7 +128,9 @@ static int32_t RadioComBridgeStart(void) // Get the settings. OPLinkSettingsData oplinkSettings; OPLinkSettingsGet(&oplinkSettings); - bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); + + // Check if this is the coordinator modem + data->isCoordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port). data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && @@ -165,12 +171,16 @@ static int32_t RadioComBridgeStart(void) // Configure our UAVObjects for updates. UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL); - if (is_coordinator) { + if (data->isCoordinator) { UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->radioEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); } else { UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); } + if (data->isCoordinator) { + registerObject(RadioComBridgeStatsHandle()); + } + // Configure the UAVObject callbacks ObjectPersistenceConnectCallback(&objectPersistenceUpdatedCb); @@ -223,6 +233,7 @@ static int32_t RadioComBridgeInitialize(void) OPLinkStatusInitialize(); ObjectPersistenceInitialize(); OPLinkReceiverInitialize(); + RadioComBridgeStatsInitialize(); // Initialise UAVTalk data->telemUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); @@ -233,9 +244,9 @@ static int32_t RadioComBridgeInitialize(void) data->radioEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); // Initialize the statistics. - data->comTxErrors = 0; - data->comTxRetries = 0; - data->UAVTalkErrors = 0; + data->telemetryTxRetries = 0; + data->radioTxRetries = 0; + data->parseUAVTalk = true; data->comSpeed = OPLINKSETTINGS_COMSPEED_9600; PIOS_COM_RADIO = PIOS_COM_RFM22B; @@ -244,6 +255,71 @@ static int32_t RadioComBridgeInitialize(void) } MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart); + +// TODO this code (badly) duplicates code from telemetry.c +// This method should be used only for periodically updated objects. +// The register method defined in telemetry.c should be factored out in a shared location so it can be +// used from here... +static void registerObject(UAVObjHandle obj) +{ + // Setup object for periodic updates + UAVObjEvent ev = { + .obj = obj, + .instId = UAVOBJ_ALL_INSTANCES, + .event = EV_UPDATED_PERIODIC, + }; + + // Get metadata + UAVObjMetadata metadata; + UAVObjGetMetadata(obj, &metadata); + + EventPeriodicQueueCreate(&ev, data->uavtalkEventQueue, metadata.telemetryUpdatePeriod); + UAVObjConnectQueue(obj, data->uavtalkEventQueue, EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ); +} + +/** + * Update telemetry statistics + */ +static void updateRadioComBridgeStats() +{ + UAVTalkStats telemetryUAVTalkStats; + UAVTalkStats radioUAVTalkStats; + RadioComBridgeStatsData radioComBridgeStats; + + // Get telemetry stats + UAVTalkGetStats(data->telemUAVTalkCon, &telemetryUAVTalkStats); + UAVTalkResetStats(data->telemUAVTalkCon); + + // Get radio stats + UAVTalkGetStats(data->radioUAVTalkCon, &radioUAVTalkStats); + UAVTalkResetStats(data->radioUAVTalkCon); + + // Get stats object data + RadioComBridgeStatsGet(&radioComBridgeStats); + + // Update stats object + radioComBridgeStats.TelemetryTxBytes += telemetryUAVTalkStats.txBytes; + radioComBridgeStats.TelemetryTxFailures += telemetryUAVTalkStats.txErrors; + radioComBridgeStats.TelemetryTxRetries += data->telemetryTxRetries; + + radioComBridgeStats.TelemetryRxBytes += telemetryUAVTalkStats.rxBytes; + radioComBridgeStats.TelemetryRxFailures += telemetryUAVTalkStats.rxErrors; + radioComBridgeStats.TelemetryRxSyncErrors += telemetryUAVTalkStats.rxSyncErrors; + radioComBridgeStats.TelemetryRxCrcErrors += telemetryUAVTalkStats.rxCrcErrors; + + radioComBridgeStats.RadioTxBytes += radioUAVTalkStats.txBytes; + radioComBridgeStats.RadioTxFailures += radioUAVTalkStats.txErrors; + radioComBridgeStats.RadioTxRetries += data->radioTxRetries; + + radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes; + radioComBridgeStats.RadioRxFailures += radioUAVTalkStats.rxErrors; + radioComBridgeStats.RadioRxSyncErrors += radioUAVTalkStats.rxSyncErrors; + radioComBridgeStats.RadioRxCrcErrors += radioUAVTalkStats.rxCrcErrors; + + // Update stats object data + RadioComBridgeStatsSet(&radioComBridgeStats); +} + /** * @brief Telemetry transmit task, regular priority * @@ -260,18 +336,20 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) #endif // Wait for queue message if (xQueueReceive(data->uavtalkEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) { - if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) { - // Send update (with retries) - uint32_t retries = 0; - int32_t success = -1; - while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; - if (!success) { - ++retries; - } - } - data->comTxRetries += retries; + if (ev.obj == RadioComBridgeStatsHandle()) { + updateRadioComBridgeStats(); } + // Send update (with retries) + uint32_t retries = 0; + int32_t success = -1; + while (retries < MAX_RETRIES && success == -1) { + success = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS) == 0; + if (success == -1) { + ++retries; + } + } + // Update stats + data->telemetryTxRetries += retries; } } } @@ -299,12 +377,12 @@ static void radioTxTask(__attribute__((unused)) void *parameters) uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; - if (!success) { + success = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS) == 0; + if (success == -1) { ++retries; } } - data->comTxRetries += retries; + data->radioTxRetries += retries; } } } @@ -333,6 +411,8 @@ static void radioRxTask(__attribute__((unused)) void *parameters) } } else if (PIOS_COM_TELEMETRY) { // Send the data straight to the telemetry port. + // FIXME following call can fail (with -2 error code) if buffer is full + // it is the caller responsibility to retry in such cases... PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process); } } @@ -425,8 +505,10 @@ static void serialRxTask(__attribute__((unused)) void *parameters) // Receive some data. uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, data->serialRxBuf, sizeof(data->serialRxBuf), MAX_PORT_DELAY); - // Send the data over the radio link. if (bytes_to_process > 0) { + // Send the data over the radio link. + // FIXME following call can fail (with -2 error code) if buffer is full + // it is the caller responsibility to retry in such cases... PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process); } } else { @@ -445,6 +527,7 @@ static void serialRxTask(__attribute__((unused)) void *parameters) */ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) { + int32_t ret; uint32_t outputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0; #if defined(PIOS_INCLUDE_USB) @@ -454,10 +537,13 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) } #endif /* PIOS_INCLUDE_USB */ if (outputPort) { - return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); + // FIXME following call can fail (with -2 error code) if buffer is full + // it is the caller responsibility to retry in such cases... + ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); } else { - return -1; + ret = -1; } + return ret; } /** @@ -477,10 +563,11 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length) // Don't send any data unless the radio port is available. if (outputPort && PIOS_COM_Available(outputPort)) { + // FIXME following call can fail (with -2 error code) if buffer is full + // it is the caller responsibility to retry in such cases... return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); } else { - // For some reason, if this function returns failure, it prevents saving settings. - return length; + return -1; } } @@ -494,12 +581,40 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length) static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte) { // Keep reading until we receive a completed packet. - UAVTalkRxState state = UAVTalkProcessInputStream(inConnectionHandle, rxbyte); + UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte); - if (state == UAVTALK_STATE_ERROR) { - data->UAVTalkErrors++; - } else if (state == UAVTALK_STATE_COMPLETE) { - UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); + if (state == UAVTALK_STATE_COMPLETE) { + // We only want to unpack certain telemetry objects + uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle); + switch (objId) { + case OPLINKSTATUS_OBJID: + case OPLINKSETTINGS_OBJID: + case OPLINKRECEIVER_OBJID: + case MetaObjectId(OPLINKSTATUS_OBJID): + case MetaObjectId(OPLINKSETTINGS_OBJID): + case MetaObjectId(OPLINKRECEIVER_OBJID): + UAVTalkReceiveObject(inConnectionHandle); + break; + case OBJECTPERSISTENCE_OBJID: + case MetaObjectId(OBJECTPERSISTENCE_OBJID): + // receive object locally + // some objects will send back a response to telemetry + // FIXME: + // OPLM will ack or nack all objects requests and acked object sends + // Receiver will probably also ack / nack the same messages + // This has some consequences like : + // Second ack/nack will not match an open transaction or will apply to wrong transaction + // Question : how does GCS handle receiving the same object twice + // The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks... + UAVTalkReceiveObject(inConnectionHandle); + // relay packet to remote modem + UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); + break; + default: + // all other packets are relayed to the remote modem + UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); + break; + } } } @@ -515,19 +630,30 @@ static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConn // Keep reading until we receive a completed packet. UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte); - if (state == UAVTALK_STATE_ERROR) { - data->UAVTalkErrors++; - } else if (state == UAVTALK_STATE_COMPLETE) { - // We only want to unpack certain objects from the remote modem. + if (state == UAVTALK_STATE_COMPLETE) { + // We only want to unpack certain objects from the remote modem + // Similarly we only want to relay certain objects to the telemetry port uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle); switch (objId) { case OPLINKSTATUS_OBJID: case OPLINKSETTINGS_OBJID: + case MetaObjectId(OPLINKSTATUS_OBJID): + case MetaObjectId(OPLINKSETTINGS_OBJID): + // Ignore object... + // These objects are shadowed by the modem and are not transmitted to the telemetry port + // - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead + // - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead break; case OPLINKRECEIVER_OBJID: + case MetaObjectId(OPLINKRECEIVER_OBJID): + // Receive object locally + // These objects are received by the modem and are not transmitted to the telemetry port + // - OPLINKRECEIVER_OBJID : not sure why + // some objects will send back a response to the remote modem UAVTalkReceiveObject(inConnectionHandle); break; default: + // all other packets are relayed to the telemetry port UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); break; } @@ -545,7 +671,7 @@ static void objectPersistenceUpdatedCb(__attribute__((unused)) UAVObjEvent *objE ObjectPersistenceGet(&obj_per); - // Is this concerning or setting object? + // Is this concerning our setting object? if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) { // Is this a save, load, or delete? bool success = false; diff --git a/flight/targets/boards/oplinkmini/firmware/Makefile b/flight/targets/boards/oplinkmini/firmware/Makefile index 12a491df5..3a9c6b113 100644 --- a/flight/targets/boards/oplinkmini/firmware/Makefile +++ b/flight/targets/boards/oplinkmini/firmware/Makefile @@ -53,6 +53,7 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/oplinksettings.c SRC += $(OPUAVSYNTHDIR)/objectpersistence.c SRC += $(OPUAVSYNTHDIR)/oplinkreceiver.c + SRC += $(OPUAVSYNTHDIR)/radiocombridgestats.c else ## Test Code SRC += $(OPTESTS)/test_common.c diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index d1b002994..f65bffd4d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -111,6 +111,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/oplinksettings.h \ $$UAVOBJECT_SYNTHETICS/oplinkstatus.h \ $$UAVOBJECT_SYNTHETICS/oplinkreceiver.h \ + $$UAVOBJECT_SYNTHETICS/radiocombridgestats.h \ $$UAVOBJECT_SYNTHETICS/osdsettings.h \ $$UAVOBJECT_SYNTHETICS/waypoint.h \ $$UAVOBJECT_SYNTHETICS/waypointactive.h \ @@ -203,6 +204,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/oplinksettings.cpp \ $$UAVOBJECT_SYNTHETICS/oplinkstatus.cpp \ $$UAVOBJECT_SYNTHETICS/oplinkreceiver.cpp \ + $$UAVOBJECT_SYNTHETICS/radiocombridgestats.cpp \ $$UAVOBJECT_SYNTHETICS/osdsettings.cpp \ $$UAVOBJECT_SYNTHETICS/waypoint.cpp \ $$UAVOBJECT_SYNTHETICS/waypointactive.cpp \ diff --git a/shared/uavobjectdefinition/radiocombridgestats.xml b/shared/uavobjectdefinition/radiocombridgestats.xml new file mode 100644 index 000000000..1fd357924 --- /dev/null +++ b/shared/uavobjectdefinition/radiocombridgestats.xml @@ -0,0 +1,26 @@ + + + Maintains the telemetry statistics from the OPLM RadioComBridge. + + + + + + + + + + + + + + + + + + + + + + +