From 6bb732fb02b0070ff965373014bffe77c237230a Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Sat, 14 Mar 2015 23:58:18 +0000 Subject: [PATCH 1/8] OP-1289 Need Revo to send two telemetry streams for OSD and GCS --- flight/modules/Telemetry/telemetry.c | 495 +++++++++++------- .../boards/revolution/firmware/pios_board.c | 5 +- 2 files changed, 320 insertions(+), 180 deletions(-) diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 142c0a2ad..c5c2350e7 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -30,6 +30,30 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +/* Telemetry uses four tasks. Two are created for the main telemetry + * stream called "TelTx" and "TelRx". Two are created to handle the OPLink + * radio connection, called "RadioTx" and "Radio Rx", the latter being + * overridden by USB if connected. + * + * The telemetry port to use is defined by PIOS_COM_TELEM_RF in + * PIOS_Board_Init(). + * + * A UAVTalk connection instance, telemUavTalkCon, is associated with the main + * telemetry channel and another, radioUavTalkCon, with the radio channel. + * Associated with each instance is a transmit routine which will send data + * to the appropriate port. + * + * Data is passed on the telemetry channels using queues. If + * PIOS_TELEM_PRIORITY_QUEUE is defined then two queues are created, one normal + * priority and the other high priority. + * + * The "Tx" tasks read events first from the priority queue and then from + * the normal queue, passing each event to processObjEvent() which ultimately + * passes each event to the UAVTalk library which results in the appropriate + * transmit routine being called to send the data back to the recipient on + * the telemetry or radio link. + */ + #include #include "telemetry.h" @@ -53,62 +77,72 @@ #ifdef PIOS_TELEM_RADIO_RX_STACK_SIZE #define STACK_SIZE_RADIO_RX_BYTES PIOS_TELEM_RADIO_RX_STACK_SIZE +#define STACK_SIZE_RADIO_TX_BYTES PIOS_TELEM_RADIO_TX_STACK_SIZE #else #define STACK_SIZE_RADIO_RX_BYTES STACK_SIZE_RX_BYTES +#define STACK_SIZE_RADIO_TX_BYTES STACK_SIZE_TX_BYTES #endif #define TASK_PRIORITY_RX (tskIDLE_PRIORITY + 2) #define TASK_PRIORITY_TX (tskIDLE_PRIORITY + 2) #define TASK_PRIORITY_RADRX (tskIDLE_PRIORITY + 2) +#define TASK_PRIORITY_RADTX (tskIDLE_PRIORITY + 2) #define REQ_TIMEOUT_MS 250 #define MAX_RETRIES 2 #define STATS_UPDATE_PERIOD_MS 4000 #define CONNECTION_TIMEOUT_MS 8000 // Private types +typedef struct { + // Port on which to communicate telemetry information + uint32_t telemetryPort; + // Main telemetry queue + xQueueHandle mainQueue; + #ifdef PIOS_TELEM_PRIORITY_QUEUE + // Priority telemetry queue + xQueueHandle priorityQueue; + #endif /* PIOS_TELEM_PRIORITY_QUEUE */ + // Transmit/receive task handles + xTaskHandle telemetryTxTaskHandle; + xTaskHandle telemetryRxTaskHandle; + // Telemetry stream + UAVTalkConnection uavTalkCon; +} telemetryContext; -// Private variables -static uint32_t telemetryPort; -#ifdef PIOS_INCLUDE_RFM22B -static uint32_t radioPort; -#endif -static xQueueHandle queue; +// Main telemetry port +static telemetryContext telemHandle; +static int32_t transmitData(uint8_t *data, int32_t length); +static void registerTelemObject(UAVObjHandle obj); -#if defined(PIOS_TELEM_PRIORITY_QUEUE) -static xQueueHandle priorityQueue; -#else -#define priorityQueue queue -#endif +// OPLink telemetry port +static telemetryContext radioHandle; +static int32_t transmitRadioData(uint8_t *data, int32_t length); +static void registerRadioObject(UAVObjHandle obj); -static xTaskHandle telemetryTxTaskHandle; -static xTaskHandle telemetryRxTaskHandle; -#ifdef PIOS_INCLUDE_RFM22B -static xTaskHandle radioRxTaskHandle; -#endif +// Telemetry stats static uint32_t txErrors; static uint32_t txRetries; static uint32_t timeOfLastObjectUpdate; -static UAVTalkConnection uavTalkCon; -#ifdef PIOS_INCLUDE_RFM22B -static UAVTalkConnection radioUavTalkCon; -#endif -// Private functions static void telemetryTxTask(void *parameters); static void telemetryRxTask(void *parameters); -#ifdef PIOS_INCLUDE_RFM22B -static void radioRxTask(void *parameters); -static int32_t transmitRadioData(uint8_t *data, int32_t length); -#endif -static int32_t transmitData(uint8_t *data, int32_t length); -static void registerObject(UAVObjHandle obj); -static void updateObject(UAVObjHandle obj, int32_t eventType); -static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs); -static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs); -static void processObjEvent(UAVObjEvent *ev); +static void updateObject( + telemetryContext *telemetryHandle, + UAVObjHandle obj, + int32_t eventType); +static void processObjEvent( + telemetryContext *telemetryHandle, + UAVObjEvent *ev); +static int32_t setUpdatePeriod( + telemetryContext *telemetryHandle, + UAVObjHandle obj, + int32_t updatePeriodMs); +static int32_t setLoggingPeriod( + telemetryContext *telemetryHandle, + UAVObjHandle obj, + int32_t updatePeriodMs); static void updateTelemetryStats(); static void gcsTelemetryStatsUpdated(); static void updateSettings(); -static uint32_t getComPort(bool input); /** * Initialise the telemetry module @@ -117,22 +151,52 @@ static uint32_t getComPort(bool input); */ int32_t TelemetryStart(void) { - // Process all registered objects and connect queue for updates - UAVObjIterate(®isterObject); + UAVObjIterate(®isterTelemObject); + UAVObjIterate(®isterRadioObject); // Listen to objects of interest - GCSTelemetryStatsConnectQueue(priorityQueue); +#ifdef PIOS_TELEM_PRIORITY_QUEUE + GCSTelemetryStatsConnectQueue(telemHandle.priorityQueue); + GCSTelemetryStatsConnectQueue(radioHandle.priorityQueue); +#else /* PIOS_TELEM_PRIORITY_QUEUE */ + GCSTelemetryStatsConnectQueue(telemHandle.mainQueue); + GCSTelemetryStatsConnectQueue(radioHandle.mainQueue); +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ // Start telemetry tasks - xTaskCreate(telemetryTxTask, "TelTx", STACK_SIZE_TX_BYTES / 4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle); - xTaskCreate(telemetryRxTask, "TelRx", STACK_SIZE_RX_BYTES / 4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle); + xTaskCreate(telemetryTxTask, + "TelTx", + STACK_SIZE_TX_BYTES / 4, + &telemHandle, + TASK_PRIORITY_TX, + &telemHandle.telemetryTxTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, + telemHandle.telemetryTxTaskHandle); + xTaskCreate(telemetryRxTask, + "TelRx", + STACK_SIZE_RX_BYTES / 4, + &telemHandle, + TASK_PRIORITY_RX, + &telemHandle.telemetryRxTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, + telemHandle.telemetryRxTaskHandle); -#ifdef PIOS_INCLUDE_RFM22B - xTaskCreate(radioRxTask, "RadioRx", STACK_SIZE_RADIO_RX_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, radioRxTaskHandle); -#endif + xTaskCreate(telemetryTxTask, + "RadioTx", + STACK_SIZE_RADIO_TX_BYTES / 4, + &radioHandle, + TASK_PRIORITY_RADTX, + &radioHandle.telemetryTxTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, + radioHandle.telemetryTxTaskHandle); + xTaskCreate(telemetryRxTask, + "RadioRx", + STACK_SIZE_RADIO_RX_BYTES / 4, + &radioHandle, + TASK_PRIORITY_RADRX, + &radioHandle.telemetryRxTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, + radioHandle.telemetryRxTaskHandle); return 0; } @@ -148,27 +212,41 @@ int32_t TelemetryInitialize(void) GCSTelemetryStatsInitialize(); // Initialize vars - timeOfLastObjectUpdate = 0; + timeOfLastObjectUpdate = 0; // Create object queues - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + telemHandle.mainQueue = xQueueCreate(MAX_QUEUE_SIZE, + sizeof(UAVObjEvent)); + radioHandle.mainQueue = xQueueCreate(MAX_QUEUE_SIZE, + sizeof(UAVObjEvent)); #if defined(PIOS_TELEM_PRIORITY_QUEUE) - priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); -#endif + telemHandle.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, + sizeof(UAVObjEvent)); + radioHandle.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, + sizeof(UAVObjEvent)); +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ // Update telemetry settings - telemetryPort = PIOS_COM_TELEM_RF; + telemHandle.telemetryPort = PIOS_COM_TELEM_RF; + #ifdef PIOS_INCLUDE_RFM22B - radioPort = PIOS_COM_RF; -#endif + radioHandle.telemetryPort = PIOS_COM_RF; +#else /* PIOS_INCLUDE_RFM22B */ + radioHandle.telemetryPort = 0; +#endif /* PIOS_INCLUDE_RFM22B */ +#ifdef PIOS_INCLUDE_USB + // if USB is connected, USB takes precedence for telemetry + if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) { + radioHandle.telemetryPort = PIOS_COM_TELEM_USB; + } +#endif /* PIOS_INCLUDE_USB */ + HwSettingsInitialize(); updateSettings(); // Initialise UAVTalk - uavTalkCon = UAVTalkInitialize(&transmitData); -#ifdef PIOS_INCLUDE_RFM22B - radioUavTalkCon = UAVTalkInitialize(&transmitRadioData); -#endif + telemHandle.uavTalkCon = UAVTalkInitialize(&transmitData); + radioHandle.uavTalkCon = UAVTalkInitialize(&transmitRadioData); // Create periodic event that will be used to update the telemetry stats // FIXME STATS_UPDATE_PERIOD_MS is 4000ms while FlighTelemetryStats update period is 5000ms... @@ -176,7 +254,22 @@ int32_t TelemetryInitialize(void) txRetries = 0; UAVObjEvent ev; memset(&ev, 0, sizeof(UAVObjEvent)); - EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS); + +#ifdef PIOS_TELEM_PRIORITY_QUEUE + EventPeriodicQueueCreate(&ev, + telemHandle.priorityQueue, + STATS_UPDATE_PERIOD_MS); + EventPeriodicQueueCreate(&ev, + radioHandle.priorityQueue, + STATS_UPDATE_PERIOD_MS); +#else /* PIOS_TELEM_PRIORITY_QUEUE */ + EventPeriodicQueueCreate(&ev, + telemHandle.mainQueue, + STATS_UPDATE_PERIOD_MS); + EventPeriodicQueueCreate(&ev, + radioHandle.mainQueue, + STATS_UPDATE_PERIOD_MS); +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ return 0; } @@ -188,14 +281,39 @@ MODULE_INITCALL(TelemetryInitialize, TelemetryStart); * telemetry settings. * \param[in] obj Object to connect */ -static void registerObject(UAVObjHandle obj) +static void registerTelemObject(UAVObjHandle obj) { if (UAVObjIsMetaobject(obj)) { // Only connect change notifications for meta objects. No periodic updates - UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES); +#ifdef PIOS_TELEM_PRIORITY_QUEUE + UAVObjConnectQueue(obj, telemHandle.priorityQueue, EV_MASK_ALL_UPDATES); +#else /* PIOS_TELEM_PRIORITY_QUEUE */ + UAVObjConnectQueue(obj, telemHandle.mainQueue, EV_MASK_ALL_UPDATES); +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ } else { // Setup object for periodic updates - updateObject(obj, EV_NONE); + updateObject( + &telemHandle, + obj, + EV_NONE); + } +} + +static void registerRadioObject(UAVObjHandle obj) +{ + if (UAVObjIsMetaobject(obj)) { + // Only connect change notifications for meta objects. No periodic updates +#ifdef PIOS_TELEM_PRIORITY_QUEUE + UAVObjConnectQueue(obj, radioHandle.priorityQueue, EV_MASK_ALL_UPDATES); +#else /* PIOS_TELEM_PRIORITY_QUEUE */ + UAVObjConnectQueue(obj, radioHandle.mainQueue, EV_MASK_ALL_UPDATES); +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ + } else { + // Setup object for periodic updates + updateObject( + &radioHandle, + obj, + EV_NONE); } } @@ -203,7 +321,10 @@ static void registerObject(UAVObjHandle obj) * Update object's queue connections and timer, depending on object's settings * \param[in] obj Object to updates */ -static void updateObject(UAVObjHandle obj, int32_t eventType) +static void updateObject( + telemetryContext *telemetryHandle, + UAVObjHandle obj, + int32_t eventType) { UAVObjMetadata metadata; UAVObjUpdateMode updateMode, loggingMode; @@ -226,13 +347,15 @@ static void updateObject(UAVObjHandle obj, int32_t eventType) switch (updateMode) { case UPDATEMODE_PERIODIC: // Set update period - setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); + setUpdatePeriod(telemetryHandle, + obj, + metadata.telemetryUpdatePeriod); // Connect queue eventMask |= EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ; break; case UPDATEMODE_ONCHANGE: // Set update period - setUpdatePeriod(obj, 0); + setUpdatePeriod(telemetryHandle, obj, 0); // Connect queue eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; break; @@ -242,7 +365,9 @@ static void updateObject(UAVObjHandle obj, int32_t eventType) eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; // Set update period on initialization and metadata change if (eventType == EV_NONE) { - setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); + setUpdatePeriod(telemetryHandle, + obj, + metadata.telemetryUpdatePeriod); } } else { // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates @@ -251,7 +376,7 @@ static void updateObject(UAVObjHandle obj, int32_t eventType) break; case UPDATEMODE_MANUAL: // Set update period - setUpdatePeriod(obj, 0); + setUpdatePeriod(telemetryHandle, obj, 0); // Connect queue eventMask |= EV_UPDATED_MANUAL | EV_UPDATE_REQ; break; @@ -259,13 +384,13 @@ static void updateObject(UAVObjHandle obj, int32_t eventType) switch (loggingMode) { case UPDATEMODE_PERIODIC: // Set update period - setLoggingPeriod(obj, metadata.loggingUpdatePeriod); + setLoggingPeriod(telemetryHandle, obj, metadata.loggingUpdatePeriod); // Connect queue eventMask |= EV_LOGGING_PERIODIC | EV_LOGGING_MANUAL; break; case UPDATEMODE_ONCHANGE: // Set update period - setLoggingPeriod(obj, 0); + setLoggingPeriod(telemetryHandle, obj, 0); // Connect queue eventMask |= EV_UPDATED | EV_LOGGING_MANUAL; break; @@ -275,7 +400,9 @@ static void updateObject(UAVObjHandle obj, int32_t eventType) eventMask |= EV_UPDATED | EV_LOGGING_MANUAL; // Set update period on initialization and metadata change if (eventType == EV_NONE) { - setLoggingPeriod(obj, metadata.loggingUpdatePeriod); + setLoggingPeriod(telemetryHandle, + obj, + metadata.loggingUpdatePeriod); } } else { // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates @@ -284,23 +411,28 @@ static void updateObject(UAVObjHandle obj, int32_t eventType) break; case UPDATEMODE_MANUAL: // Set update period - setLoggingPeriod(obj, 0); + setLoggingPeriod(telemetryHandle, obj, 0); // Connect queue eventMask |= EV_LOGGING_MANUAL; break; } + // note that all setting objects have implicitly IsPriority=true +#ifdef PIOS_TELEM_PRIORITY_QUEUE if (UAVObjIsPriority(obj)) { - UAVObjConnectQueue(obj, priorityQueue, eventMask); - } else { - UAVObjConnectQueue(obj, queue, eventMask); - } + UAVObjConnectQueue(obj, telemetryHandle->priorityQueue, eventMask); + } else +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ + UAVObjConnectQueue(obj, telemetryHandle->mainQueue, eventMask); } + /** * Processes queue events */ -static void processObjEvent(UAVObjEvent *ev) +static void processObjEvent( + telemetryContext *telemetryHandle, + UAVObjEvent *ev) { UAVObjMetadata metadata; UAVObjUpdateMode updateMode; @@ -325,7 +457,10 @@ static void processObjEvent(UAVObjEvent *ev) // Send update to GCS (with retries) while (retries < MAX_RETRIES && success == -1) { // call blocks until ack is received or timeout - success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); + success = UAVTalkSendObject(telemetryHandle->uavTalkCon, + ev->obj, + ev->instId, + UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); if (success == -1) { ++retries; } @@ -339,7 +474,10 @@ static void processObjEvent(UAVObjEvent *ev) // Request object update from GCS (with retries) while (retries < MAX_RETRIES && success == -1) { // call blocks until update is received or timeout - success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); + success = UAVTalkSendObjectRequest(telemetryHandle->uavTalkCon, + ev->obj, + ev->instId, + REQ_TIMEOUT_MS); if (success == -1) { ++retries; } @@ -353,11 +491,17 @@ static void processObjEvent(UAVObjEvent *ev) // If this is a metaobject then make necessary telemetry updates if (UAVObjIsMetaobject(ev->obj)) { // linked object will be the actual object the metadata are for - updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); + updateObject( + telemetryHandle, + UAVObjGetLinkedObj(ev->obj), + EV_NONE); } else { if (updateMode == UPDATEMODE_THROTTLED) { // If this is UPDATEMODE_THROTTLED, the event mask changes on every event. - updateObject(ev->obj, ev->event); + updateObject( + telemetryHandle, + ev->obj, + ev->event); } } } @@ -378,7 +522,10 @@ static void processObjEvent(UAVObjEvent *ev) } if (updateMode == UPDATEMODE_THROTTLED) { // If this is UPDATEMODE_THROTTLED, the event mask changes on every event. - updateObject(ev->obj, ev->event); + updateObject( + telemetryHandle, + ev->obj, + ev->event); } } } @@ -386,37 +533,43 @@ static void processObjEvent(UAVObjEvent *ev) /** * Telemetry transmit task, regular priority */ -static void telemetryTxTask(__attribute__((unused)) void *parameters) +static void telemetryTxTask(void *parameters) { + telemetryContext *telemetryHandle = (telemetryContext *)parameters; UAVObjEvent ev; + /* Check for a bad context */ + if (!telemetryHandle) { + return; + } + // Loop forever while (1) { /** * Tries to empty the high priority queue before handling any standard priority item */ -#if defined(PIOS_TELEM_PRIORITY_QUEUE) +#ifdef PIOS_TELEM_PRIORITY_QUEUE // empty priority queue, non-blocking - while (xQueueReceive(priorityQueue, &ev, 0) == pdTRUE) { + while (xQueueReceive(telemetryHandle->priorityQueue, &ev, 0) == pdTRUE) { // Process event - processObjEvent(&ev); + processObjEvent(telemetryHandle, &ev); } // check regular queue and process update - non-blocking - if (xQueueReceive(queue, &ev, 0) == pdTRUE) { + if (xQueueReceive(telemetryHandle->mainQueue, &ev, 0) == pdTRUE) { // Process event - processObjEvent(&ev); + processObjEvent(telemetryHandle, &ev); // if both queues are empty, wait on priority queue for updates (1 tick) then repeat cycle - } else if (xQueueReceive(priorityQueue, &ev, 1) == pdTRUE) { + } else if (xQueueReceive(telemetryHandle->priorityQueue, &ev, 1) == pdTRUE) { // Process event - processObjEvent(&ev); + processObjEvent(telemetryHandle, &ev); } #else // wait on queue for updates (1 tick) then repeat cycle - if (xQueueReceive(queue, &ev, 1) == pdTRUE) { + if (xQueueReceive(telemetryHandle->mainQueue, &ev, 1) == pdTRUE) { // Process event - processObjEvent(&ev); + processObjEvent(telemetryHandle, &ev); } -#endif /* if defined(PIOS_TELEM_PRIORITY_QUEUE) */ +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ } } @@ -424,11 +577,18 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) /** * Telemetry receive task. Processes queue events and periodic updates. */ -static void telemetryRxTask(__attribute__((unused)) void *parameters) +static void telemetryRxTask(void *parameters) { + telemetryContext *telemetryHandle = (telemetryContext *)parameters; + + /* Check for a bad context */ + if (!telemetryHandle) { + return; + } + // Task loop while (1) { - uint32_t inputPort = getComPort(true); + uint32_t inputPort = telemetryHandle->telemetryPort; if (inputPort) { // Block until data are available @@ -438,7 +598,7 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500); if (bytes_to_process > 0) { for (uint8_t i = 0; i < bytes_to_process; i++) { - UAVTalkProcessInputStream(uavTalkCon, serial_data[i]); + UAVTalkProcessInputStream(telemetryHandle->uavTalkCon, serial_data[i]); } } } else { @@ -447,46 +607,6 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) } } -#ifdef PIOS_INCLUDE_RFM22B -/** - * Radio telemetry receive task. Processes queue events and periodic updates. - */ -static void radioRxTask(__attribute__((unused)) void *parameters) -{ - // Task loop - while (1) { - if (radioPort) { - // Block until data are available - uint8_t serial_data[1]; - uint16_t bytes_to_process; - - bytes_to_process = PIOS_COM_ReceiveBuffer(radioPort, serial_data, sizeof(serial_data), 500); - if (bytes_to_process > 0) { - for (uint8_t i = 0; i < bytes_to_process; i++) { - UAVTalkProcessInputStream(radioUavTalkCon, serial_data[i]); - } - } - } else { - vTaskDelay(5); - } - } -} -/** - * Transmit data buffer to the radioport. - * \param[in] data Data buffer to send - * \param[in] length Length of buffer - * \return -1 on failure - * \return number of bytes transmitted on success - */ -static int32_t transmitRadioData(uint8_t *data, int32_t length) -{ - if (radioPort) { - return PIOS_COM_SendBuffer(radioPort, data, length); - } - - return -1; -} -#endif /* PIOS_INCLUDE_RFM22B */ /** * Transmit data buffer to the modem or USB port. @@ -497,7 +617,44 @@ static int32_t transmitRadioData(uint8_t *data, int32_t length) */ static int32_t transmitData(uint8_t *data, int32_t length) { - uint32_t outputPort = getComPort(false); + uint32_t outputPort; + + outputPort = PIOS_COM_TELEM_RF; + + // Anticipate next input on the port on which this output occurs + telemHandle.telemetryPort = outputPort; + + if (outputPort) { + return PIOS_COM_SendBuffer(outputPort, data, length); + } + + return -1; +} + + +/** + * Transmit data buffer to the radioport. + * \param[in] data Data buffer to send + * \param[in] length Length of buffer + * \return -1 on failure + * \return number of bytes transmitted on success + */ +static int32_t transmitRadioData(uint8_t *data, int32_t length) +{ +#ifdef PIOS_INCLUDE_RFM22B + uint32_t outputPort = PIOS_COM_RF; +#else /* PIOS_INCLUDE_RFM22B */ + uint32_t outputPort = 0; +#endif /* PIOS_INCLUDE_RFM22B */ +#ifdef PIOS_INCLUDE_USB + // if USB is connected, USB takes precedence for telemetry + if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) { + outputPort = PIOS_COM_TELEM_USB; + } +#endif /* PIOS_INCLUDE_USB */ + + // Anticipate next input on the port on which this output occurs + radioHandle.telemetryPort = outputPort; if (outputPort) { return PIOS_COM_SendBuffer(outputPort, data, length); @@ -513,7 +670,10 @@ static int32_t transmitData(uint8_t *data, int32_t length) * \return 0 Success * \return -1 Failure */ -static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs) +static int32_t setUpdatePeriod( + telemetryContext *telemetryHandle, + UAVObjHandle obj, + int32_t updatePeriodMs) { UAVObjEvent ev; int32_t ret; @@ -524,7 +684,12 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs) ev.event = EV_UPDATED_PERIODIC; ev.lowPriority = true; - xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue; +#ifdef PIOS_TELEM_PRIORITY_QUEUE + xQueueHandle targetQueue = UAVObjIsPriority(obj) ? telemetryHandle->priorityQueue : + telemetryHandle->mainQueue; +#else /* PIOS_TELEM_PRIORITY_QUEUE */ + xQueueHandle targetQueue = telemetryHandle->mainQueue; +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs); if (ret == -1) { @@ -540,7 +705,10 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs) * \return 0 Success * \return -1 Failure */ -static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs) +static int32_t setLoggingPeriod( + telemetryContext *telemetryHandle, + UAVObjHandle obj, + int32_t updatePeriodMs) { UAVObjEvent ev; int32_t ret; @@ -551,7 +719,12 @@ static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs) ev.event = EV_LOGGING_PERIODIC; ev.lowPriority = true; - xQueueHandle targetQueue = UAVObjIsPriority(obj) ? priorityQueue : queue; +#ifdef PIOS_TELEM_PRIORITY_QUEUE + xQueueHandle targetQueue = UAVObjIsPriority(obj) ? telemetryHandle->priorityQueue : + telemetryHandle->mainQueue; +#else /* PIOS_TELEM_PRIORITY_QUEUE */ + xQueueHandle targetQueue = telemetryHandle->mainQueue; +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs); if (ret == -1) { @@ -590,10 +763,8 @@ static void updateTelemetryStats() uint32_t timeNow; // Get stats - UAVTalkGetStats(uavTalkCon, &utalkStats, true); -#ifdef PIOS_INCLUDE_RFM22B - UAVTalkAddStats(radioUavTalkCon, &utalkStats, true); -#endif + UAVTalkGetStats(telemHandle.uavTalkCon, &utalkStats, true); + UAVTalkAddStats(radioHandle.uavTalkCon, &utalkStats, true); // Get object data FlightTelemetryStatsGet(&flightStats); @@ -685,7 +856,7 @@ static void updateTelemetryStats() */ static void updateSettings() { - if (telemetryPort) { + if (telemHandle.telemetryPort) { // Retrieve settings uint8_t speed; HwSettingsTelemetrySpeedGet(&speed); @@ -693,58 +864,30 @@ static void updateSettings() // Set port speed switch (speed) { case HWSETTINGS_TELEMETRYSPEED_2400: - PIOS_COM_ChangeBaud(telemetryPort, 2400); + PIOS_COM_ChangeBaud(telemHandle.telemetryPort, 2400); break; case HWSETTINGS_TELEMETRYSPEED_4800: - PIOS_COM_ChangeBaud(telemetryPort, 4800); + PIOS_COM_ChangeBaud(telemHandle.telemetryPort, 4800); break; case HWSETTINGS_TELEMETRYSPEED_9600: - PIOS_COM_ChangeBaud(telemetryPort, 9600); + PIOS_COM_ChangeBaud(telemHandle.telemetryPort, 9600); break; case HWSETTINGS_TELEMETRYSPEED_19200: - PIOS_COM_ChangeBaud(telemetryPort, 19200); + PIOS_COM_ChangeBaud(telemHandle.telemetryPort, 19200); break; case HWSETTINGS_TELEMETRYSPEED_38400: - PIOS_COM_ChangeBaud(telemetryPort, 38400); + PIOS_COM_ChangeBaud(telemHandle.telemetryPort, 38400); break; case HWSETTINGS_TELEMETRYSPEED_57600: - PIOS_COM_ChangeBaud(telemetryPort, 57600); + PIOS_COM_ChangeBaud(telemHandle.telemetryPort, 57600); break; case HWSETTINGS_TELEMETRYSPEED_115200: - PIOS_COM_ChangeBaud(telemetryPort, 115200); + PIOS_COM_ChangeBaud(telemHandle.telemetryPort, 115200); break; } } } -/** - * Determine input/output com port as highest priority available - * @param[in] input Returns the approproate input com port if true, else the appropriate output com port - */ -#ifdef PIOS_INCLUDE_RFM22B -static uint32_t getComPort(bool input) -#else -static uint32_t getComPort(__attribute__((unused)) bool input) -#endif -{ -#if defined(PIOS_INCLUDE_USB) - // if USB is connected, USB takes precedence for telemetry - if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) { - return PIOS_COM_TELEM_USB; - } else -#endif /* PIOS_INCLUDE_USB */ -#ifdef PIOS_INCLUDE_RFM22B - // PIOS_COM_RF input is handled by a separate RX thread and therefore must be ignored - if (input && telemetryPort == PIOS_COM_RF) { - return 0; - } else -#endif /* PIOS_INCLUDE_RFM22B */ - if (PIOS_COM_Available(telemetryPort)) { - return telemetryPort; - } else { - return 0; - } -} /** * @} diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index da954a045..35f218a75 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -740,10 +740,7 @@ void PIOS_Board_Init(void) tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { PIOS_Assert(0); } - /* Set Telemetry to use OPLinkMini if no other telemetry is configured (USB always overrides anyway) */ - if (!pios_com_telem_rf_id) { - pios_com_telem_rf_id = pios_com_rf_id; - } + oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED; // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex. From 5952cfa27574cef575b88c4cb04dcc7fa76e8d59 Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Sun, 15 Mar 2015 00:48:36 +0000 Subject: [PATCH 2/8] Year is 2015 --- flight/modules/Telemetry/telemetry.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index c5c2350e7..3ef5bbecd 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -9,7 +9,7 @@ * @{ * * @file telemetry.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. * @brief Telemetry module, handles telemetry and UAVObject updates * @see The GNU Public License (GPL) Version 3 * From 12d3bdbe8ada6e7e231c3c1db0097bcc89304a78 Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Sun, 15 Mar 2015 18:19:19 +0000 Subject: [PATCH 3/8] OP-1289 add per channel port selection routines +review OPReview-985 --- flight/modules/Telemetry/telemetry.c | 114 +++++++++++++++------------ 1 file changed, 62 insertions(+), 52 deletions(-) diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 3ef5bbecd..83be2e424 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -93,14 +93,14 @@ // Private types typedef struct { - // Port on which to communicate telemetry information - uint32_t telemetryPort; + // Determine port on which to communicate telemetry information + uint32_t (*telemetryPort)(); // Main telemetry queue xQueueHandle mainQueue; - #ifdef PIOS_TELEM_PRIORITY_QUEUE +#ifdef PIOS_TELEM_PRIORITY_QUEUE // Priority telemetry queue xQueueHandle priorityQueue; - #endif /* PIOS_TELEM_PRIORITY_QUEUE */ +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ // Transmit/receive task handles xTaskHandle telemetryTxTaskHandle; xTaskHandle telemetryRxTaskHandle; @@ -108,15 +108,17 @@ typedef struct { UAVTalkConnection uavTalkCon; } telemetryContext; -// Main telemetry port +// Main telemetry channel static telemetryContext telemHandle; static int32_t transmitData(uint8_t *data, int32_t length); static void registerTelemObject(UAVObjHandle obj); +static uint32_t telemPort(); -// OPLink telemetry port +// OPLink telemetry channel static telemetryContext radioHandle; static int32_t transmitRadioData(uint8_t *data, int32_t length); static void registerRadioObject(UAVObjHandle obj); +static uint32_t radioPort(); // Telemetry stats static uint32_t txErrors; @@ -142,7 +144,7 @@ static int32_t setLoggingPeriod( int32_t updatePeriodMs); static void updateTelemetryStats(); static void gcsTelemetryStatsUpdated(); -static void updateSettings(); +static void updateSettings(telemetryContext *telemetryHandle); /** * Initialise the telemetry module @@ -226,23 +228,12 @@ int32_t TelemetryInitialize(void) sizeof(UAVObjEvent)); #endif /* PIOS_TELEM_PRIORITY_QUEUE */ - // Update telemetry settings - telemHandle.telemetryPort = PIOS_COM_TELEM_RF; - -#ifdef PIOS_INCLUDE_RFM22B - radioHandle.telemetryPort = PIOS_COM_RF; -#else /* PIOS_INCLUDE_RFM22B */ - radioHandle.telemetryPort = 0; -#endif /* PIOS_INCLUDE_RFM22B */ -#ifdef PIOS_INCLUDE_USB - // if USB is connected, USB takes precedence for telemetry - if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) { - radioHandle.telemetryPort = PIOS_COM_TELEM_USB; - } -#endif /* PIOS_INCLUDE_USB */ + // Set channel port handlers + telemHandle.telemetryPort = telemPort; + radioHandle.telemetryPort = radioPort; HwSettingsInitialize(); - updateSettings(); + updateSettings(&telemHandle); // Initialise UAVTalk telemHandle.uavTalkCon = UAVTalkInitialize(&transmitData); @@ -319,6 +310,7 @@ static void registerRadioObject(UAVObjHandle obj) /** * Update object's queue connections and timer, depending on object's settings + * \param[in] telemetry channel context * \param[in] obj Object to updates */ static void updateObject( @@ -588,7 +580,7 @@ static void telemetryRxTask(void *parameters) // Task loop while (1) { - uint32_t inputPort = telemetryHandle->telemetryPort; + uint32_t inputPort = telemetryHandle->telemetryPort(); if (inputPort) { // Block until data are available @@ -608,6 +600,38 @@ static void telemetryRxTask(void *parameters) } +/** + * Determine the port to be used for communication on the telemetry channel + * \return com port number + */ +static uint32_t telemPort() +{ + return PIOS_COM_TELEM_RF; +} + + +/** + * Determine the port to be used for communication on the radio channel + * \return com port number + */ +static uint32_t radioPort() +{ +#ifdef PIOS_INCLUDE_RFM22B + uint32_t port = PIOS_COM_RF; +#else /* PIOS_INCLUDE_RFM22B */ + uint32_t port = 0; +#endif /* PIOS_INCLUDE_RFM22B */ +#ifdef PIOS_INCLUDE_USB + // if USB is connected, USB takes precedence for telemetry + if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) { + port = PIOS_COM_TELEM_USB; + } +#endif /* PIOS_INCLUDE_USB */ + + return port; +} + + /** * Transmit data buffer to the modem or USB port. * \param[in] data Data buffer to send @@ -617,12 +641,7 @@ static void telemetryRxTask(void *parameters) */ static int32_t transmitData(uint8_t *data, int32_t length) { - uint32_t outputPort; - - outputPort = PIOS_COM_TELEM_RF; - - // Anticipate next input on the port on which this output occurs - telemHandle.telemetryPort = outputPort; + uint32_t outputPort = telemHandle.telemetryPort(); if (outputPort) { return PIOS_COM_SendBuffer(outputPort, data, length); @@ -641,20 +660,7 @@ static int32_t transmitData(uint8_t *data, int32_t length) */ static int32_t transmitRadioData(uint8_t *data, int32_t length) { -#ifdef PIOS_INCLUDE_RFM22B - uint32_t outputPort = PIOS_COM_RF; -#else /* PIOS_INCLUDE_RFM22B */ - uint32_t outputPort = 0; -#endif /* PIOS_INCLUDE_RFM22B */ -#ifdef PIOS_INCLUDE_USB - // if USB is connected, USB takes precedence for telemetry - if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) { - outputPort = PIOS_COM_TELEM_USB; - } -#endif /* PIOS_INCLUDE_USB */ - - // Anticipate next input on the port on which this output occurs - radioHandle.telemetryPort = outputPort; + uint32_t outputPort = radioHandle.telemetryPort(); if (outputPort) { return PIOS_COM_SendBuffer(outputPort, data, length); @@ -665,6 +671,7 @@ static int32_t transmitRadioData(uint8_t *data, int32_t length) /** * Set update period of object (it must be already setup for periodic updates) + * \param[in] telemetry channel context * \param[in] obj The object to update * \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled * \return 0 Success @@ -700,6 +707,7 @@ static int32_t setUpdatePeriod( /** * Set logging update period of object (it must be already setup for periodic updates) + * \param[in] telemetry channel context * \param[in] obj The object to update * \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled * \return 0 Success @@ -854,9 +862,11 @@ static void updateTelemetryStats() * settings, etc. Thus the HwSettings object which contains the * telemetry port speed is used for now. */ -static void updateSettings() +static void updateSettings(telemetryContext *telemetryHandle) { - if (telemHandle.telemetryPort) { + uint32_t port = telemetryHandle->telemetryPort(); + + if (port) { // Retrieve settings uint8_t speed; HwSettingsTelemetrySpeedGet(&speed); @@ -864,25 +874,25 @@ static void updateSettings() // Set port speed switch (speed) { case HWSETTINGS_TELEMETRYSPEED_2400: - PIOS_COM_ChangeBaud(telemHandle.telemetryPort, 2400); + PIOS_COM_ChangeBaud(port, 2400); break; case HWSETTINGS_TELEMETRYSPEED_4800: - PIOS_COM_ChangeBaud(telemHandle.telemetryPort, 4800); + PIOS_COM_ChangeBaud(port, 4800); break; case HWSETTINGS_TELEMETRYSPEED_9600: - PIOS_COM_ChangeBaud(telemHandle.telemetryPort, 9600); + PIOS_COM_ChangeBaud(port, 9600); break; case HWSETTINGS_TELEMETRYSPEED_19200: - PIOS_COM_ChangeBaud(telemHandle.telemetryPort, 19200); + PIOS_COM_ChangeBaud(port, 19200); break; case HWSETTINGS_TELEMETRYSPEED_38400: - PIOS_COM_ChangeBaud(telemHandle.telemetryPort, 38400); + PIOS_COM_ChangeBaud(port, 38400); break; case HWSETTINGS_TELEMETRYSPEED_57600: - PIOS_COM_ChangeBaud(telemHandle.telemetryPort, 57600); + PIOS_COM_ChangeBaud(port, 57600); break; case HWSETTINGS_TELEMETRYSPEED_115200: - PIOS_COM_ChangeBaud(telemHandle.telemetryPort, 115200); + PIOS_COM_ChangeBaud(port, 115200); break; } } From 45005a167f4f253cbb07ce0f210bbe786baa358d Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Sun, 15 Mar 2015 23:11:42 +0000 Subject: [PATCH 4/8] OP-1289 Revise naming of the two channels of data and associated functions The code now uses a "local" prefix to refer to the telemetry channel associated with a port on the FC and a "radio" prefix to refer to the telemetry channel associated with the OPLink/USB. +review OPReview-985 --- flight/modules/Telemetry/telemetry.c | 218 ++++++++++++++------------- 1 file changed, 111 insertions(+), 107 deletions(-) diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 83be2e424..6f6a4e0d9 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -35,11 +35,15 @@ * radio connection, called "RadioTx" and "Radio Rx", the latter being * overridden by USB if connected. * - * The telemetry port to use is defined by PIOS_COM_TELEM_RF in + * The following code uses a "local" prefix to refer to the telemetry channel + * associated with a port on the FC and a "radio" prefix to refer to the + * telemetry channel associated with the OPLink/USB. + * + * The "local" telemetry port to use is defined by PIOS_COM_TELEM_RF in * PIOS_Board_Init(). * - * A UAVTalk connection instance, telemUavTalkCon, is associated with the main - * telemetry channel and another, radioUavTalkCon, with the radio channel. + * A UAVTalk connection instance, telemUavTalkCon, is associated with the + * "local" channel and another, radioUavTalkCon, with the "radio" channel. * Associated with each instance is a transmit routine which will send data * to the appropriate port. * @@ -51,7 +55,7 @@ * the normal queue, passing each event to processObjEvent() which ultimately * passes each event to the UAVTalk library which results in the appropriate * transmit routine being called to send the data back to the recipient on - * the telemetry or radio link. + * the "local" or "radio" link. */ #include @@ -94,7 +98,7 @@ // Private types typedef struct { // Determine port on which to communicate telemetry information - uint32_t (*telemetryPort)(); + uint32_t (*getPort)(); // Main telemetry queue xQueueHandle mainQueue; #ifdef PIOS_TELEM_PRIORITY_QUEUE @@ -102,20 +106,20 @@ typedef struct { xQueueHandle priorityQueue; #endif /* PIOS_TELEM_PRIORITY_QUEUE */ // Transmit/receive task handles - xTaskHandle telemetryTxTaskHandle; - xTaskHandle telemetryRxTaskHandle; + xTaskHandle txTaskHandle; + xTaskHandle rxTaskHandle; // Telemetry stream UAVTalkConnection uavTalkCon; -} telemetryContext; +} channelContext; // Main telemetry channel -static telemetryContext telemHandle; -static int32_t transmitData(uint8_t *data, int32_t length); -static void registerTelemObject(UAVObjHandle obj); -static uint32_t telemPort(); +static channelContext localChannel; +static int32_t transmitLocalData(uint8_t *data, int32_t length); +static void registerLocalObject(UAVObjHandle obj); +static uint32_t localPort(); // OPLink telemetry channel -static telemetryContext radioHandle; +static channelContext radioChannel; static int32_t transmitRadioData(uint8_t *data, int32_t length); static void registerRadioObject(UAVObjHandle obj); static uint32_t radioPort(); @@ -128,23 +132,23 @@ static uint32_t timeOfLastObjectUpdate; static void telemetryTxTask(void *parameters); static void telemetryRxTask(void *parameters); static void updateObject( - telemetryContext *telemetryHandle, + channelContext *channel, UAVObjHandle obj, int32_t eventType); static void processObjEvent( - telemetryContext *telemetryHandle, + channelContext *channel, UAVObjEvent *ev); static int32_t setUpdatePeriod( - telemetryContext *telemetryHandle, + channelContext *channel, UAVObjHandle obj, int32_t updatePeriodMs); static int32_t setLoggingPeriod( - telemetryContext *telemetryHandle, + channelContext *channel, UAVObjHandle obj, int32_t updatePeriodMs); static void updateTelemetryStats(); static void gcsTelemetryStatsUpdated(); -static void updateSettings(telemetryContext *telemetryHandle); +static void updateSettings(channelContext *channel); /** * Initialise the telemetry module @@ -153,52 +157,52 @@ static void updateSettings(telemetryContext *telemetryHandle); */ int32_t TelemetryStart(void) { - UAVObjIterate(®isterTelemObject); + UAVObjIterate(®isterLocalObject); UAVObjIterate(®isterRadioObject); // Listen to objects of interest #ifdef PIOS_TELEM_PRIORITY_QUEUE - GCSTelemetryStatsConnectQueue(telemHandle.priorityQueue); - GCSTelemetryStatsConnectQueue(radioHandle.priorityQueue); + GCSTelemetryStatsConnectQueue(localChannel.priorityQueue); + GCSTelemetryStatsConnectQueue(radioChannel.priorityQueue); #else /* PIOS_TELEM_PRIORITY_QUEUE */ - GCSTelemetryStatsConnectQueue(telemHandle.mainQueue); - GCSTelemetryStatsConnectQueue(radioHandle.mainQueue); + GCSTelemetryStatsConnectQueue(localChannel.mainQueue); + GCSTelemetryStatsConnectQueue(radioChannel.mainQueue); #endif /* PIOS_TELEM_PRIORITY_QUEUE */ // Start telemetry tasks xTaskCreate(telemetryTxTask, "TelTx", STACK_SIZE_TX_BYTES / 4, - &telemHandle, + &localChannel, TASK_PRIORITY_TX, - &telemHandle.telemetryTxTaskHandle); + &localChannel.txTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, - telemHandle.telemetryTxTaskHandle); + localChannel.txTaskHandle); xTaskCreate(telemetryRxTask, "TelRx", STACK_SIZE_RX_BYTES / 4, - &telemHandle, + &localChannel, TASK_PRIORITY_RX, - &telemHandle.telemetryRxTaskHandle); + &localChannel.rxTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, - telemHandle.telemetryRxTaskHandle); + localChannel.rxTaskHandle); xTaskCreate(telemetryTxTask, "RadioTx", STACK_SIZE_RADIO_TX_BYTES / 4, - &radioHandle, + &radioChannel, TASK_PRIORITY_RADTX, - &radioHandle.telemetryTxTaskHandle); + &radioChannel.txTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, - radioHandle.telemetryTxTaskHandle); + radioChannel.txTaskHandle); xTaskCreate(telemetryRxTask, "RadioRx", STACK_SIZE_RADIO_RX_BYTES / 4, - &radioHandle, + &radioChannel, TASK_PRIORITY_RADRX, - &radioHandle.telemetryRxTaskHandle); + &radioChannel.rxTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, - radioHandle.telemetryRxTaskHandle); + radioChannel.rxTaskHandle); return 0; } @@ -214,30 +218,30 @@ int32_t TelemetryInitialize(void) GCSTelemetryStatsInitialize(); // Initialize vars - timeOfLastObjectUpdate = 0; + timeOfLastObjectUpdate = 0; // Create object queues - telemHandle.mainQueue = xQueueCreate(MAX_QUEUE_SIZE, - sizeof(UAVObjEvent)); - radioHandle.mainQueue = xQueueCreate(MAX_QUEUE_SIZE, - sizeof(UAVObjEvent)); + localChannel.mainQueue = xQueueCreate(MAX_QUEUE_SIZE, + sizeof(UAVObjEvent)); + radioChannel.mainQueue = xQueueCreate(MAX_QUEUE_SIZE, + sizeof(UAVObjEvent)); #if defined(PIOS_TELEM_PRIORITY_QUEUE) - telemHandle.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, - sizeof(UAVObjEvent)); - radioHandle.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, - sizeof(UAVObjEvent)); + localChannel.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, + sizeof(UAVObjEvent)); + radioChannel.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, + sizeof(UAVObjEvent)); #endif /* PIOS_TELEM_PRIORITY_QUEUE */ // Set channel port handlers - telemHandle.telemetryPort = telemPort; - radioHandle.telemetryPort = radioPort; + localChannel.getPort = localPort; + radioChannel.getPort = radioPort; HwSettingsInitialize(); - updateSettings(&telemHandle); + updateSettings(&localChannel); // Initialise UAVTalk - telemHandle.uavTalkCon = UAVTalkInitialize(&transmitData); - radioHandle.uavTalkCon = UAVTalkInitialize(&transmitRadioData); + localChannel.uavTalkCon = UAVTalkInitialize(&transmitLocalData); + radioChannel.uavTalkCon = UAVTalkInitialize(&transmitRadioData); // Create periodic event that will be used to update the telemetry stats // FIXME STATS_UPDATE_PERIOD_MS is 4000ms while FlighTelemetryStats update period is 5000ms... @@ -248,17 +252,17 @@ int32_t TelemetryInitialize(void) #ifdef PIOS_TELEM_PRIORITY_QUEUE EventPeriodicQueueCreate(&ev, - telemHandle.priorityQueue, + localChannel.priorityQueue, STATS_UPDATE_PERIOD_MS); EventPeriodicQueueCreate(&ev, - radioHandle.priorityQueue, + radioChannel.priorityQueue, STATS_UPDATE_PERIOD_MS); #else /* PIOS_TELEM_PRIORITY_QUEUE */ EventPeriodicQueueCreate(&ev, - telemHandle.mainQueue, + localChannel.mainQueue, STATS_UPDATE_PERIOD_MS); EventPeriodicQueueCreate(&ev, - radioHandle.mainQueue, + radioChannel.mainQueue, STATS_UPDATE_PERIOD_MS); #endif /* PIOS_TELEM_PRIORITY_QUEUE */ @@ -272,19 +276,19 @@ MODULE_INITCALL(TelemetryInitialize, TelemetryStart); * telemetry settings. * \param[in] obj Object to connect */ -static void registerTelemObject(UAVObjHandle obj) +static void registerLocalObject(UAVObjHandle obj) { if (UAVObjIsMetaobject(obj)) { // Only connect change notifications for meta objects. No periodic updates #ifdef PIOS_TELEM_PRIORITY_QUEUE - UAVObjConnectQueue(obj, telemHandle.priorityQueue, EV_MASK_ALL_UPDATES); + UAVObjConnectQueue(obj, localChannel.priorityQueue, EV_MASK_ALL_UPDATES); #else /* PIOS_TELEM_PRIORITY_QUEUE */ - UAVObjConnectQueue(obj, telemHandle.mainQueue, EV_MASK_ALL_UPDATES); + UAVObjConnectQueue(obj, localChannel.mainQueue, EV_MASK_ALL_UPDATES); #endif /* PIOS_TELEM_PRIORITY_QUEUE */ } else { // Setup object for periodic updates updateObject( - &telemHandle, + &localChannel, obj, EV_NONE); } @@ -295,14 +299,14 @@ static void registerRadioObject(UAVObjHandle obj) if (UAVObjIsMetaobject(obj)) { // Only connect change notifications for meta objects. No periodic updates #ifdef PIOS_TELEM_PRIORITY_QUEUE - UAVObjConnectQueue(obj, radioHandle.priorityQueue, EV_MASK_ALL_UPDATES); + UAVObjConnectQueue(obj, radioChannel.priorityQueue, EV_MASK_ALL_UPDATES); #else /* PIOS_TELEM_PRIORITY_QUEUE */ - UAVObjConnectQueue(obj, radioHandle.mainQueue, EV_MASK_ALL_UPDATES); + UAVObjConnectQueue(obj, radioChannel.mainQueue, EV_MASK_ALL_UPDATES); #endif /* PIOS_TELEM_PRIORITY_QUEUE */ } else { // Setup object for periodic updates updateObject( - &radioHandle, + &radioChannel, obj, EV_NONE); } @@ -314,7 +318,7 @@ static void registerRadioObject(UAVObjHandle obj) * \param[in] obj Object to updates */ static void updateObject( - telemetryContext *telemetryHandle, + channelContext *channel, UAVObjHandle obj, int32_t eventType) { @@ -339,7 +343,7 @@ static void updateObject( switch (updateMode) { case UPDATEMODE_PERIODIC: // Set update period - setUpdatePeriod(telemetryHandle, + setUpdatePeriod(channel, obj, metadata.telemetryUpdatePeriod); // Connect queue @@ -347,7 +351,7 @@ static void updateObject( break; case UPDATEMODE_ONCHANGE: // Set update period - setUpdatePeriod(telemetryHandle, obj, 0); + setUpdatePeriod(channel, obj, 0); // Connect queue eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; break; @@ -357,7 +361,7 @@ static void updateObject( eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; // Set update period on initialization and metadata change if (eventType == EV_NONE) { - setUpdatePeriod(telemetryHandle, + setUpdatePeriod(channel, obj, metadata.telemetryUpdatePeriod); } @@ -368,7 +372,7 @@ static void updateObject( break; case UPDATEMODE_MANUAL: // Set update period - setUpdatePeriod(telemetryHandle, obj, 0); + setUpdatePeriod(channel, obj, 0); // Connect queue eventMask |= EV_UPDATED_MANUAL | EV_UPDATE_REQ; break; @@ -376,13 +380,13 @@ static void updateObject( switch (loggingMode) { case UPDATEMODE_PERIODIC: // Set update period - setLoggingPeriod(telemetryHandle, obj, metadata.loggingUpdatePeriod); + setLoggingPeriod(channel, obj, metadata.loggingUpdatePeriod); // Connect queue eventMask |= EV_LOGGING_PERIODIC | EV_LOGGING_MANUAL; break; case UPDATEMODE_ONCHANGE: // Set update period - setLoggingPeriod(telemetryHandle, obj, 0); + setLoggingPeriod(channel, obj, 0); // Connect queue eventMask |= EV_UPDATED | EV_LOGGING_MANUAL; break; @@ -392,7 +396,7 @@ static void updateObject( eventMask |= EV_UPDATED | EV_LOGGING_MANUAL; // Set update period on initialization and metadata change if (eventType == EV_NONE) { - setLoggingPeriod(telemetryHandle, + setLoggingPeriod(channel, obj, metadata.loggingUpdatePeriod); } @@ -403,7 +407,7 @@ static void updateObject( break; case UPDATEMODE_MANUAL: // Set update period - setLoggingPeriod(telemetryHandle, obj, 0); + setLoggingPeriod(channel, obj, 0); // Connect queue eventMask |= EV_LOGGING_MANUAL; break; @@ -412,10 +416,10 @@ static void updateObject( // note that all setting objects have implicitly IsPriority=true #ifdef PIOS_TELEM_PRIORITY_QUEUE if (UAVObjIsPriority(obj)) { - UAVObjConnectQueue(obj, telemetryHandle->priorityQueue, eventMask); + UAVObjConnectQueue(obj, channel->priorityQueue, eventMask); } else #endif /* PIOS_TELEM_PRIORITY_QUEUE */ - UAVObjConnectQueue(obj, telemetryHandle->mainQueue, eventMask); + UAVObjConnectQueue(obj, channel->mainQueue, eventMask); } @@ -423,7 +427,7 @@ static void updateObject( * Processes queue events */ static void processObjEvent( - telemetryContext *telemetryHandle, + channelContext *channel, UAVObjEvent *ev) { UAVObjMetadata metadata; @@ -449,7 +453,7 @@ static void processObjEvent( // Send update to GCS (with retries) while (retries < MAX_RETRIES && success == -1) { // call blocks until ack is received or timeout - success = UAVTalkSendObject(telemetryHandle->uavTalkCon, + success = UAVTalkSendObject(channel->uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); @@ -466,7 +470,7 @@ static void processObjEvent( // Request object update from GCS (with retries) while (retries < MAX_RETRIES && success == -1) { // call blocks until update is received or timeout - success = UAVTalkSendObjectRequest(telemetryHandle->uavTalkCon, + success = UAVTalkSendObjectRequest(channel->uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); @@ -484,14 +488,14 @@ static void processObjEvent( if (UAVObjIsMetaobject(ev->obj)) { // linked object will be the actual object the metadata are for updateObject( - telemetryHandle, + channel, UAVObjGetLinkedObj(ev->obj), EV_NONE); } else { if (updateMode == UPDATEMODE_THROTTLED) { // If this is UPDATEMODE_THROTTLED, the event mask changes on every event. updateObject( - telemetryHandle, + channel, ev->obj, ev->event); } @@ -515,7 +519,7 @@ static void processObjEvent( if (updateMode == UPDATEMODE_THROTTLED) { // If this is UPDATEMODE_THROTTLED, the event mask changes on every event. updateObject( - telemetryHandle, + channel, ev->obj, ev->event); } @@ -527,11 +531,11 @@ static void processObjEvent( */ static void telemetryTxTask(void *parameters) { - telemetryContext *telemetryHandle = (telemetryContext *)parameters; + channelContext *channel = (channelContext *)parameters; UAVObjEvent ev; /* Check for a bad context */ - if (!telemetryHandle) { + if (!channel) { return; } @@ -542,24 +546,24 @@ static void telemetryTxTask(void *parameters) */ #ifdef PIOS_TELEM_PRIORITY_QUEUE // empty priority queue, non-blocking - while (xQueueReceive(telemetryHandle->priorityQueue, &ev, 0) == pdTRUE) { + while (xQueueReceive(channel->priorityQueue, &ev, 0) == pdTRUE) { // Process event - processObjEvent(telemetryHandle, &ev); + processObjEvent(channel, &ev); } // check regular queue and process update - non-blocking - if (xQueueReceive(telemetryHandle->mainQueue, &ev, 0) == pdTRUE) { + if (xQueueReceive(channel->mainQueue, &ev, 0) == pdTRUE) { // Process event - processObjEvent(telemetryHandle, &ev); + processObjEvent(channel, &ev); // if both queues are empty, wait on priority queue for updates (1 tick) then repeat cycle - } else if (xQueueReceive(telemetryHandle->priorityQueue, &ev, 1) == pdTRUE) { + } else if (xQueueReceive(channel->priorityQueue, &ev, 1) == pdTRUE) { // Process event - processObjEvent(telemetryHandle, &ev); + processObjEvent(channel, &ev); } #else // wait on queue for updates (1 tick) then repeat cycle - if (xQueueReceive(telemetryHandle->mainQueue, &ev, 1) == pdTRUE) { + if (xQueueReceive(channel->mainQueue, &ev, 1) == pdTRUE) { // Process event - processObjEvent(telemetryHandle, &ev); + processObjEvent(channel, &ev); } #endif /* PIOS_TELEM_PRIORITY_QUEUE */ } @@ -571,16 +575,16 @@ static void telemetryTxTask(void *parameters) */ static void telemetryRxTask(void *parameters) { - telemetryContext *telemetryHandle = (telemetryContext *)parameters; + channelContext *channel = (channelContext *)parameters; /* Check for a bad context */ - if (!telemetryHandle) { + if (!channel) { return; } // Task loop while (1) { - uint32_t inputPort = telemetryHandle->telemetryPort(); + uint32_t inputPort = channel->getPort(); if (inputPort) { // Block until data are available @@ -590,7 +594,7 @@ static void telemetryRxTask(void *parameters) bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500); if (bytes_to_process > 0) { for (uint8_t i = 0; i < bytes_to_process; i++) { - UAVTalkProcessInputStream(telemetryHandle->uavTalkCon, serial_data[i]); + UAVTalkProcessInputStream(channel->uavTalkCon, serial_data[i]); } } } else { @@ -604,7 +608,7 @@ static void telemetryRxTask(void *parameters) * Determine the port to be used for communication on the telemetry channel * \return com port number */ -static uint32_t telemPort() +static uint32_t localPort() { return PIOS_COM_TELEM_RF; } @@ -639,9 +643,9 @@ static uint32_t radioPort() * \return -1 on failure * \return number of bytes transmitted on success */ -static int32_t transmitData(uint8_t *data, int32_t length) +static int32_t transmitLocalData(uint8_t *data, int32_t length) { - uint32_t outputPort = telemHandle.telemetryPort(); + uint32_t outputPort = localChannel.getPort(); if (outputPort) { return PIOS_COM_SendBuffer(outputPort, data, length); @@ -660,7 +664,7 @@ static int32_t transmitData(uint8_t *data, int32_t length) */ static int32_t transmitRadioData(uint8_t *data, int32_t length) { - uint32_t outputPort = radioHandle.telemetryPort(); + uint32_t outputPort = radioChannel.getPort(); if (outputPort) { return PIOS_COM_SendBuffer(outputPort, data, length); @@ -678,7 +682,7 @@ static int32_t transmitRadioData(uint8_t *data, int32_t length) * \return -1 Failure */ static int32_t setUpdatePeriod( - telemetryContext *telemetryHandle, + channelContext *channel, UAVObjHandle obj, int32_t updatePeriodMs) { @@ -692,10 +696,10 @@ static int32_t setUpdatePeriod( ev.lowPriority = true; #ifdef PIOS_TELEM_PRIORITY_QUEUE - xQueueHandle targetQueue = UAVObjIsPriority(obj) ? telemetryHandle->priorityQueue : - telemetryHandle->mainQueue; + xQueueHandle targetQueue = UAVObjIsPriority(obj) ? channel->priorityQueue : + channel->mainQueue; #else /* PIOS_TELEM_PRIORITY_QUEUE */ - xQueueHandle targetQueue = telemetryHandle->mainQueue; + xQueueHandle targetQueue = channel->mainQueue; #endif /* PIOS_TELEM_PRIORITY_QUEUE */ ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs); @@ -714,7 +718,7 @@ static int32_t setUpdatePeriod( * \return -1 Failure */ static int32_t setLoggingPeriod( - telemetryContext *telemetryHandle, + channelContext *channel, UAVObjHandle obj, int32_t updatePeriodMs) { @@ -728,10 +732,10 @@ static int32_t setLoggingPeriod( ev.lowPriority = true; #ifdef PIOS_TELEM_PRIORITY_QUEUE - xQueueHandle targetQueue = UAVObjIsPriority(obj) ? telemetryHandle->priorityQueue : - telemetryHandle->mainQueue; + xQueueHandle targetQueue = UAVObjIsPriority(obj) ? channel->priorityQueue : + channel->mainQueue; #else /* PIOS_TELEM_PRIORITY_QUEUE */ - xQueueHandle targetQueue = telemetryHandle->mainQueue; + xQueueHandle targetQueue = channel->mainQueue; #endif /* PIOS_TELEM_PRIORITY_QUEUE */ ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs); @@ -771,8 +775,8 @@ static void updateTelemetryStats() uint32_t timeNow; // Get stats - UAVTalkGetStats(telemHandle.uavTalkCon, &utalkStats, true); - UAVTalkAddStats(radioHandle.uavTalkCon, &utalkStats, true); + UAVTalkGetStats(localChannel.uavTalkCon, &utalkStats, true); + UAVTalkAddStats(radioChannel.uavTalkCon, &utalkStats, true); // Get object data FlightTelemetryStatsGet(&flightStats); @@ -862,9 +866,9 @@ static void updateTelemetryStats() * settings, etc. Thus the HwSettings object which contains the * telemetry port speed is used for now. */ -static void updateSettings(telemetryContext *telemetryHandle) +static void updateSettings(channelContext *channel) { - uint32_t port = telemetryHandle->telemetryPort(); + uint32_t port = channel->getPort(); if (port) { // Retrieve settings From ba217a3d7d6674579a312a584b28bf5e51234971 Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Tue, 17 Mar 2015 20:52:08 +0000 Subject: [PATCH 5/8] OP-1289 Revise naming of mainQueue to queue As per review comments +review OPReview-985 --- flight/modules/Telemetry/telemetry.c | 32 ++++++++++++++-------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 6f6a4e0d9..ee7e53cda 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -100,7 +100,7 @@ typedef struct { // Determine port on which to communicate telemetry information uint32_t (*getPort)(); // Main telemetry queue - xQueueHandle mainQueue; + xQueueHandle queue; #ifdef PIOS_TELEM_PRIORITY_QUEUE // Priority telemetry queue xQueueHandle priorityQueue; @@ -165,8 +165,8 @@ int32_t TelemetryStart(void) GCSTelemetryStatsConnectQueue(localChannel.priorityQueue); GCSTelemetryStatsConnectQueue(radioChannel.priorityQueue); #else /* PIOS_TELEM_PRIORITY_QUEUE */ - GCSTelemetryStatsConnectQueue(localChannel.mainQueue); - GCSTelemetryStatsConnectQueue(radioChannel.mainQueue); + GCSTelemetryStatsConnectQueue(localChannel.queue); + GCSTelemetryStatsConnectQueue(radioChannel.queue); #endif /* PIOS_TELEM_PRIORITY_QUEUE */ // Start telemetry tasks @@ -221,9 +221,9 @@ int32_t TelemetryInitialize(void) timeOfLastObjectUpdate = 0; // Create object queues - localChannel.mainQueue = xQueueCreate(MAX_QUEUE_SIZE, + localChannel.queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - radioChannel.mainQueue = xQueueCreate(MAX_QUEUE_SIZE, + radioChannel.queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); #if defined(PIOS_TELEM_PRIORITY_QUEUE) localChannel.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, @@ -259,10 +259,10 @@ int32_t TelemetryInitialize(void) STATS_UPDATE_PERIOD_MS); #else /* PIOS_TELEM_PRIORITY_QUEUE */ EventPeriodicQueueCreate(&ev, - localChannel.mainQueue, + localChannel.queue, STATS_UPDATE_PERIOD_MS); EventPeriodicQueueCreate(&ev, - radioChannel.mainQueue, + radioChannel.queue, STATS_UPDATE_PERIOD_MS); #endif /* PIOS_TELEM_PRIORITY_QUEUE */ @@ -283,7 +283,7 @@ static void registerLocalObject(UAVObjHandle obj) #ifdef PIOS_TELEM_PRIORITY_QUEUE UAVObjConnectQueue(obj, localChannel.priorityQueue, EV_MASK_ALL_UPDATES); #else /* PIOS_TELEM_PRIORITY_QUEUE */ - UAVObjConnectQueue(obj, localChannel.mainQueue, EV_MASK_ALL_UPDATES); + UAVObjConnectQueue(obj, localChannel.queue, EV_MASK_ALL_UPDATES); #endif /* PIOS_TELEM_PRIORITY_QUEUE */ } else { // Setup object for periodic updates @@ -301,7 +301,7 @@ static void registerRadioObject(UAVObjHandle obj) #ifdef PIOS_TELEM_PRIORITY_QUEUE UAVObjConnectQueue(obj, radioChannel.priorityQueue, EV_MASK_ALL_UPDATES); #else /* PIOS_TELEM_PRIORITY_QUEUE */ - UAVObjConnectQueue(obj, radioChannel.mainQueue, EV_MASK_ALL_UPDATES); + UAVObjConnectQueue(obj, radioChannel.queue, EV_MASK_ALL_UPDATES); #endif /* PIOS_TELEM_PRIORITY_QUEUE */ } else { // Setup object for periodic updates @@ -419,7 +419,7 @@ static void updateObject( UAVObjConnectQueue(obj, channel->priorityQueue, eventMask); } else #endif /* PIOS_TELEM_PRIORITY_QUEUE */ - UAVObjConnectQueue(obj, channel->mainQueue, eventMask); + UAVObjConnectQueue(obj, channel->queue, eventMask); } @@ -551,7 +551,7 @@ static void telemetryTxTask(void *parameters) processObjEvent(channel, &ev); } // check regular queue and process update - non-blocking - if (xQueueReceive(channel->mainQueue, &ev, 0) == pdTRUE) { + if (xQueueReceive(channel->queue, &ev, 0) == pdTRUE) { // Process event processObjEvent(channel, &ev); // if both queues are empty, wait on priority queue for updates (1 tick) then repeat cycle @@ -561,7 +561,7 @@ static void telemetryTxTask(void *parameters) } #else // wait on queue for updates (1 tick) then repeat cycle - if (xQueueReceive(channel->mainQueue, &ev, 1) == pdTRUE) { + if (xQueueReceive(channel->queue, &ev, 1) == pdTRUE) { // Process event processObjEvent(channel, &ev); } @@ -697,9 +697,9 @@ static int32_t setUpdatePeriod( #ifdef PIOS_TELEM_PRIORITY_QUEUE xQueueHandle targetQueue = UAVObjIsPriority(obj) ? channel->priorityQueue : - channel->mainQueue; + channel->queue; #else /* PIOS_TELEM_PRIORITY_QUEUE */ - xQueueHandle targetQueue = channel->mainQueue; + xQueueHandle targetQueue = channel->queue; #endif /* PIOS_TELEM_PRIORITY_QUEUE */ ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs); @@ -733,9 +733,9 @@ static int32_t setLoggingPeriod( #ifdef PIOS_TELEM_PRIORITY_QUEUE xQueueHandle targetQueue = UAVObjIsPriority(obj) ? channel->priorityQueue : - channel->mainQueue; + channel->queue; #else /* PIOS_TELEM_PRIORITY_QUEUE */ - xQueueHandle targetQueue = channel->mainQueue; + xQueueHandle targetQueue = channel->queue; #endif /* PIOS_TELEM_PRIORITY_QUEUE */ ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs); From 91e03a15410c54baf4af035156c68968800abdf5 Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Fri, 3 Apr 2015 12:37:37 +0100 Subject: [PATCH 6/8] OP-1289 Adding tracking of new RadioTx task --- flight/modules/Telemetry/telemetry.c | 4 ++-- shared/uavobjectdefinition/taskinfo.xml | 9 ++++++--- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index ee7e53cda..70b0cf8e4 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -32,7 +32,7 @@ /* Telemetry uses four tasks. Two are created for the main telemetry * stream called "TelTx" and "TelRx". Two are created to handle the OPLink - * radio connection, called "RadioTx" and "Radio Rx", the latter being + * radio connection, called "RadioTx" and "RadioRx", the latter being * overridden by USB if connected. * * The following code uses a "local" prefix to refer to the telemetry channel @@ -193,7 +193,7 @@ int32_t TelemetryStart(void) &radioChannel, TASK_PRIORITY_RADTX, &radioChannel.txTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIOTX, radioChannel.txTaskHandle); xTaskCreate(telemetryRxTask, "RadioRx", diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index 77310cfee..893660676 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -23,8 +23,9 @@ TelemetryTx TelemetryRx - + RadioTx RadioRx + Com2UsbBridge Usb2ComBridge @@ -54,8 +55,9 @@ TelemetryTx TelemetryRx - + RadioTx RadioRx + Com2UsbBridge Usb2ComBridge @@ -89,8 +91,9 @@ TelemetryTx TelemetryRx - + RadioTx RadioRx + Com2UsbBridge Usb2ComBridge From 50ce26bd80966c1e41ea0f24d96868bfc92b7933 Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Sat, 11 Apr 2015 10:06:19 +0100 Subject: [PATCH 7/8] OP-1803 Only create local telemetry queues/tasks if needed --- flight/modules/Telemetry/telemetry.c | 134 +++++++++++++++------------ 1 file changed, 76 insertions(+), 58 deletions(-) diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 70b0cf8e4..9d3d1d5e6 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -157,36 +157,45 @@ static void updateSettings(channelContext *channel); */ int32_t TelemetryStart(void) { - UAVObjIterate(®isterLocalObject); + // Only start the local telemetry tasks if needed + if (localPort()) { + UAVObjIterate(®isterLocalObject); + + // Listen to objects of interest +#ifdef PIOS_TELEM_PRIORITY_QUEUE + GCSTelemetryStatsConnectQueue(localChannel.priorityQueue); +#else /* PIOS_TELEM_PRIORITY_QUEUE */ + GCSTelemetryStatsConnectQueue(localChannel.queue); +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ + // Start telemetry tasks + xTaskCreate(telemetryTxTask, + "TelTx", + STACK_SIZE_TX_BYTES / 4, + &localChannel, + TASK_PRIORITY_TX, + &localChannel.txTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, + localChannel.txTaskHandle); + xTaskCreate(telemetryRxTask, + "TelRx", + STACK_SIZE_RX_BYTES / 4, + &localChannel, + TASK_PRIORITY_RX, + &localChannel.rxTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, + localChannel.rxTaskHandle); + } + + // Start the telemetry tasks associated with Radio/USB UAVObjIterate(®isterRadioObject); // Listen to objects of interest #ifdef PIOS_TELEM_PRIORITY_QUEUE - GCSTelemetryStatsConnectQueue(localChannel.priorityQueue); GCSTelemetryStatsConnectQueue(radioChannel.priorityQueue); #else /* PIOS_TELEM_PRIORITY_QUEUE */ - GCSTelemetryStatsConnectQueue(localChannel.queue); GCSTelemetryStatsConnectQueue(radioChannel.queue); #endif /* PIOS_TELEM_PRIORITY_QUEUE */ - // Start telemetry tasks - xTaskCreate(telemetryTxTask, - "TelTx", - STACK_SIZE_TX_BYTES / 4, - &localChannel, - TASK_PRIORITY_TX, - &localChannel.txTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, - localChannel.txTaskHandle); - xTaskCreate(telemetryRxTask, - "TelRx", - STACK_SIZE_RX_BYTES / 4, - &localChannel, - TASK_PRIORITY_RX, - &localChannel.rxTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, - localChannel.rxTaskHandle); - xTaskCreate(telemetryTxTask, "RadioTx", STACK_SIZE_RADIO_TX_BYTES / 4, @@ -207,6 +216,35 @@ int32_t TelemetryStart(void) return 0; } +/* Intialise a telemetry channel */ +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)); +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ + + // Initialise UAVTalk + channel->uavTalkCon = UAVTalkInitialize(&transmitLocalData); + + // Create periodic event that will be used to update the telemetry stats + UAVObjEvent ev; + memset(&ev, 0, sizeof(UAVObjEvent)); + +#ifdef PIOS_TELEM_PRIORITY_QUEUE + EventPeriodicQueueCreate(&ev, + channel->priorityQueue, + STATS_UPDATE_PERIOD_MS); +#else /* PIOS_TELEM_PRIORITY_QUEUE */ + EventPeriodicQueueCreate(&ev, + channel->queue, + STATS_UPDATE_PERIOD_MS); +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ +} + /** * Initialise the telemetry module * \return -1 if initialisation failed @@ -214,58 +252,38 @@ int32_t TelemetryStart(void) */ int32_t TelemetryInitialize(void) { + HwSettingsInitialize(); + FlightTelemetryStatsInitialize(); GCSTelemetryStatsInitialize(); // Initialize vars - timeOfLastObjectUpdate = 0; + timeOfLastObjectUpdate = 0; - // Create object queues - localChannel.queue = xQueueCreate(MAX_QUEUE_SIZE, - sizeof(UAVObjEvent)); - radioChannel.queue = xQueueCreate(MAX_QUEUE_SIZE, - sizeof(UAVObjEvent)); -#if defined(PIOS_TELEM_PRIORITY_QUEUE) - localChannel.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, - sizeof(UAVObjEvent)); - radioChannel.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, - sizeof(UAVObjEvent)); -#endif /* PIOS_TELEM_PRIORITY_QUEUE */ + // Reset link stats + txErrors = 0; + txRetries = 0; // Set channel port handlers localChannel.getPort = localPort; radioChannel.getPort = radioPort; - HwSettingsInitialize(); + // Set the local telemetry baud rate updateSettings(&localChannel); + // Only initialise local channel if telemetry port enabled + if (localPort()) { + // Initialise channel + TelemetryInitializeChannel(&localChannel); + // Initialise UAVTalk + localChannel.uavTalkCon = UAVTalkInitialize(&transmitLocalData); + } + + // Initialise channel + TelemetryInitializeChannel(&radioChannel); // Initialise UAVTalk - localChannel.uavTalkCon = UAVTalkInitialize(&transmitLocalData); radioChannel.uavTalkCon = UAVTalkInitialize(&transmitRadioData); - // Create periodic event that will be used to update the telemetry stats - // FIXME STATS_UPDATE_PERIOD_MS is 4000ms while FlighTelemetryStats update period is 5000ms... - txErrors = 0; - txRetries = 0; - UAVObjEvent ev; - memset(&ev, 0, sizeof(UAVObjEvent)); - -#ifdef PIOS_TELEM_PRIORITY_QUEUE - EventPeriodicQueueCreate(&ev, - localChannel.priorityQueue, - STATS_UPDATE_PERIOD_MS); - EventPeriodicQueueCreate(&ev, - radioChannel.priorityQueue, - STATS_UPDATE_PERIOD_MS); -#else /* PIOS_TELEM_PRIORITY_QUEUE */ - EventPeriodicQueueCreate(&ev, - localChannel.queue, - STATS_UPDATE_PERIOD_MS); - EventPeriodicQueueCreate(&ev, - radioChannel.queue, - STATS_UPDATE_PERIOD_MS); -#endif /* PIOS_TELEM_PRIORITY_QUEUE */ - return 0; } From 32a25e7044ae812d239d0b71b9051338db846ab3 Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Sat, 11 Apr 2015 10:06:19 +0100 Subject: [PATCH 8/8] OP-1289 Only create local telemetry queues/tasks if needed --- flight/modules/Telemetry/telemetry.c | 134 +++++++++++++++------------ 1 file changed, 76 insertions(+), 58 deletions(-) diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 70b0cf8e4..9d3d1d5e6 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -157,36 +157,45 @@ static void updateSettings(channelContext *channel); */ int32_t TelemetryStart(void) { - UAVObjIterate(®isterLocalObject); + // Only start the local telemetry tasks if needed + if (localPort()) { + UAVObjIterate(®isterLocalObject); + + // Listen to objects of interest +#ifdef PIOS_TELEM_PRIORITY_QUEUE + GCSTelemetryStatsConnectQueue(localChannel.priorityQueue); +#else /* PIOS_TELEM_PRIORITY_QUEUE */ + GCSTelemetryStatsConnectQueue(localChannel.queue); +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ + // Start telemetry tasks + xTaskCreate(telemetryTxTask, + "TelTx", + STACK_SIZE_TX_BYTES / 4, + &localChannel, + TASK_PRIORITY_TX, + &localChannel.txTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, + localChannel.txTaskHandle); + xTaskCreate(telemetryRxTask, + "TelRx", + STACK_SIZE_RX_BYTES / 4, + &localChannel, + TASK_PRIORITY_RX, + &localChannel.rxTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, + localChannel.rxTaskHandle); + } + + // Start the telemetry tasks associated with Radio/USB UAVObjIterate(®isterRadioObject); // Listen to objects of interest #ifdef PIOS_TELEM_PRIORITY_QUEUE - GCSTelemetryStatsConnectQueue(localChannel.priorityQueue); GCSTelemetryStatsConnectQueue(radioChannel.priorityQueue); #else /* PIOS_TELEM_PRIORITY_QUEUE */ - GCSTelemetryStatsConnectQueue(localChannel.queue); GCSTelemetryStatsConnectQueue(radioChannel.queue); #endif /* PIOS_TELEM_PRIORITY_QUEUE */ - // Start telemetry tasks - xTaskCreate(telemetryTxTask, - "TelTx", - STACK_SIZE_TX_BYTES / 4, - &localChannel, - TASK_PRIORITY_TX, - &localChannel.txTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, - localChannel.txTaskHandle); - xTaskCreate(telemetryRxTask, - "TelRx", - STACK_SIZE_RX_BYTES / 4, - &localChannel, - TASK_PRIORITY_RX, - &localChannel.rxTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, - localChannel.rxTaskHandle); - xTaskCreate(telemetryTxTask, "RadioTx", STACK_SIZE_RADIO_TX_BYTES / 4, @@ -207,6 +216,35 @@ int32_t TelemetryStart(void) return 0; } +/* Intialise a telemetry channel */ +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)); +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ + + // Initialise UAVTalk + channel->uavTalkCon = UAVTalkInitialize(&transmitLocalData); + + // Create periodic event that will be used to update the telemetry stats + UAVObjEvent ev; + memset(&ev, 0, sizeof(UAVObjEvent)); + +#ifdef PIOS_TELEM_PRIORITY_QUEUE + EventPeriodicQueueCreate(&ev, + channel->priorityQueue, + STATS_UPDATE_PERIOD_MS); +#else /* PIOS_TELEM_PRIORITY_QUEUE */ + EventPeriodicQueueCreate(&ev, + channel->queue, + STATS_UPDATE_PERIOD_MS); +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ +} + /** * Initialise the telemetry module * \return -1 if initialisation failed @@ -214,58 +252,38 @@ int32_t TelemetryStart(void) */ int32_t TelemetryInitialize(void) { + HwSettingsInitialize(); + FlightTelemetryStatsInitialize(); GCSTelemetryStatsInitialize(); // Initialize vars - timeOfLastObjectUpdate = 0; + timeOfLastObjectUpdate = 0; - // Create object queues - localChannel.queue = xQueueCreate(MAX_QUEUE_SIZE, - sizeof(UAVObjEvent)); - radioChannel.queue = xQueueCreate(MAX_QUEUE_SIZE, - sizeof(UAVObjEvent)); -#if defined(PIOS_TELEM_PRIORITY_QUEUE) - localChannel.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, - sizeof(UAVObjEvent)); - radioChannel.priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, - sizeof(UAVObjEvent)); -#endif /* PIOS_TELEM_PRIORITY_QUEUE */ + // Reset link stats + txErrors = 0; + txRetries = 0; // Set channel port handlers localChannel.getPort = localPort; radioChannel.getPort = radioPort; - HwSettingsInitialize(); + // Set the local telemetry baud rate updateSettings(&localChannel); + // Only initialise local channel if telemetry port enabled + if (localPort()) { + // Initialise channel + TelemetryInitializeChannel(&localChannel); + // Initialise UAVTalk + localChannel.uavTalkCon = UAVTalkInitialize(&transmitLocalData); + } + + // Initialise channel + TelemetryInitializeChannel(&radioChannel); // Initialise UAVTalk - localChannel.uavTalkCon = UAVTalkInitialize(&transmitLocalData); radioChannel.uavTalkCon = UAVTalkInitialize(&transmitRadioData); - // Create periodic event that will be used to update the telemetry stats - // FIXME STATS_UPDATE_PERIOD_MS is 4000ms while FlighTelemetryStats update period is 5000ms... - txErrors = 0; - txRetries = 0; - UAVObjEvent ev; - memset(&ev, 0, sizeof(UAVObjEvent)); - -#ifdef PIOS_TELEM_PRIORITY_QUEUE - EventPeriodicQueueCreate(&ev, - localChannel.priorityQueue, - STATS_UPDATE_PERIOD_MS); - EventPeriodicQueueCreate(&ev, - radioChannel.priorityQueue, - STATS_UPDATE_PERIOD_MS); -#else /* PIOS_TELEM_PRIORITY_QUEUE */ - EventPeriodicQueueCreate(&ev, - localChannel.queue, - STATS_UPDATE_PERIOD_MS); - EventPeriodicQueueCreate(&ev, - radioChannel.queue, - STATS_UPDATE_PERIOD_MS); -#endif /* PIOS_TELEM_PRIORITY_QUEUE */ - return 0; }