diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 5a3c5a62e..dad266505 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -100,29 +100,34 @@ #ifdef PIOS_INCLUDE_RFM22B #define HAS_RADIO #endif + // Private types typedef struct { // Determine port on which to communicate telemetry information uint32_t (*getPort)(); // Main telemetry queue xQueueHandle queue; + #ifdef PIOS_TELEM_PRIORITY_QUEUE // Priority telemetry queue xQueueHandle priorityQueue; #endif /* PIOS_TELEM_PRIORITY_QUEUE */ + // Transmit/receive task handles xTaskHandle txTaskHandle; xTaskHandle rxTaskHandle; // Telemetry stream UAVTalkConnection uavTalkCon; } channelContext; + #ifdef HAS_RADIO // Main telemetry channel static channelContext localChannel; static int32_t transmitLocalData(uint8_t *data, int32_t length); static void registerLocalObject(UAVObjHandle obj); static uint32_t localPort(); -#endif +#endif /* ifdef HAS_RADIO */ + static void updateSettings(channelContext *channel); // OPLink telemetry channel @@ -165,6 +170,7 @@ static void gcsTelemetryStatsUpdated(); */ int32_t TelemetryStart(void) { + #ifdef HAS_RADIO // Only start the local telemetry tasks if needed if (localPort()) { @@ -195,6 +201,7 @@ int32_t TelemetryStart(void) localChannel.rxTaskHandle); } #endif /* ifdef HAS_RADIO */ + // Start the telemetry tasks associated with Radio/USB UAVObjIterate(®isterRadioObject); @@ -231,6 +238,7 @@ void TelemetryInitializeChannel(channelContext *channel) // Create object queues channel->queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + #if defined(PIOS_TELEM_PRIORITY_QUEUE) channel->priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); @@ -283,6 +291,7 @@ int32_t TelemetryInitialize(void) // Reset link stats txErrors = 0; txRetries = 0; + #ifdef HAS_RADIO // Set channel port handlers localChannel.getPort = localPort; @@ -297,8 +306,8 @@ int32_t TelemetryInitialize(void) // Initialise UAVTalk localChannel.uavTalkCon = UAVTalkInitialize(&transmitLocalData); } - #endif /* ifdef HAS_RADIO */ + // Set channel port handlers radioChannel.getPort = radioPort; @@ -314,6 +323,7 @@ int32_t TelemetryInitialize(void) } MODULE_INITCALL(TelemetryInitialize, TelemetryStart); + #ifdef HAS_RADIO /** * Register a new object, adds object to local list and connects the queue depending on the object's @@ -464,6 +474,7 @@ static void updateObject( UAVObjConnectQueue(obj, channel->priorityQueue, eventMask); } else #endif /* PIOS_TELEM_PRIORITY_QUEUE */ + UAVObjConnectQueue(obj, channel->queue, eventMask); } @@ -589,6 +600,7 @@ static void telemetryTxTask(void *parameters) /** * Tries to empty the high priority queue before handling any standard priority item */ + #ifdef PIOS_TELEM_PRIORITY_QUEUE // empty priority queue, non-blocking while (xQueueReceive(channel->priorityQueue, &ev, 0) == pdTRUE) { @@ -611,6 +623,7 @@ static void telemetryTxTask(void *parameters) processObjEvent(channel, &ev); } #endif /* PIOS_TELEM_PRIORITY_QUEUE */ + } } @@ -655,6 +668,7 @@ static uint32_t localPort() { return PIOS_COM_TELEM_RF; } + #endif /* ifdef HAS_RADIO */ /** @@ -816,9 +830,11 @@ static void updateTelemetryStats() // Get stats UAVTalkGetStats(radioChannel.uavTalkCon, &utalkStats, true); + #ifdef HAS_RADIO UAVTalkAddStats(localChannel.uavTalkCon, &utalkStats, true); #endif + // Get object data FlightTelemetryStatsGet(&flightStats); GCSTelemetryStatsGet(&gcsStats);