diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 86d565891..aeaaf5917 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 * @@ -30,6 +30,34 @@ * 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 "RadioRx", the latter being + * overridden by USB if connected. + * + * 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 + * "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. + * + * 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 "local" or "radio" link. + */ + #include #include "telemetry.h" @@ -53,62 +81,74 @@ #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 { + // 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; -// Private variables -static uint32_t telemetryPort; -#ifdef PIOS_INCLUDE_RFM22B -static uint32_t radioPort; -#endif -static xQueueHandle queue; +// 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(); -#if defined(PIOS_TELEM_PRIORITY_QUEUE) -static xQueueHandle priorityQueue; -#else -#define priorityQueue queue -#endif +// OPLink telemetry channel +static channelContext radioChannel; +static int32_t transmitRadioData(uint8_t *data, int32_t length); +static void registerRadioObject(UAVObjHandle obj); +static uint32_t radioPort(); -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( + channelContext *channel, + UAVObjHandle obj, + int32_t eventType); +static void processObjEvent( + channelContext *channel, + UAVObjEvent *ev); +static int32_t setUpdatePeriod( + channelContext *channel, + UAVObjHandle obj, + int32_t updatePeriodMs); +static int32_t setLoggingPeriod( + channelContext *channel, + UAVObjHandle obj, + int32_t updatePeriodMs); static void updateTelemetryStats(); static void gcsTelemetryStatsUpdated(); -static void updateSettings(); -static uint32_t getComPort(bool input); +static void updateSettings(channelContext *channel); /** * Initialise the telemetry module @@ -117,28 +157,94 @@ static uint32_t getComPort(bool input); */ int32_t TelemetryStart(void) { - // Process all registered objects and connect queue for updates - UAVObjIterate(®isterObject); + // 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 - GCSTelemetryStatsConnectQueue(priorityQueue); +#ifdef PIOS_TELEM_PRIORITY_QUEUE + GCSTelemetryStatsConnectQueue(radioChannel.priorityQueue); +#else /* PIOS_TELEM_PRIORITY_QUEUE */ + GCSTelemetryStatsConnectQueue(radioChannel.queue); +#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); - -#ifdef PIOS_INCLUDE_RFM22B - if (radioPort) { - 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, + &radioChannel, + TASK_PRIORITY_RADTX, + &radioChannel.txTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIOTX, + radioChannel.txTaskHandle); + xTaskCreate(telemetryRxTask, + "RadioRx", + STACK_SIZE_RADIO_RX_BYTES / 4, + &radioChannel, + TASK_PRIORITY_RADRX, + &radioChannel.rxTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, + radioChannel.rxTaskHandle); 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 @@ -146,39 +252,37 @@ int32_t TelemetryStart(void) */ int32_t TelemetryInitialize(void) { + HwSettingsInitialize(); + FlightTelemetryStatsInitialize(); GCSTelemetryStatsInitialize(); // Initialize vars timeOfLastObjectUpdate = 0; - // Create object queues - queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); -#if defined(PIOS_TELEM_PRIORITY_QUEUE) - priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); -#endif - - // Update telemetry settings - telemetryPort = PIOS_COM_TELEM_RF; -#ifdef PIOS_INCLUDE_RFM22B - radioPort = PIOS_COM_RF; -#endif - HwSettingsInitialize(); - updateSettings(); - - // Initialise UAVTalk - uavTalkCon = UAVTalkInitialize(&transmitData); -#ifdef PIOS_INCLUDE_RFM22B - radioUavTalkCon = UAVTalkInitialize(&transmitRadioData); -#endif - - // 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... + // Reset link stats txErrors = 0; txRetries = 0; - UAVObjEvent ev; - memset(&ev, 0, sizeof(UAVObjEvent)); - EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS); + + // Set channel port handlers + localChannel.getPort = localPort; + radioChannel.getPort = radioPort; + + // 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 + radioChannel.uavTalkCon = UAVTalkInitialize(&transmitRadioData); return 0; } @@ -190,22 +294,51 @@ MODULE_INITCALL(TelemetryInitialize, TelemetryStart); * telemetry settings. * \param[in] obj Object to connect */ -static void registerObject(UAVObjHandle obj) +static void registerLocalObject(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, localChannel.priorityQueue, EV_MASK_ALL_UPDATES); +#else /* PIOS_TELEM_PRIORITY_QUEUE */ + UAVObjConnectQueue(obj, localChannel.queue, EV_MASK_ALL_UPDATES); +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ } else { // Setup object for periodic updates - updateObject(obj, EV_NONE); + updateObject( + &localChannel, + 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, radioChannel.priorityQueue, EV_MASK_ALL_UPDATES); +#else /* PIOS_TELEM_PRIORITY_QUEUE */ + UAVObjConnectQueue(obj, radioChannel.queue, EV_MASK_ALL_UPDATES); +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ + } else { + // Setup object for periodic updates + updateObject( + &radioChannel, + obj, + EV_NONE); } } /** * 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(UAVObjHandle obj, int32_t eventType) +static void updateObject( + channelContext *channel, + UAVObjHandle obj, + int32_t eventType) { UAVObjMetadata metadata; UAVObjUpdateMode updateMode, loggingMode; @@ -228,13 +361,15 @@ static void updateObject(UAVObjHandle obj, int32_t eventType) switch (updateMode) { case UPDATEMODE_PERIODIC: // Set update period - setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); + setUpdatePeriod(channel, + 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(channel, obj, 0); // Connect queue eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; break; @@ -244,7 +379,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(channel, + obj, + metadata.telemetryUpdatePeriod); } } else { // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates @@ -253,7 +390,7 @@ static void updateObject(UAVObjHandle obj, int32_t eventType) break; case UPDATEMODE_MANUAL: // Set update period - setUpdatePeriod(obj, 0); + setUpdatePeriod(channel, obj, 0); // Connect queue eventMask |= EV_UPDATED_MANUAL | EV_UPDATE_REQ; break; @@ -261,13 +398,13 @@ static void updateObject(UAVObjHandle obj, int32_t eventType) switch (loggingMode) { case UPDATEMODE_PERIODIC: // Set update period - setLoggingPeriod(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(obj, 0); + setLoggingPeriod(channel, obj, 0); // Connect queue eventMask |= EV_UPDATED | EV_LOGGING_MANUAL; break; @@ -277,7 +414,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(channel, + obj, + metadata.loggingUpdatePeriod); } } else { // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates @@ -286,23 +425,28 @@ static void updateObject(UAVObjHandle obj, int32_t eventType) break; case UPDATEMODE_MANUAL: // Set update period - setLoggingPeriod(obj, 0); + setLoggingPeriod(channel, 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, channel->priorityQueue, eventMask); + } else +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ + UAVObjConnectQueue(obj, channel->queue, eventMask); } + /** * Processes queue events */ -static void processObjEvent(UAVObjEvent *ev) +static void processObjEvent( + channelContext *channel, + UAVObjEvent *ev) { UAVObjMetadata metadata; UAVObjUpdateMode updateMode; @@ -327,7 +471,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(channel->uavTalkCon, + ev->obj, + ev->instId, + UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); if (success == -1) { ++retries; } @@ -341,7 +488,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(channel->uavTalkCon, + ev->obj, + ev->instId, + REQ_TIMEOUT_MS); if (success == -1) { ++retries; } @@ -355,11 +505,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( + channel, + 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( + channel, + ev->obj, + ev->event); } } } @@ -380,7 +536,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( + channel, + ev->obj, + ev->event); } } } @@ -388,37 +547,43 @@ static void processObjEvent(UAVObjEvent *ev) /** * Telemetry transmit task, regular priority */ -static void telemetryTxTask(__attribute__((unused)) void *parameters) +static void telemetryTxTask(void *parameters) { + channelContext *channel = (channelContext *)parameters; UAVObjEvent ev; + /* Check for a bad context */ + if (!channel) { + 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(channel->priorityQueue, &ev, 0) == pdTRUE) { // Process event - processObjEvent(&ev); + processObjEvent(channel, &ev); } // check regular queue and process update - non-blocking - if (xQueueReceive(queue, &ev, 0) == pdTRUE) { + if (xQueueReceive(channel->queue, &ev, 0) == pdTRUE) { // Process event - processObjEvent(&ev); + processObjEvent(channel, &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(channel->priorityQueue, &ev, 1) == pdTRUE) { // Process event - processObjEvent(&ev); + processObjEvent(channel, &ev); } #else // wait on queue for updates (1 tick) then repeat cycle - if (xQueueReceive(queue, &ev, 1) == pdTRUE) { + if (xQueueReceive(channel->queue, &ev, 1) == pdTRUE) { // Process event - processObjEvent(&ev); + processObjEvent(channel, &ev); } -#endif /* if defined(PIOS_TELEM_PRIORITY_QUEUE) */ +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ } } @@ -426,11 +591,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) { + channelContext *channel = (channelContext *)parameters; + + /* Check for a bad context */ + if (!channel) { + return; + } + // Task loop while (1) { - uint32_t inputPort = getComPort(true); + uint32_t inputPort = channel->getPort(); if (inputPort) { // Block until data are available @@ -439,7 +611,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) { - UAVTalkProcessInputStream(uavTalkCon, serial_data, bytes_to_process); + UAVTalkProcessInputStream(channel->uavTalkCon, serial_data, bytes_to_process); } } else { vTaskDelay(5); @@ -447,28 +619,58 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) } } -#ifdef PIOS_INCLUDE_RFM22B + /** - * Radio telemetry receive task. Processes queue events and periodic updates. + * Determine the port to be used for communication on the telemetry channel + * \return com port number */ -static void radioRxTask(__attribute__((unused)) void *parameters) +static uint32_t localPort() { - // Task loop - while (1) { - if (radioPort) { - // Block until data are available - uint8_t serial_data[16]; - uint16_t bytes_to_process; - - bytes_to_process = PIOS_COM_ReceiveBuffer(radioPort, serial_data, sizeof(serial_data), 500); - if (bytes_to_process > 0) { - UAVTalkProcessInputStream(radioUavTalkCon, serial_data, bytes_to_process); - } - } else { - vTaskDelay(5); - } - } + 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 + * \param[in] length Length of buffer + * \return -1 on failure + * \return number of bytes transmitted on success + */ +static int32_t transmitLocalData(uint8_t *data, int32_t length) +{ + uint32_t outputPort = localChannel.getPort(); + + if (outputPort) { + return PIOS_COM_SendBuffer(outputPort, data, length); + } + + return -1; +} + + /** * Transmit data buffer to the radioport. * \param[in] data Data buffer to send @@ -478,24 +680,7 @@ static void radioRxTask(__attribute__((unused)) void *parameters) */ 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. - * \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 transmitData(uint8_t *data, int32_t length) -{ - uint32_t outputPort = getComPort(false); + uint32_t outputPort = radioChannel.getPort(); if (outputPort) { return PIOS_COM_SendBuffer(outputPort, data, length); @@ -506,12 +691,16 @@ static int32_t transmitData(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 * \return -1 Failure */ -static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs) +static int32_t setUpdatePeriod( + channelContext *channel, + UAVObjHandle obj, + int32_t updatePeriodMs) { UAVObjEvent ev; int32_t ret; @@ -522,7 +711,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) ? channel->priorityQueue : + channel->queue; +#else /* PIOS_TELEM_PRIORITY_QUEUE */ + xQueueHandle targetQueue = channel->queue; +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs); if (ret == -1) { @@ -533,12 +727,16 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs) /** * 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 * \return -1 Failure */ -static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs) +static int32_t setLoggingPeriod( + channelContext *channel, + UAVObjHandle obj, + int32_t updatePeriodMs) { UAVObjEvent ev; int32_t ret; @@ -549,7 +747,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) ? channel->priorityQueue : + channel->queue; +#else /* PIOS_TELEM_PRIORITY_QUEUE */ + xQueueHandle targetQueue = channel->queue; +#endif /* PIOS_TELEM_PRIORITY_QUEUE */ ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs); if (ret == -1) { @@ -588,10 +791,8 @@ static void updateTelemetryStats() uint32_t timeNow; // Get stats - UAVTalkGetStats(uavTalkCon, &utalkStats, true); -#ifdef PIOS_INCLUDE_RFM22B - UAVTalkAddStats(radioUavTalkCon, &utalkStats, true); -#endif + UAVTalkGetStats(localChannel.uavTalkCon, &utalkStats, true); + UAVTalkAddStats(radioChannel.uavTalkCon, &utalkStats, true); // Get object data FlightTelemetryStatsGet(&flightStats); @@ -681,9 +882,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(channelContext *channel) { - if (telemetryPort) { + uint32_t port = channel->getPort(); + + if (port) { // Retrieve settings uint8_t speed; HwSettingsTelemetrySpeedGet(&speed); @@ -691,58 +894,30 @@ static void updateSettings() // Set port speed switch (speed) { case HWSETTINGS_TELEMETRYSPEED_2400: - PIOS_COM_ChangeBaud(telemetryPort, 2400); + PIOS_COM_ChangeBaud(port, 2400); break; case HWSETTINGS_TELEMETRYSPEED_4800: - PIOS_COM_ChangeBaud(telemetryPort, 4800); + PIOS_COM_ChangeBaud(port, 4800); break; case HWSETTINGS_TELEMETRYSPEED_9600: - PIOS_COM_ChangeBaud(telemetryPort, 9600); + PIOS_COM_ChangeBaud(port, 9600); break; case HWSETTINGS_TELEMETRYSPEED_19200: - PIOS_COM_ChangeBaud(telemetryPort, 19200); + PIOS_COM_ChangeBaud(port, 19200); break; case HWSETTINGS_TELEMETRYSPEED_38400: - PIOS_COM_ChangeBaud(telemetryPort, 38400); + PIOS_COM_ChangeBaud(port, 38400); break; case HWSETTINGS_TELEMETRYSPEED_57600: - PIOS_COM_ChangeBaud(telemetryPort, 57600); + PIOS_COM_ChangeBaud(port, 57600); break; case HWSETTINGS_TELEMETRYSPEED_115200: - PIOS_COM_ChangeBaud(telemetryPort, 115200); + PIOS_COM_ChangeBaud(port, 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. 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