rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/steve/OP-1289_two_telemetry_streams' into next

Browse Source 
Conflicts:
	flight/modules/Telemetry/telemetry.c
This commit is contained in:
Alessio Morale 2015-04-18 19:10:13 +02:00
commit 5c76c0ee07
3 changed files with 373 additions and 198 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -9,7 +9,7 @@
* @{ * @{
* *
* @file telemetry.c * @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 * @brief Telemetry module, handles telemetry and UAVObject updates
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3
* *
@ -30,6 +30,34 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * 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 <openpilot.h> #include <openpilot.h>
#include "telemetry.h" #include "telemetry.h"
@ -53,62 +81,74 @@
#ifdef PIOS_TELEM_RADIO_RX_STACK_SIZE #ifdef PIOS_TELEM_RADIO_RX_STACK_SIZE
#define STACK_SIZE_RADIO_RX_BYTES 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 #else
#define STACK_SIZE_RADIO_RX_BYTES STACK_SIZE_RX_BYTES #define STACK_SIZE_RADIO_RX_BYTES STACK_SIZE_RX_BYTES
#define STACK_SIZE_RADIO_TX_BYTES STACK_SIZE_TX_BYTES
#endif #endif
#define TASK_PRIORITY_RX (tskIDLE_PRIORITY + 2) #define TASK_PRIORITY_RX (tskIDLE_PRIORITY + 2)
#define TASK_PRIORITY_TX (tskIDLE_PRIORITY + 2) #define TASK_PRIORITY_TX (tskIDLE_PRIORITY + 2)
#define TASK_PRIORITY_RADRX (tskIDLE_PRIORITY + 2) #define TASK_PRIORITY_RADRX (tskIDLE_PRIORITY + 2)
#define TASK_PRIORITY_RADTX (tskIDLE_PRIORITY + 2)
#define REQ_TIMEOUT_MS 250 #define REQ_TIMEOUT_MS 250
#define MAX_RETRIES 2 #define MAX_RETRIES 2
#define STATS_UPDATE_PERIOD_MS 4000 #define STATS_UPDATE_PERIOD_MS 4000
#define CONNECTION_TIMEOUT_MS 8000 #define CONNECTION_TIMEOUT_MS 8000
// Private types // 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 // Main telemetry channel
static uint32_t telemetryPort; static channelContext localChannel;
#ifdef PIOS_INCLUDE_RFM22B static int32_t transmitLocalData(uint8_t *data, int32_t length);
static uint32_t radioPort; static void registerLocalObject(UAVObjHandle obj);
#endif static uint32_t localPort();
static xQueueHandle queue;
#if defined(PIOS_TELEM_PRIORITY_QUEUE) // OPLink telemetry channel
static xQueueHandle priorityQueue; static channelContext radioChannel;
#else static int32_t transmitRadioData(uint8_t *data, int32_t length);
#define priorityQueue queue static void registerRadioObject(UAVObjHandle obj);
#endif static uint32_t radioPort();
static xTaskHandle telemetryTxTaskHandle; // Telemetry stats
static xTaskHandle telemetryRxTaskHandle;
#ifdef PIOS_INCLUDE_RFM22B
static xTaskHandle radioRxTaskHandle;
#endif
static uint32_t txErrors; static uint32_t txErrors;
static uint32_t txRetries; static uint32_t txRetries;
static uint32_t timeOfLastObjectUpdate; static uint32_t timeOfLastObjectUpdate;
static UAVTalkConnection uavTalkCon;
#ifdef PIOS_INCLUDE_RFM22B
static UAVTalkConnection radioUavTalkCon;
#endif
// Private functions
static void telemetryTxTask(void *parameters); static void telemetryTxTask(void *parameters);
static void telemetryRxTask(void *parameters); static void telemetryRxTask(void *parameters);
#ifdef PIOS_INCLUDE_RFM22B static void updateObject(
static void radioRxTask(void *parameters); channelContext *channel,
static int32_t transmitRadioData(uint8_t *data, int32_t length); UAVObjHandle obj,
#endif int32_t eventType);
static int32_t transmitData(uint8_t *data, int32_t length); static void processObjEvent(
static void registerObject(UAVObjHandle obj); channelContext *channel,
static void updateObject(UAVObjHandle obj, int32_t eventType); UAVObjEvent *ev);
static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs); static int32_t setUpdatePeriod(
static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs); channelContext *channel,
static void processObjEvent(UAVObjEvent *ev); UAVObjHandle obj,
int32_t updatePeriodMs);
static int32_t setLoggingPeriod(
channelContext *channel,
UAVObjHandle obj,
int32_t updatePeriodMs);
static void updateTelemetryStats(); static void updateTelemetryStats();
static void gcsTelemetryStatsUpdated(); static void gcsTelemetryStatsUpdated();
static void updateSettings(); static void updateSettings(channelContext *channel);
static uint32_t getComPort(bool input);
/** /**
* Initialise the telemetry module * Initialise the telemetry module
@ -117,28 +157,94 @@ static uint32_t getComPort(bool input);
*/ */
int32_t TelemetryStart(void) int32_t TelemetryStart(void)
{ {
// Process all registered objects and connect queue for updates // Only start the local telemetry tasks if needed
UAVObjIterate(&registerObject); if (localPort()) {
UAVObjIterate(&registerLocalObject);
// 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(&registerRadioObject);
// Listen to objects of interest // 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,
xTaskCreate(telemetryTxTask, "TelTx", STACK_SIZE_TX_BYTES / 4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle); "RadioTx",
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTX, telemetryTxTaskHandle); STACK_SIZE_RADIO_TX_BYTES / 4,
xTaskCreate(telemetryRxTask, "TelRx", STACK_SIZE_RX_BYTES / 4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle); &radioChannel,
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle); TASK_PRIORITY_RADTX,
&radioChannel.txTaskHandle);
#ifdef PIOS_INCLUDE_RFM22B PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIOTX,
if (radioPort) { radioChannel.txTaskHandle);
xTaskCreate(radioRxTask, "RadioRx", STACK_SIZE_RADIO_RX_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle); xTaskCreate(telemetryRxTask,
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, radioRxTaskHandle); "RadioRx",
} STACK_SIZE_RADIO_RX_BYTES / 4,
#endif &radioChannel,
TASK_PRIORITY_RADRX,
&radioChannel.rxTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX,
radioChannel.rxTaskHandle);
return 0; 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 * Initialise the telemetry module
* \return -1 if initialisation failed * \return -1 if initialisation failed
@ -146,39 +252,37 @@ int32_t TelemetryStart(void)
*/ */
int32_t TelemetryInitialize(void) int32_t TelemetryInitialize(void)
{ {
HwSettingsInitialize();
FlightTelemetryStatsInitialize(); FlightTelemetryStatsInitialize();
GCSTelemetryStatsInitialize(); GCSTelemetryStatsInitialize();
// Initialize vars // Initialize vars
timeOfLastObjectUpdate = 0; timeOfLastObjectUpdate = 0;
// Create object queues // Reset link stats
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...
txErrors = 0; txErrors = 0;
txRetries = 0; txRetries = 0;
UAVObjEvent ev;
memset(&ev, 0, sizeof(UAVObjEvent)); // Set channel port handlers
EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS); 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; return 0;
} }
@ -190,22 +294,51 @@ MODULE_INITCALL(TelemetryInitialize, TelemetryStart);
* telemetry settings. * telemetry settings.
* \param[in] obj Object to connect * \param[in] obj Object to connect
*/ */
static void registerObject(UAVObjHandle obj) static void registerLocalObject(UAVObjHandle obj)
{ {
if (UAVObjIsMetaobject(obj)) { if (UAVObjIsMetaobject(obj)) {
// Only connect change notifications for meta objects. No periodic updates // 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 { } else {
// Setup object for periodic updates // 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 * Update object's queue connections and timer, depending on object's settings
* \param[in] telemetry channel context
* \param[in] obj Object to updates * \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; UAVObjMetadata metadata;
UAVObjUpdateMode updateMode, loggingMode; UAVObjUpdateMode updateMode, loggingMode;
@ -228,13 +361,15 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
switch (updateMode) { switch (updateMode) {
case UPDATEMODE_PERIODIC: case UPDATEMODE_PERIODIC:
// Set update period // Set update period
setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); setUpdatePeriod(channel,
obj,
metadata.telemetryUpdatePeriod);
// Connect queue // Connect queue
eventMask |= EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ; eventMask |= EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
break; break;
case UPDATEMODE_ONCHANGE: case UPDATEMODE_ONCHANGE:
// Set update period // Set update period
setUpdatePeriod(obj, 0); setUpdatePeriod(channel, obj, 0);
// Connect queue // Connect queue
eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
break; break;
@ -244,7 +379,9 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; eventMask |= EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
// Set update period on initialization and metadata change // Set update period on initialization and metadata change
if (eventType == EV_NONE) { if (eventType == EV_NONE) {
setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); setUpdatePeriod(channel,
obj,
metadata.telemetryUpdatePeriod);
} }
} else { } else {
// Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates // 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; break;
case UPDATEMODE_MANUAL: case UPDATEMODE_MANUAL:
// Set update period // Set update period
setUpdatePeriod(obj, 0); setUpdatePeriod(channel, obj, 0);
// Connect queue // Connect queue
eventMask |= EV_UPDATED_MANUAL | EV_UPDATE_REQ; eventMask |= EV_UPDATED_MANUAL | EV_UPDATE_REQ;
break; break;
@ -261,13 +398,13 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
switch (loggingMode) { switch (loggingMode) {
case UPDATEMODE_PERIODIC: case UPDATEMODE_PERIODIC:
// Set update period // Set update period
setLoggingPeriod(obj, metadata.loggingUpdatePeriod); setLoggingPeriod(channel, obj, metadata.loggingUpdatePeriod);
// Connect queue // Connect queue
eventMask |= EV_LOGGING_PERIODIC | EV_LOGGING_MANUAL; eventMask |= EV_LOGGING_PERIODIC | EV_LOGGING_MANUAL;
break; break;
case UPDATEMODE_ONCHANGE: case UPDATEMODE_ONCHANGE:
// Set update period // Set update period
setLoggingPeriod(obj, 0); setLoggingPeriod(channel, obj, 0);
// Connect queue // Connect queue
eventMask |= EV_UPDATED | EV_LOGGING_MANUAL; eventMask |= EV_UPDATED | EV_LOGGING_MANUAL;
break; break;
@ -277,7 +414,9 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
eventMask |= EV_UPDATED | EV_LOGGING_MANUAL; eventMask |= EV_UPDATED | EV_LOGGING_MANUAL;
// Set update period on initialization and metadata change // Set update period on initialization and metadata change
if (eventType == EV_NONE) { if (eventType == EV_NONE) {
setLoggingPeriod(obj, metadata.loggingUpdatePeriod); setLoggingPeriod(channel,
obj,
metadata.loggingUpdatePeriod);
} }
} else { } else {
// Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates // 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; break;
case UPDATEMODE_MANUAL: case UPDATEMODE_MANUAL:
// Set update period // Set update period
setLoggingPeriod(obj, 0); setLoggingPeriod(channel, obj, 0);
// Connect queue // Connect queue
eventMask |= EV_LOGGING_MANUAL; eventMask |= EV_LOGGING_MANUAL;
break; break;
} }
// note that all setting objects have implicitly IsPriority=true // note that all setting objects have implicitly IsPriority=true
#ifdef PIOS_TELEM_PRIORITY_QUEUE
if (UAVObjIsPriority(obj)) { if (UAVObjIsPriority(obj)) {
UAVObjConnectQueue(obj, priorityQueue, eventMask); UAVObjConnectQueue(obj, channel->priorityQueue, eventMask);
} else { } else
UAVObjConnectQueue(obj, queue, eventMask); #endif /* PIOS_TELEM_PRIORITY_QUEUE */
} UAVObjConnectQueue(obj, channel->queue, eventMask);
} }
/** /**
* Processes queue events * Processes queue events
*/ */
static void processObjEvent(UAVObjEvent *ev) static void processObjEvent(
channelContext *channel,
UAVObjEvent *ev)
{ {
UAVObjMetadata metadata; UAVObjMetadata metadata;
UAVObjUpdateMode updateMode; UAVObjUpdateMode updateMode;
@ -327,7 +471,10 @@ static void processObjEvent(UAVObjEvent *ev)
// Send update to GCS (with retries) // Send update to GCS (with retries)
while (retries < MAX_RETRIES && success == -1) { while (retries < MAX_RETRIES && success == -1) {
// call blocks until ack is received or timeout // 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) { if (success == -1) {
++retries; ++retries;
} }
@ -341,7 +488,10 @@ static void processObjEvent(UAVObjEvent *ev)
// Request object update from GCS (with retries) // Request object update from GCS (with retries)
while (retries < MAX_RETRIES && success == -1) { while (retries < MAX_RETRIES && success == -1) {
// call blocks until update is received or timeout // 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) { if (success == -1) {
++retries; ++retries;
} }
@ -355,11 +505,17 @@ static void processObjEvent(UAVObjEvent *ev)
// If this is a metaobject then make necessary telemetry updates // If this is a metaobject then make necessary telemetry updates
if (UAVObjIsMetaobject(ev->obj)) { if (UAVObjIsMetaobject(ev->obj)) {
// linked object will be the actual object the metadata are for // linked object will be the actual object the metadata are for
updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); updateObject(
channel,
UAVObjGetLinkedObj(ev->obj),
EV_NONE);
} else { } else {
if (updateMode == UPDATEMODE_THROTTLED) { if (updateMode == UPDATEMODE_THROTTLED) {
// If this is UPDATEMODE_THROTTLED, the event mask changes on every event. // 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 (updateMode == UPDATEMODE_THROTTLED) {
// If this is UPDATEMODE_THROTTLED, the event mask changes on every event. // 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 * Telemetry transmit task, regular priority
*/ */
static void telemetryTxTask(__attribute__((unused)) void *parameters) static void telemetryTxTask(void *parameters)
{ {
channelContext *channel = (channelContext *)parameters;
UAVObjEvent ev; UAVObjEvent ev;
/* Check for a bad context */
if (!channel) {
return;
}
// Loop forever // Loop forever
while (1) { while (1) {
/** /**
* Tries to empty the high priority queue before handling any standard priority item * 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 // empty priority queue, non-blocking
while (xQueueReceive(priorityQueue, &ev, 0) == pdTRUE) { while (xQueueReceive(channel->priorityQueue, &ev, 0) == pdTRUE) {
// Process event // Process event
processObjEvent(&ev); processObjEvent(channel, &ev);
} }
// check regular queue and process update - non-blocking // check regular queue and process update - non-blocking
if (xQueueReceive(queue, &ev, 0) == pdTRUE) { if (xQueueReceive(channel->queue, &ev, 0) == pdTRUE) {
// Process event // Process event
processObjEvent(&ev); processObjEvent(channel, &ev);
// if both queues are empty, wait on priority queue for updates (1 tick) then repeat cycle // 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 // Process event
processObjEvent(&ev); processObjEvent(channel, &ev);
} }
#else #else
// wait on queue for updates (1 tick) then repeat cycle // 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 // 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. * 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 // Task loop
while (1) { while (1) {
uint32_t inputPort = getComPort(true); uint32_t inputPort = channel->getPort();
if (inputPort) { if (inputPort) {
// Block until data are available // 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); bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500);
if (bytes_to_process > 0) { if (bytes_to_process > 0) {
UAVTalkProcessInputStream(uavTalkCon, serial_data, bytes_to_process); UAVTalkProcessInputStream(channel->uavTalkCon, serial_data, bytes_to_process);
} }
} else { } else {
vTaskDelay(5); 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 return PIOS_COM_TELEM_RF;
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);
}
}
} }
/**
* 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. * Transmit data buffer to the radioport.
* \param[in] data Data buffer to send * \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) static int32_t transmitRadioData(uint8_t *data, int32_t length)
{ {
if (radioPort) { uint32_t outputPort = radioChannel.getPort();
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);
if (outputPort) { if (outputPort) {
return PIOS_COM_SendBuffer(outputPort, data, length); 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) * 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] obj The object to update
* \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled * \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled
* \return 0 Success * \return 0 Success
* \return -1 Failure * \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; UAVObjEvent ev;
int32_t ret; int32_t ret;
@ -522,7 +711,12 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
ev.event = EV_UPDATED_PERIODIC; ev.event = EV_UPDATED_PERIODIC;
ev.lowPriority = true; 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); ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs);
if (ret == -1) { 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) * 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] obj The object to update
* \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled * \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled
* \return 0 Success * \return 0 Success
* \return -1 Failure * \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; UAVObjEvent ev;
int32_t ret; int32_t ret;
@ -549,7 +747,12 @@ static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs)
ev.event = EV_LOGGING_PERIODIC; ev.event = EV_LOGGING_PERIODIC;
ev.lowPriority = true; 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); ret = EventPeriodicQueueUpdate(&ev, targetQueue, updatePeriodMs);
if (ret == -1) { if (ret == -1) {
@ -588,10 +791,8 @@ static void updateTelemetryStats()
uint32_t timeNow; uint32_t timeNow;
// Get stats // Get stats
UAVTalkGetStats(uavTalkCon, &utalkStats, true); UAVTalkGetStats(localChannel.uavTalkCon, &utalkStats, true);
#ifdef PIOS_INCLUDE_RFM22B UAVTalkAddStats(radioChannel.uavTalkCon, &utalkStats, true);
UAVTalkAddStats(radioUavTalkCon, &utalkStats, true);
#endif
// Get object data // Get object data
FlightTelemetryStatsGet(&flightStats); FlightTelemetryStatsGet(&flightStats);
@ -681,9 +882,11 @@ static void updateTelemetryStats()
* settings, etc. Thus the HwSettings object which contains the * settings, etc. Thus the HwSettings object which contains the
* telemetry port speed is used for now. * 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 // Retrieve settings
uint8_t speed; uint8_t speed;
HwSettingsTelemetrySpeedGet(&speed); HwSettingsTelemetrySpeedGet(&speed);
@ -691,58 +894,30 @@ static void updateSettings()
// Set port speed // Set port speed
switch (speed) { switch (speed) {
case HWSETTINGS_TELEMETRYSPEED_2400: case HWSETTINGS_TELEMETRYSPEED_2400:
PIOS_COM_ChangeBaud(telemetryPort, 2400); PIOS_COM_ChangeBaud(port, 2400);
break; break;
case HWSETTINGS_TELEMETRYSPEED_4800: case HWSETTINGS_TELEMETRYSPEED_4800:
PIOS_COM_ChangeBaud(telemetryPort, 4800); PIOS_COM_ChangeBaud(port, 4800);
break; break;
case HWSETTINGS_TELEMETRYSPEED_9600: case HWSETTINGS_TELEMETRYSPEED_9600:
PIOS_COM_ChangeBaud(telemetryPort, 9600); PIOS_COM_ChangeBaud(port, 9600);
break; break;
case HWSETTINGS_TELEMETRYSPEED_19200: case HWSETTINGS_TELEMETRYSPEED_19200:
PIOS_COM_ChangeBaud(telemetryPort, 19200); PIOS_COM_ChangeBaud(port, 19200);
break; break;
case HWSETTINGS_TELEMETRYSPEED_38400: case HWSETTINGS_TELEMETRYSPEED_38400:
PIOS_COM_ChangeBaud(telemetryPort, 38400); PIOS_COM_ChangeBaud(port, 38400);
break; break;
case HWSETTINGS_TELEMETRYSPEED_57600: case HWSETTINGS_TELEMETRYSPEED_57600:
PIOS_COM_ChangeBaud(telemetryPort, 57600); PIOS_COM_ChangeBaud(port, 57600);
break; break;
case HWSETTINGS_TELEMETRYSPEED_115200: case HWSETTINGS_TELEMETRYSPEED_115200:
PIOS_COM_ChangeBaud(telemetryPort, 115200); PIOS_COM_ChangeBaud(port, 115200);
break; 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;
}
}
/** /**
* @} * @}

5
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -740,10 +740,7 @@ void PIOS_Board_Init(void)
tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) {
PIOS_Assert(0); 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; 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. // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex.

9
shared/uavobjectdefinition/taskinfo.xml
View File

@ -23,8 +23,9 @@
<!-- telemetry --> <!-- telemetry -->
<elementname>TelemetryTx</elementname> <elementname>TelemetryTx</elementname>
<elementname>TelemetryRx</elementname> <elementname>TelemetryRx</elementname>
<!-- com --> <elementname>RadioTx</elementname>
<elementname>RadioRx</elementname> <elementname>RadioRx</elementname>
<!-- com -->
<elementname>Com2UsbBridge</elementname> <elementname>Com2UsbBridge</elementname>
<elementname>Usb2ComBridge</elementname> <elementname>Usb2ComBridge</elementname>
<!-- optional --> <!-- optional -->
@ -54,8 +55,9 @@
<!-- telemetry --> <!-- telemetry -->
<elementname>TelemetryTx</elementname> <elementname>TelemetryTx</elementname>
<elementname>TelemetryRx</elementname> <elementname>TelemetryRx</elementname>
<!-- com --> <elementname>RadioTx</elementname>
<elementname>RadioRx</elementname> <elementname>RadioRx</elementname>
<!-- com -->
<elementname>Com2UsbBridge</elementname> <elementname>Com2UsbBridge</elementname>
<elementname>Usb2ComBridge</elementname> <elementname>Usb2ComBridge</elementname>
<!-- optional --> <!-- optional -->
@ -89,8 +91,9 @@
<!-- telemetry --> <!-- telemetry -->
<elementname>TelemetryTx</elementname> <elementname>TelemetryTx</elementname>
<elementname>TelemetryRx</elementname> <elementname>TelemetryRx</elementname>
<!-- com --> <elementname>RadioTx</elementname>
<elementname>RadioRx</elementname> <elementname>RadioRx</elementname>
<!-- com -->
<elementname>Com2UsbBridge</elementname> <elementname>Com2UsbBridge</elementname>
<elementname>Usb2ComBridge</elementname> <elementname>Usb2ComBridge</elementname>
<!-- optional --> <!-- optional -->