Merged in webbbn/librepilot/LP-548-output-oplinkreceiver-objects-fro (pull request #461)

LP-548 output oplinkreceiver objects from OPLink

Approved-by: Philippe Renon <philippe_renon@yahoo.fr>
Approved-by: Vladimir Zidar <mr_w@mindnever.org>
Approved-by: Lalanne Laurent <f5soh@free.fr>
Approved-by: Brian Webb <webbbn@gmail.com>

Author: webbbn

flight/modules/RadioComBridge/RadioComBridge.c

@@ -551,10 +551,8 @@ static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalk
             switch (objId) {
             case OPLINKSTATUS_OBJID:
             case OPLINKSETTINGS_OBJID:
-            case OPLINKRECEIVER_OBJID:
             case MetaObjectId(OPLINKSTATUS_OBJID):
             case MetaObjectId(OPLINKSETTINGS_OBJID):
-            case MetaObjectId(OPLINKRECEIVER_OBJID):
                 UAVTalkReceiveObject(inConnectionHandle);
                 break;
             case OBJECTPERSISTENCE_OBJID:
@@ -613,10 +611,9 @@ static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConn
             case OPLINKRECEIVER_OBJID:
             case MetaObjectId(OPLINKRECEIVER_OBJID):
                 // Receive object locally
-                // These objects are received by the modem and are not transmitted to the telemetry port
-                // - OPLINKRECEIVER_OBJID : not sure why
-                // some objects will send back a response to the remote modem
                 UAVTalkReceiveObject(inConnectionHandle);
+                // Also send the packet to the telemetry point.
+                UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
                 break;
             default:
                 // all other packets are relayed to the telemetry port

flight/modules/UAVOMSPBridge/UAVOMSPBridge.c

@@ -43,7 +43,7 @@
 #include "gpspositionsensor.h"
 #include "manualcontrolcommand.h"
 #include "manualcontrolsettings.h"
-#include "oplinkstatus.h"
+#include "oplinkreceiver.h"
 #include "accessorydesired.h"
 #include "attitudestate.h"
 #include "airspeedstate.h"
@@ -481,7 +481,7 @@ static void msp_send_analog(struct msp_bridge *m)
 #ifdef PIOS_INCLUDE_OPLINKRCVR
     if (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) {
         int8_t rssi;
-        OPLinkStatusRSSIGet(&rssi);
+        OPLinkReceiverRSSIGet(&rssi);
 
         // MSP values have no units, and OSD rssi display requires calibration anyway, so we will translate OPLINK_LOW_RSSI to OPLINK_HIGH_RSSI -> 0-1023
         if (rssi < OPLINK_LOW_RSSI) {

flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c

@@ -52,7 +52,7 @@
 #include "taskinfo.h"
 #include "mavlink.h"
 #include "hwsettings.h"
-#include "oplinkstatus.h"
+#include "oplinkreceiver.h"
 #include "receiverstatus.h"
 #include "manualcontrolsettings.h"
 
@@ -250,7 +250,7 @@ static void mavlink_send_rc_channels()
 #ifdef PIOS_INCLUDE_OPLINKRCVR
     if (channelGroups.Throttle == MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK) {
         int8_t rssi;
-        OPLinkStatusRSSIGet(&rssi);
+        OPLinkReceiverRSSIGet(&rssi);
 
         if (rssi < OPLINK_LOW_RSSI) {
             rssi = OPLINK_LOW_RSSI;

flight/pios/common/pios_board_io.c

@@ -54,14 +54,16 @@
 # ifdef PIOS_INCLUDE_GCSRCVR
 #  include <pios_gcsrcvr_priv.h>
 # endif
+# ifdef PIOS_INCLUDE_OPLINKRCVR
+#  include <oplinkreceiver.h>
+#  include <pios_oplinkrcvr_priv.h>
+# endif
 #endif /* PIOS_INCLUDE_RCVR */
 
 #ifdef PIOS_INCLUDE_RFM22B
 # include <oplinksettings.h>
 # include <oplinkstatus.h>
 # ifdef PIOS_INCLUDE_RCVR
-#  include <oplinkreceiver.h>
-#  include <pios_oplinkrcvr_priv.h>
 #  include <pios_openlrs.h>
 #  include <pios_openlrs_rcvr_priv.h>
 # endif /* PIOS_INCLUDE_RCVR */
@@ -525,7 +527,7 @@ void PIOS_BOARD_IO_Configure_PPM_RCVR(const struct pios_ppm_cfg *ppm_cfg)
 }
 #endif /* PIOS_INCLUDE_PPM */
 
-#ifdef PIOS_INCLUDE_GCSRCVR
+#if defined(PIOS_INCLUDE_GCSRCVR)
 void PIOS_BOARD_IO_Configure_GCS_RCVR()
 {
     GCSReceiverInitialize();
@@ -539,6 +541,24 @@ void PIOS_BOARD_IO_Configure_GCS_RCVR()
 }
 #endif /* PIOS_INCLUDE_GCSRCVR */
 
+#if defined(PIOS_INCLUDE_OPLINKRCVR) && defined(PIOS_INCLUDE_RCVR)
+void PIOS_BOARD_IO_Configure_OPLink_RCVR()
+{
+    uint32_t pios_oplinkrcvr_id;
+    OPLinkReceiverInitialize();
+#if defined(PIOS_INCLUDE_RFM22B)
+    PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id, pios_rfm22b_id);
+#else /* PIOS_INCLUDE_RFM22B */
+    PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id);
+#endif /* PIOS_INCLUDE_RFM22B */
+    uint32_t pios_oplinkrcvr_rcvr_id;
+    if (PIOS_RCVR_Init(&pios_oplinkrcvr_rcvr_id, &pios_oplinkrcvr_rcvr_driver, pios_oplinkrcvr_id)) {
+        PIOS_Assert(0);
+    }
+    pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_oplinkrcvr_rcvr_id;
+}
+#endif /* PIOS_INCLUDE_OPLINKRCVR && PIOS_INCLUDE_RCVR */
+
 #ifdef PIOS_INCLUDE_RFM22B
 
 void PIOS_BOARD_IO_Configure_RFM22B()
@@ -596,15 +616,6 @@ void PIOS_BOARD_IO_Configure_RFM22B()
             if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_SPI_RFM22B_ADAPTER, rfm22b_cfg->slave_num, rfm22b_cfg, oplinkSettings.RFBand)) {
                 PIOS_Assert(0);
             }
-#if defined(PIOS_INCLUDE_OPLINKRCVR) && defined(PIOS_INCLUDE_RCVR)
-            uint32_t pios_oplinkrcvr_id;
-            PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id, pios_rfm22b_id);
-            uint32_t pios_oplinkrcvr_rcvr_id;
-            if (PIOS_RCVR_Init(&pios_oplinkrcvr_rcvr_id, &pios_oplinkrcvr_rcvr_driver, pios_oplinkrcvr_id)) {
-                PIOS_Assert(0);
-            }
-            pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_oplinkrcvr_rcvr_id;
-#endif /* PIOS_INCLUDE_OPLINKRCVR && PIOS_INCLUDE_RCVR */
 
             /* Configure the radio com interface */
             if (PIOS_COM_Init(&pios_com_pri_radio_id, &pios_rfm22b_com_driver, pios_rfm22b_id,

flight/pios/common/pios_oplinkrcvr.c

@@ -37,6 +37,8 @@
 #include <oplinkstatus.h>
 #include <pios_oplinkrcvr_priv.h>
 
+// Put receiver in failsafe if not updated within timeout
+#define PIOS_OPLINK_RCVR_TIMEOUT_MS  100
 
 /* Provide a RCVR driver */
 static int32_t PIOS_OPLinkRCVR_Get(uint32_t rcvr_id, uint8_t channel);
@@ -60,11 +62,14 @@ struct pios_oplinkrcvr_dev {
     bool    Fresh;
 };
 
+static struct pios_oplinkrcvr_dev *global_oplinkrcvr_dev;
+
 static bool PIOS_oplinkrcvr_validate(struct pios_oplinkrcvr_dev *oplinkrcvr_dev)
 {
     return oplinkrcvr_dev->magic == PIOS_OPLINKRCVR_DEV_MAGIC;
 }
 
+#if defined(PIOS_INCLUDE_RFM22B)
 static void PIOS_oplinkrcvr_ppm_callback(uint32_t oplinkrcvr_id, const int16_t *channels)
 {
     /* Recover our device context */
@@ -78,10 +83,21 @@ static void PIOS_oplinkrcvr_ppm_callback(uint32_t oplinkrcvr_id, const int16_t *
     for (uint8_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; ++i) {
         oplinkrcvr_dev->oplinkreceiverdata.Channel[i] = (i < RFM22B_PPM_NUM_CHANNELS) ? channels[i] : PIOS_RCVR_TIMEOUT;
     }
+
+    // Update the RSSI and quality fields.
+    int8_t rssi;
+    OPLinkStatusRSSIGet(&rssi);
+    oplinkrcvr_dev->oplinkreceiverdata.RSSI = rssi;
+    uint16_t quality;
+    OPLinkStatusLinkQualityGet(&quality);
+    // Link quality is 0-128, so scale it down to 0-100
+    oplinkrcvr_dev->oplinkreceiverdata.LinkQuality = quality * 100 / 128;
+
     OPLinkReceiverSet(&oplinkrcvr_dev->oplinkreceiverdata);
 
     oplinkrcvr_dev->Fresh = true;
 }
+#endif /* PIOS_INCLUDE_RFM22B */
 
 #if defined(PIOS_INCLUDE_FREERTOS)
 static struct pios_oplinkrcvr_dev *PIOS_oplinkrcvr_alloc(void)
@@ -97,6 +113,9 @@ static struct pios_oplinkrcvr_dev *PIOS_oplinkrcvr_alloc(void)
     oplinkrcvr_dev->Fresh = false;
     oplinkrcvr_dev->supv_timer = 0;
 
+    /* The update callback cannot receive the device pointer, so set it in a global */
+    global_oplinkrcvr_dev = oplinkrcvr_dev;
+
     return oplinkrcvr_dev;
 }
 #else
@@ -119,7 +138,21 @@ static struct pios_oplinkrcvr_dev *PIOS_oplinkrcvr_alloc(void)
 }
 #endif /* if defined(PIOS_INCLUDE_FREERTOS) */
 
+static void oplinkreceiver_updated(UAVObjEvent *ev)
+{
+    struct pios_oplinkrcvr_dev *oplinkrcvr_dev = global_oplinkrcvr_dev;
+
+    if (ev->obj == OPLinkReceiverHandle()) {
+        OPLinkReceiverGet(&oplinkrcvr_dev->oplinkreceiverdata);
+        oplinkrcvr_dev->Fresh = true;
+    }
+}
+
+#if defined(PIOS_INCLUDE_RFM22B)
 extern int32_t PIOS_OPLinkRCVR_Init(uint32_t *oplinkrcvr_id, uint32_t rfm22b_id)
+#else
+extern int32_t PIOS_OPLinkRCVR_Init(uint32_t *oplinkrcvr_id)
+#endif
 {
     struct pios_oplinkrcvr_dev *oplinkrcvr_dev;
 
@@ -134,8 +167,13 @@ extern int32_t PIOS_OPLinkRCVR_Init(uint32_t *oplinkrcvr_id, uint32_t rfm22b_id)
         oplinkrcvr_dev->oplinkreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT;
     }
 
+#if defined(PIOS_INCLUDE_RFM22B)
     /* Register ppm callback */
     PIOS_RFM22B_SetPPMCallback(rfm22b_id, PIOS_oplinkrcvr_ppm_callback, (uint32_t)oplinkrcvr_dev);
+#endif
+
+    /* Updates could come over the telemetry channel, so register uavobj callback */
+    OPLinkReceiverConnectCallback(oplinkreceiver_updated);
 
     /* Register the failsafe timer callback. */
     if (!PIOS_RTC_RegisterTickCallback(PIOS_oplinkrcvr_Supervisor, (uint32_t)oplinkrcvr_dev)) {
@@ -199,14 +237,17 @@ static void PIOS_oplinkrcvr_Supervisor(uint32_t oplinkrcvr_id)
     oplinkrcvr_dev->Fresh = false;
 }
 
-static uint8_t PIOS_OPLinkRCVR_Quality_Get(__attribute__((unused)) uint32_t oplinkrcvr_id)
+static uint8_t PIOS_OPLinkRCVR_Quality_Get(uint32_t oplinkrcvr_id)
 {
-    uint16_t oplink_quality;
+    /* Recover our device context */
+    struct pios_oplinkrcvr_dev *oplinkrcvr_dev = (struct pios_oplinkrcvr_dev *)oplinkrcvr_id;
 
-    OPLinkStatusLinkQualityGet(&oplink_quality);
+    if (!PIOS_oplinkrcvr_validate(oplinkrcvr_dev)) {
+        /* Invalid device specified */
+        return 0;
+    }
 
-    /* link_status is in the range 0-128, so scale to a % */
-    return oplink_quality * 100 / 128;
+    return oplinkrcvr_dev->oplinkreceiverdata.LinkQuality;
 }
 
 #endif /* PIOS_INCLUDE_OPLINKRCVR */

flight/pios/common/pios_rfm22b.c

@@ -1798,6 +1798,7 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev)
     }
 
     // Should we append PPM data to the packet?
+    bool ppm_valid = false;
     if (radio_dev->ppm_send_mode) {
         len = RFM22B_PPM_NUM_CHANNELS + (radio_dev->ppm_only_mode ? 2 : 1);
 
@@ -1817,10 +1818,13 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev)
             if ((val == PIOS_RCVR_INVALID) || (val == PIOS_RCVR_TIMEOUT)) {
                 val = RFM22B_PPM_INVALID;
             } else if (val > RFM22B_PPM_MAX_US) {
+                ppm_valid = true;
                 val = RFM22B_PPM_MAX;
             } else if (val < RFM22B_PPM_MIN_US) {
+                ppm_valid = true;
                 val = RFM22B_PPM_MIN;
             } else {
+                ppm_valid = true;
                 val = (val - RFM22B_PPM_MIN_US) / RFM22B_PPM_SCALE + RFM22B_PPM_MIN;
             }
 
@@ -1844,6 +1848,7 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev)
     }
 
     // Append data from the com interface if applicable.
+    bool packet_data = false;
     if (!radio_dev->ppm_only_mode) {
         uint8_t newlen  = 0;
         bool need_yield = false;
@@ -1863,7 +1868,8 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev)
             i++;
         }
         if (newlen) {
-            *(p + len) = radio_dev->last_stream_sent;
+            packet_data = true;
+            *(p + len)  = radio_dev->last_stream_sent;
             len += newlen + 1;
         }
     }
@@ -1885,7 +1891,7 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev)
     }
 
     // Only count the packet if it contains valid data.
-    if (radio_dev->ppm_only_mode || (len > RS_ECC_NPARITY)) {
+    if (ppm_valid || packet_data) {
         TX_LED_ON;
         radio_dev->stats.tx_byte_count += len;
     }

flight/pios/inc/pios_board_io.h

@@ -268,4 +268,8 @@ void PIOS_BOARD_IO_Configure_RadioAuxStream(HwSettingsRadioAuxStreamOptions radi
 void PIOS_BOARD_IO_Configure_GCS_RCVR();
 #endif
 
+#ifdef PIOS_INCLUDE_OPLINKRCVR
+void PIOS_BOARD_IO_Configure_OPLink_RCVR();
+#endif
+
 #endif /* PIOS_BOARD_IO_H */

flight/pios/inc/pios_oplinkrcvr_priv.h

@@ -35,7 +35,11 @@
 
 extern const struct pios_rcvr_driver pios_oplinkrcvr_rcvr_driver;
 
+#if defined(PIOS_INCLUDE_RFM22B)
 extern int32_t PIOS_OPLinkRCVR_Init(uint32_t *oplinkrcvr_id, uint32_t rfm22b_id);
+#else
+extern int32_t PIOS_OPLinkRCVR_Init(uint32_t *oplinkrcvr_id);
+#endif
 
 #endif /* PIOS_OPLINKRCVR_PRIV_H */
 

flight/targets/boards/ccf3d/firmware/inc/pios_config.h

@@ -105,8 +105,8 @@
 #define PIOS_INCLUDE_SRXL
 #define PIOS_INCLUDE_HOTT
 #define PIOS_INCLUDE_IBUS
-/* #define PIOS_INCLUDE_GCSRCVR */
-/* #define PIOS_INCLUDE_OPLINKRCVR */
+#define PIOS_INCLUDE_GCSRCVR
+#define PIOS_INCLUDE_OPLINKRCVR
 
 /* PIOS abstract receiver interface */
 #define PIOS_INCLUDE_RCVR

flight/targets/boards/ccf3d/firmware/pios_board.c

@@ -251,10 +251,13 @@ void PIOS_Board_Init(void)
         break;
     }
 
+#ifdef PIOS_INCLUDE_GCSRCVR
+    PIOS_BOARD_IO_Configure_GCS_RCVR();
+#endif
 
-#if defined(PIOS_INCLUDE_GCSRCVR)
-    PIOS_BOARD_IO_Configure_GCSRCVR();
-#endif /* PIOS_INCLUDE_GCSRCVR */
+#ifdef PIOS_INCLUDE_OPLINKRCVR
+    PIOS_BOARD_IO_Configure_OPLink_RCVR();
+#endif
 
 #ifndef PIOS_ENABLE_DEBUG_PINS
     switch ((HwSettingsCC_RcvrPortOptions)hwsettings_rcvrport) {