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