flight/modules/RadioComBridge/RadioComBridge.c

   1 /**
   2  ******************************************************************************
   3
   4  * @addtogroup OpenPilotModules OpenPilot Modules
   5  * @{
   6  * @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module
   7  * @brief Bridge Com and Radio ports
   8  * @{
   9  *
  10  * @file       RadioComBridge.c
  11  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012-2013.
  12  * @brief      Bridges selected Com Port to the COM VCP emulated serial port
  13  * @see        The GNU Public License (GPL) Version 3
  14  *
  15  *****************************************************************************/
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 // ****************
  33
  34 #include <openpilot.h>
  35 #include <radiocombridge.h>
  36 #include <oplinkstatus.h>
  37 #include <objectpersistence.h>
  38 #include <oplinksettings.h>
  39 #include <oplinkreceiver.h>
  40 #include <radiocombridgestats.h>
  41 #include <uavtalk_priv.h>
  42 #include <pios_rfm22b.h>
  43 #include <ecc.h>
  44 #include <pios_board_io.h>
  45 #include <stdbool.h>
  46 #include <manualcontrolsettings.h>
  47
  48 // ****************
  49 // Private constants
  50
  51 #define TELEM_STACK_SIZE_WORDS 150
  52 #define PPM_STACK_SIZE_WORDS   150
  53 #define TASK_PRIORITY          (tskIDLE_PRIORITY + 1)
  54 #define MAX_RETRIES            2
  55 #define RETRY_TIMEOUT_MS       20
  56 #define EVENT_QUEUE_SIZE       10
  57 #define MAX_PORT_DELAY         200
  58 #define PPM_INPUT_TIMEOUT      100
  59
  60 #define PIOS_PPM_RECEIVER      pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM]
  61
  62 // ****************
  63 // Private types
  64
  65 typedef struct {
  66     // The task handles.
  67     xTaskHandle telemetryTxTaskHandle;
  68     xTaskHandle telemetryRxTaskHandle;
  69     xTaskHandle telemRadioTxTaskHandle;
  70     xTaskHandle telemRadioRxTaskHandle;
  71     xTaskHandle PPMInputTaskHandle;
  72
  73     // The UAVTalk connection on the com side.
  74     UAVTalkConnection telemUAVTalkCon;
  75     UAVTalkConnection radioUAVTalkCon;
  76
  77     // Queue handles.
  78     xQueueHandle uavtalkEventQueue;
  79     xQueueHandle radioEventQueue;
  80
  81     // Error statistics.
  82     uint32_t     telemetryTxRetries;
  83     uint32_t     radioTxRetries;
  84
  85     // Is this modem the coordinator
  86     bool isCoordinator;
  87 } RadioComBridgeData;
  88
  89 // ****************
  90 // Private functions
  91
  92 static void telemetryTxTask(void *parameters);
  93 static void telemetryRxTask(void *parameters);
  94 static void telemRadioTxTask(void *parameters);
  95 static void telemRadioRxTask(void *parameters);
  96 static void PPMInputTask(void *parameters);
  97 static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length);
  98 static int32_t RadioSendHandler(uint8_t *buf, int32_t length);
  99 static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t count);
 100 static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t count);
 101 static void objectPersistenceUpdatedCb(UAVObjEvent *objEv);
 102 static void registerObject(UAVObjHandle obj);
 103
 104 // ****************
 105 // Private variables
 106
 107 static RadioComBridgeData *data;
 108
 109 /**
 110  * @brief Start the module
 111  *
 112  * @return -1 if initialisation failed, 0 on success
 113  */
 114 static int32_t RadioComBridgeStart(void)
 115 {
 116     if (data) {
 117         // Get the settings.
 118         OPLinkSettingsData oplinkSettings;
 119         OPLinkSettingsGet(&oplinkSettings);
 120
 121         // Check if this is the coordinator modem
 122         data->isCoordinator = (oplinkSettings.Protocol == OPLINKSETTINGS_PROTOCOL_OPLINKCOORDINATOR);
 123
 124         // Set the maximum radio RF power.
 125         switch (oplinkSettings.MaxRFPower) {
 126         case OPLINKSETTINGS_MAXRFPOWER_125:
 127             PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0);
 128             break;
 129         case OPLINKSETTINGS_MAXRFPOWER_16:
 130             PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1);
 131             break;
 132         case OPLINKSETTINGS_MAXRFPOWER_316:
 133             PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2);
 134             break;
 135         case OPLINKSETTINGS_MAXRFPOWER_63:
 136             PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3);
 137             break;
 138         case OPLINKSETTINGS_MAXRFPOWER_126:
 139             PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4);
 140             break;
 141         case OPLINKSETTINGS_MAXRFPOWER_25:
 142             PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5);
 143             break;
 144         case OPLINKSETTINGS_MAXRFPOWER_50:
 145             PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6);
 146             break;
 147         case OPLINKSETTINGS_MAXRFPOWER_100:
 148             PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7);
 149             break;
 150         default:
 151             // do nothing
 152             break;
 153         }
 154
 155         // Configure our UAVObjects for updates.
 156         UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL);
 157         if (data->isCoordinator) {
 158             UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->radioEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
 159         } else {
 160             UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
 161         }
 162         registerObject(OPLinkStatusHandle());
 163
 164         if (data->isCoordinator) {
 165             registerObject(RadioComBridgeStatsHandle());
 166         }
 167
 168         // Configure the UAVObject callbacks
 169         ObjectPersistenceConnectCallback(&objectPersistenceUpdatedCb);
 170
 171         // Start the primary tasks for receiving/sending UAVTalk packets from the GCS.
 172         // These tasks are always needed at least for configuration over HID.
 173         xTaskCreate(telemetryTxTask, "telemetryTxTask", TELEM_STACK_SIZE_WORDS, NULL, TASK_PRIORITY, &(data->telemetryTxTaskHandle));
 174         xTaskCreate(telemetryRxTask, "telemetryRxTask", TELEM_STACK_SIZE_WORDS, NULL, TASK_PRIORITY, &(data->telemetryRxTaskHandle));
 175 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMETRYTX)
 176         PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRYTX);
 177 #endif
 178 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMETRYRX)
 179         PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRYRX);
 180 #endif
 181         // Start the tasks for sending/receiving telemetry over a radio link if a telemetry (GCS) link configured.
 182         if (PIOS_COM_GCS_OUT) {
 183             xTaskCreate(telemRadioTxTask, "telemRadioTxTask", TELEM_STACK_SIZE_WORDS, NULL, TASK_PRIORITY, &(data->telemRadioTxTaskHandle));
 184             xTaskCreate(telemRadioRxTask, "telemRadioRxTask", TELEM_STACK_SIZE_WORDS, NULL, TASK_PRIORITY, &(data->telemRadioRxTaskHandle));
 185 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMRADIOTX)
 186             PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMRADIOTX);
 187 #endif
 188 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMRADIORX)
 189             PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMRADIORX);
 190 #endif
 191         }
 192
 193         // Start the task for reading and processing PPM data if it's configured.
 194         if (PIOS_PPM_RECEIVER) {
 195             xTaskCreate(PPMInputTask, "PPMInputTask", PPM_STACK_SIZE_WORDS, NULL, TASK_PRIORITY, &(data->PPMInputTaskHandle));
 196 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_PPMINPUT)
 197             PIOS_WDG_RegisterFlag(PIOS_WDG_PPMINPUT);
 198 #endif
 199         }
 200
 201         return 0;
 202     }
 203     return -1;
 204 }
 205
 206 /**
 207  * @brief Initialise the module
 208  *
 209  * @return -1 if initialisation failed on success
 210  */
 211 static int32_t RadioComBridgeInitialize(void)
 212 {
 213     // allocate and initialize the static data storage only if module is enabled
 214     data = (RadioComBridgeData *)pios_malloc(sizeof(RadioComBridgeData));
 215     if (!data) {
 216         return -1;
 217     }
 218
 219     // Initialize the UAVObjects that we use
 220     OPLinkStatusInitialize();
 221     ObjectPersistenceInitialize();
 222     OPLinkReceiverInitialize();
 223     RadioComBridgeStatsInitialize();
 224
 225     // Initialise UAVTalk
 226     data->telemUAVTalkCon    = UAVTalkInitialize(&UAVTalkSendHandler);
 227     data->radioUAVTalkCon    = UAVTalkInitialize(&RadioSendHandler);
 228
 229     // Initialize the queues.
 230     data->uavtalkEventQueue  = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
 231     data->radioEventQueue    = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
 232
 233     // Initialize the statistics.
 234     data->telemetryTxRetries = 0;
 235     data->radioTxRetries     = 0;
 236
 237     return 0;
 238 }
 239 MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart);
 240
 241
 242 // TODO this code (badly) duplicates code from telemetry.c
 243 // This method should be used only for periodically updated objects.
 244 // The register method defined in telemetry.c should be factored out in a shared location so it can be
 245 // used from here...
 246 static void registerObject(UAVObjHandle obj)
 247 {
 248     // Setup object for periodic updates
 249     UAVObjEvent ev = {
 250         .obj    = obj,
 251         .instId = UAVOBJ_ALL_INSTANCES,
 252         .event  = EV_UPDATED_PERIODIC,
 253         .lowPriority = true,
 254     };
 255
 256     // Get metadata
 257     UAVObjMetadata metadata;
 258
 259     UAVObjGetMetadata(obj, &metadata);
 260
 261     EventPeriodicQueueCreate(&ev, data->uavtalkEventQueue, metadata.telemetryUpdatePeriod);
 262     UAVObjConnectQueue(obj, data->uavtalkEventQueue, EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
 263 }
 264
 265 /**
 266  * Update telemetry statistics
 267  */
 268 static void updateRadioComBridgeStats()
 269 {
 270     UAVTalkStats telemetryUAVTalkStats;
 271     UAVTalkStats radioUAVTalkStats;
 272     RadioComBridgeStatsData radioComBridgeStats;
 273
 274     // Get telemetry stats
 275     UAVTalkGetStats(data->telemUAVTalkCon, &telemetryUAVTalkStats, true);
 276
 277     // Get radio stats
 278     UAVTalkGetStats(data->radioUAVTalkCon, &radioUAVTalkStats, true);
 279
 280     // Get stats object data
 281     RadioComBridgeStatsGet(&radioComBridgeStats);
 282
 283     radioComBridgeStats.TelemetryTxRetries     = data->telemetryTxRetries;
 284     radioComBridgeStats.RadioTxRetries         = data->radioTxRetries;
 285
 286     // Update stats object
 287     radioComBridgeStats.TelemetryTxBytes      += telemetryUAVTalkStats.txBytes;
 288     radioComBridgeStats.TelemetryTxFailures   += telemetryUAVTalkStats.txErrors;
 289
 290     radioComBridgeStats.TelemetryRxBytes      += telemetryUAVTalkStats.rxBytes;
 291     radioComBridgeStats.TelemetryRxFailures   += telemetryUAVTalkStats.rxErrors;
 292     radioComBridgeStats.TelemetryRxSyncErrors += telemetryUAVTalkStats.rxSyncErrors;
 293     radioComBridgeStats.TelemetryRxCrcErrors  += telemetryUAVTalkStats.rxCrcErrors;
 294
 295     radioComBridgeStats.RadioTxBytes += radioUAVTalkStats.txBytes;
 296     radioComBridgeStats.RadioTxFailures       += radioUAVTalkStats.txErrors;
 297
 298     radioComBridgeStats.RadioRxBytes += radioUAVTalkStats.rxBytes;
 299     radioComBridgeStats.RadioRxFailures       += radioUAVTalkStats.rxErrors;
 300     radioComBridgeStats.RadioRxSyncErrors     += radioUAVTalkStats.rxSyncErrors;
 301     radioComBridgeStats.RadioRxCrcErrors      += radioUAVTalkStats.rxCrcErrors;
 302
 303     // Update stats object data
 304     RadioComBridgeStatsSet(&radioComBridgeStats);
 305 }
 306
 307 /**
 308  * @brief Reads the PPM input device and sends out OPLinkReceiver objects.
 309  *
 310  * @param[in] parameters  The task parameters (unused)
 311  */
 312 static void PPMInputTask(__attribute__((unused)) void *parameters)
 313 {
 314     xSemaphoreHandle sem = PIOS_RCVR_GetSemaphore(PIOS_PPM_RECEIVER, 1);
 315     int16_t channels[RFM22B_PPM_NUM_CHANNELS];
 316
 317     while (1) {
 318 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_PPMINPUT)
 319         PIOS_WDG_UpdateFlag(PIOS_WDG_PPMINPUT);
 320 #endif
 321
 322         // Wait for the receiver semaphore.
 323         if (xSemaphoreTake(sem, PPM_INPUT_TIMEOUT) == pdTRUE) {
 324             // Read the receiver inputs.
 325             for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) {
 326                 channels[i] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i + 1);
 327             }
 328         } else {
 329             // Failsafe
 330             for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) {
 331                 channels[i] = PIOS_RCVR_INVALID;
 332             }
 333         }
 334
 335         // Pass the channel values to the radio device.
 336         PIOS_RFM22B_PPMSet(pios_rfm22b_id, channels, RFM22B_PPM_NUM_CHANNELS);
 337     }
 338 }
 339
 340
 341 /****************************************************************************
 342 * Telemetry tasks and functions
 343 ****************************************************************************/
 344
 345 /**
 346  * @brief Receives events on the Radio->GCS telemetry stream and sends
 347  *        the requested object(s) to the GCS.
 348  *
 349  * @param[in] parameters  None.
 350  */
 351 static void telemetryTxTask(__attribute__((unused)) void *parameters)
 352 {
 353     UAVObjEvent ev;
 354
 355     // Loop forever
 356     while (1) {
 357 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMETRYTX)
 358         PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMETRYTX);
 359 #endif
 360         // Wait for queue message
 361         if (xQueueReceive(data->uavtalkEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
 362             if (ev.obj == RadioComBridgeStatsHandle()) {
 363                 updateRadioComBridgeStats();
 364             }
 365             // Send update (with retries)
 366             int32_t ret = -1;
 367             uint32_t retries = 0;
 368             while (retries <= MAX_RETRIES && ret == -1) {
 369                 ret = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS);
 370                 if (ret == -1) {
 371                     ++retries;
 372                 }
 373             }
 374             // Update stats
 375             data->telemetryTxRetries += retries;
 376         }
 377     }
 378 }
 379
 380 /**
 381  * @brief Receives events on the GCS->Radio telemetry stream and sends
 382  *        the requested object(s) over the radio telenetry stream.
 383  *
 384  * @param[in] parameters  none.
 385  */
 386 static void telemRadioTxTask(__attribute__((unused)) void *parameters)
 387 {
 388     // Task loop
 389     while (1) {
 390 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMRADIOTX)
 391         PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMRADIOTX);
 392 #endif
 393
 394         // Process the radio event queue, sending UAVObjects over the radio link as necessary.
 395         UAVObjEvent ev;
 396
 397         // Wait for queue message
 398         if (xQueueReceive(data->radioEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
 399             if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) {
 400                 // Send update (with retries)
 401                 int32_t ret = -1;
 402                 uint32_t retries = 0;
 403                 while (retries <= MAX_RETRIES && ret == -1) {
 404                     ret = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS);
 405                     if (ret == -1) {
 406                         ++retries;
 407                     }
 408                 }
 409                 data->radioTxRetries += retries;
 410             }
 411         }
 412     }
 413 }
 414
 415 /**
 416  * @brief Reads data from the radio telemetry port and processes it
 417  *        through the Radio->GCS telemetry stream.
 418  *
 419  * @param[in] parameters  The task parameters
 420  */
 421 static void telemRadioRxTask(__attribute__((unused)) void *parameters)
 422 {
 423     // Task loop
 424     while (1) {
 425 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMRADIORX)
 426         PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMRADIORX);
 427 #endif
 428         if (PIOS_COM_GCS_OUT) {
 429             uint8_t serial_data[16];
 430             uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_GCS_OUT, serial_data, sizeof(serial_data), MAX_PORT_DELAY);
 431             if (bytes_to_process > 0) {
 432                 // Pass the data through the UAVTalk parser.
 433                 ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data, bytes_to_process);
 434             }
 435         } else {
 436             vTaskDelay(5);
 437         }
 438     }
 439 }
 440
 441 /**
 442  * @brief Reads data from the GCS telemetry port and processes it
 443  *        through the GCS->Radio telemetry stream.
 444  *
 445  * @param[in] parameters  The task parameters
 446  */
 447 static void telemetryRxTask(__attribute__((unused)) void *parameters)
 448 {
 449     // Task loop
 450     while (1) {
 451         // uint32_t inputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0;
 452         uint32_t inputPort = PIOS_COM_GCS;
 453 #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_TELEMETRYRX)
 454         PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMETRYRX);
 455 #endif
 456 #if defined(PIOS_INCLUDE_USB)
 457         // Determine output port (USB takes priority over telemetry port)
 458         if (PIOS_USB_CheckAvailable(0) && PIOS_COM_HID) {
 459             inputPort = PIOS_COM_HID;
 460         }
 461 #endif /* PIOS_INCLUDE_USB */
 462         if (inputPort) {
 463             uint8_t serial_data[16];
 464             uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), MAX_PORT_DELAY);
 465             if (bytes_to_process > 0) {
 466                 ProcessTelemetryStream(data->telemUAVTalkCon, data->radioUAVTalkCon, serial_data, bytes_to_process);
 467             }
 468         } else {
 469             vTaskDelay(5);
 470         }
 471     }
 472 }
 473
 474 /**
 475  * @brief Send data from the Radio->GCS telemetry stream to the GCS port.
 476  *
 477  * @param[in] buf Data buffer to send
 478  * @param[in] length Length of buffer
 479  * @return -1 on failure
 480  * @return number of bytes transmitted on success
 481  */
 482 static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
 483 {
 484     int32_t ret;
 485     uint32_t outputPort = PIOS_COM_GCS;
 486
 487 #if defined(PIOS_INCLUDE_USB)
 488     // Determine output port (USB takes priority over telemetry port)
 489     if (PIOS_COM_HID && PIOS_COM_Available(PIOS_COM_HID)) {
 490         outputPort = PIOS_COM_HID;
 491     }
 492 #endif /* PIOS_INCLUDE_USB */
 493     if (outputPort) {
 494         // Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
 495         // It is the caller responsibility to retry in such cases...
 496         ret = -2;
 497         uint8_t count = 5;
 498         while (count-- > 0 && ret < -1) {
 499             ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
 500         }
 501     } else {
 502         ret = -1;
 503     }
 504     return ret;
 505 }
 506
 507 /**
 508  * @brief Send data from the GCS telemetry stream to the Radio->GCS port.
 509  *
 510  * @param[in] buf Data buffer to send
 511  * @param[in] length Length of buffer
 512  * @return -1 on failure
 513  * @return number of bytes transmitted on success
 514  */
 515 static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
 516 {
 517     uint32_t outputPort = PIOS_COM_GCS_OUT;
 518
 519     // Don't send any data unless the radio port is available.
 520     if (outputPort && PIOS_COM_Available(outputPort)) {
 521         // Following call can fail with -2 error code (buffer full) or -3 error code (could not acquire send mutex)
 522         // It is the caller responsibility to retry in such cases...
 523         int32_t ret   = -2;
 524         uint8_t count = 5;
 525         while (count-- > 0 && ret < -1) {
 526             ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
 527         }
 528         return ret;
 529     } else {
 530         return -1;
 531     }
 532 }
 533
 534 /**
 535  * @brief Process a byte of data received on the telemetry stream
 536  *
 537  * @param[in] inConnectionHandle  The UAVTalk connection handle on the telemetry port
 538  * @param[in] outConnectionHandle  The UAVTalk connection handle on the radio port.
 539  * @param[in] rxbyte  The received byte.
 540  */
 541 static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t length)
 542 {
 543     uint8_t position = 0;
 544
 545     // Keep reading until we receive a completed packet.
 546     while (position < length) {
 547         UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbuffer, length, &position);
 548         if (state == UAVTALK_STATE_COMPLETE) {
 549             // We only want to unpack certain telemetry objects
 550             uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
 551             switch (objId) {
 552             case OPLINKSTATUS_OBJID:
 553             case OPLINKSETTINGS_OBJID:
 554             case MetaObjectId(OPLINKSTATUS_OBJID):
 555             case MetaObjectId(OPLINKSETTINGS_OBJID):
 556                 UAVTalkReceiveObject(inConnectionHandle);
 557                 break;
 558             case OBJECTPERSISTENCE_OBJID:
 559             case MetaObjectId(OBJECTPERSISTENCE_OBJID):
 560                 // receive object locally
 561                 // some objects will send back a response to telemetry
 562                 // FIXME:
 563                 // OPLM will ack or nack all objects requests and acked object sends
 564                 // Receiver will probably also ack / nack the same messages
 565                 // This has some consequences like :
 566                 // Second ack/nack will not match an open transaction or will apply to wrong transaction
 567                 // Question : how does GCS handle receiving the same object twice
 568                 // The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks...
 569                 UAVTalkReceiveObject(inConnectionHandle);
 570                 // relay packet to remote modem
 571                 UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
 572                 break;
 573             default:
 574                 // all other packets are relayed to the remote modem
 575                 UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
 576                 break;
 577             }
 578         }
 579     }
 580 }
 581
 582 /**
 583  * @brief Process a byte of data received on the radio data stream.
 584  *
 585  * @param[in] inConnectionHandle  The UAVTalk connection handle on the radio port.
 586  * @param[in] outConnectionHandle  The UAVTalk connection handle on the telemetry port.
 587  * @param[in] rxbuffer  The received buffer.
 588  * @param[in] length  buffer length
 589  */
 590 static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t *rxbuffer, uint8_t length)
 591 {
 592     uint8_t position = 0;
 593
 594     // Keep reading until we receive a completed packet.
 595     while (position < length) {
 596         UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbuffer, length, &position);
 597         if (state == UAVTALK_STATE_COMPLETE) {
 598             // We only want to unpack certain objects from the remote modem
 599             // Similarly we only want to relay certain objects to the telemetry port
 600             uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
 601             switch (objId) {
 602             case OPLINKSTATUS_OBJID:
 603             case OPLINKSETTINGS_OBJID:
 604             case MetaObjectId(OPLINKSTATUS_OBJID):
 605             case MetaObjectId(OPLINKSETTINGS_OBJID):
 606                 // Ignore object...
 607                 // These objects are shadowed by the modem and are not transmitted to the telemetry port
 608                 // - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead
 609                 // - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead
 610                 break;
 611             case OPLINKRECEIVER_OBJID:
 612             case MetaObjectId(OPLINKRECEIVER_OBJID):
 613                 // Receive object locally
 614                 UAVTalkReceiveObject(inConnectionHandle);
 615                 // Also send the packet to the telemetry point.
 616                 UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
 617                 break;
 618             default:
 619                 // all other packets are relayed to the telemetry port
 620                 UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
 621                 break;
 622             }
 623         }
 624     }
 625 }
 626
 627 /**
 628  * @brief Callback that is called when the ObjectPersistence UAVObject is changed.
 629  * @param[in] objEv  The event that precipitated the callback.
 630  */
 631 static void objectPersistenceUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
 632 {
 633     // Get the ObjectPersistence object.
 634     ObjectPersistenceData obj_per;
 635
 636     ObjectPersistenceGet(&obj_per);
 637
 638     // Is this concerning our setting object?
 639     if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) {
 640         // Is this a save, load, or delete?
 641         bool success = false;
 642         switch (obj_per.Operation) {
 643         case OBJECTPERSISTENCE_OPERATION_LOAD:
 644         {
 645 #if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS)
 646             // Load the settings.
 647             void *obj = UAVObjGetByID(obj_per.ObjectID);
 648             if (obj == 0) {
 649                 success = false;
 650             } else {
 651                 // Load selected instance
 652                 success = (UAVObjLoad(obj, obj_per.InstanceID) == 0);
 653             }
 654 #endif
 655             break;
 656         }
 657         case OBJECTPERSISTENCE_OPERATION_SAVE:
 658         {
 659 #if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS)
 660             void *obj = UAVObjGetByID(obj_per.ObjectID);
 661             if (obj == 0) {
 662                 success = false;
 663             } else {
 664                 // Save selected instance
 665                 success = UAVObjSave(obj, obj_per.InstanceID) == 0;
 666             }
 667 #endif
 668             break;
 669         }
 670         case OBJECTPERSISTENCE_OPERATION_DELETE:
 671         {
 672 #if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS)
 673             void *obj = UAVObjGetByID(obj_per.ObjectID);
 674             if (obj == 0) {
 675                 success = false;
 676             } else {
 677                 // Save selected instance
 678                 success = UAVObjDelete(obj, obj_per.InstanceID) == 0;
 679             }
 680 #endif
 681             break;
 682         }
 683         default:
 684             break;
 685         }
 686         if (success == true) {
 687             obj_per.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED;
 688             ObjectPersistenceSet(&obj_per);
 689         }
 690     }
 691 }