AP_Periph: add support for DroneCAN RCInput packets

Tools/AP_Periph/AP_Periph.cpp

@@ -163,6 +163,10 @@ void AP_Periph_FW::init()
     battery.lib.init();
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_RCIN
+    rcin_init();
+#endif
+
 #if defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_RC_OUT)
     hal.rcout->init();
 #endif
@@ -457,6 +461,10 @@ void AP_Periph_FW::update()
     }
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_RCIN
+    rcin_update();
+#endif
+
     static uint32_t fiftyhz_last_update_ms;
     if (now - fiftyhz_last_update_ms >= 20) {
         // update at 50Hz

Tools/AP_Periph/AP_Periph.h

@@ -32,6 +32,7 @@
 #if HAL_WITH_ESC_TELEM
 #include <AP_ESC_Telem/AP_ESC_Telem.h>
 #endif
+#include <AP_RCProtocol/AP_RCProtocol_config.h>
 
 #include <AP_NMEA_Output/AP_NMEA_Output.h>
 #if HAL_NMEA_OUTPUT_ENABLED && !(HAL_GCS_ENABLED && defined(HAL_PERIPH_ENABLE_GPS))
@@ -278,6 +279,15 @@ public:
     void rcout_handle_safety_state(uint8_t safety_state);
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_RCIN
+    void rcin_init();
+    void rcin_update();
+    void can_send_RCInput(uint8_t quality, uint16_t *values, uint8_t nvalues, bool in_failsafe, bool quality_valid);
+    bool rcin_initialised;
+    uint32_t rcin_last_sent_RCInput_ms;
+    const char *rcin_rc_protocol;  // protocol currently being decoded
+#endif
+
 #if AP_TEMPERATURE_SENSOR_ENABLED
     AP_TemperatureSensor temperature_sensor;
 #endif

Tools/AP_Periph/Parameters.cpp

@@ -16,6 +16,10 @@ extern const AP_HAL::HAL &hal;
 #define AP_PERIPH_RANGEFINDER_PORT_DEFAULT 3
 #endif
 
+#ifndef AP_PERIPH_RC1_PORT_DEFAULT
+#define AP_PERIPH_RC1_PORT_DEFAULT -1
+#endif
+
 #ifndef HAL_PERIPH_GPS_PORT_DEFAULT
 #define HAL_PERIPH_GPS_PORT_DEFAULT 3
 #endif
@@ -563,6 +567,35 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GOBJECT(rpm_sensor, "RPM", AP_RPM),
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_RCIN
+    // RC_PROTOCOLS copied from RC_Channel/RC_Channels_Varinfo.h
+    // @Param: RC_PROTOCOLS
+    // @DisplayName: RC protocols enabled
+    // @Description: Bitmask of enabled RC protocols. Allows narrowing the protocol detection to only specific types of RC receivers which can avoid issues with incorrect detection. Set to 1 to enable all protocols.
+    // @User: Advanced
+    // @Bitmask: 0:All,1:PPM,2:IBUS,3:SBUS,4:SBUS_NI,5:DSM,6:SUMD,7:SRXL,8:SRXL2,9:CRSF,10:ST24,11:FPORT,12:FPORT2,13:FastSBUS
+    GSCALAR(rcin_protocols, "RC_PROTOCOLS", 1),
+
+    // RC_PROTOCOLS copied from RC_Channel/RC_Channels_Varinfo.h
+    // @Param: RC_MSGRATE
+    // @DisplayName: DroneCAN RC Message rate
+    // @Description: Rate at which RC input is sent via DroneCAN
+    // @User: Advanced
+    // @Increment: 1
+    // @Range: 0 255
+    // @Units: Hz
+    GSCALAR(rcin_rate_hz, "RC_MSGRATE", 50),
+
+    // @Param: RC1_PORT
+    // @DisplayName: RC input port
+    // @Description: This is the serial port number where SERIALx_PROTOCOL will be set to RC input.
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    // @RebootRequired: True
+    GSCALAR(rcin1_port, "RC1_PORT", AP_PERIPH_RC1_PORT_DEFAULT),
+#endif  // HAL_PERIPH_ENABLE_RCIN
+
     AP_VAREND
 };
 

Tools/AP_Periph/Parameters.h

@@ -78,6 +78,9 @@ public:
         k_param_esc_serial_port1,
         k_param_networking,
         k_param_rpm_sensor,
+        k_param_rcin_protocols,
+        k_param_rcin_rate_hz,
+        k_param_rcin1_port,
     };
 
     AP_Int16 format_version;
@@ -111,6 +114,12 @@ public:
     AP_Int16 rangefinder_max_rate;
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_RCIN
+    AP_Int32 rcin_protocols;
+    AP_Int8 rcin_rate_hz;
+    AP_Int8 rcin1_port;
+#endif
+
 #if HAL_PROXIMITY_ENABLED
     AP_Int32 proximity_baud;
     AP_Int8 proximity_port;

Tools/AP_Periph/can.cpp

@@ -2771,7 +2771,6 @@ void AP_Periph_FW::can_efi_update(void)
 }
 #endif // HAL_PERIPH_ENABLE_EFI
 
-
 // printf to CAN LogMessage for debugging
 void can_printf(const char *fmt, ...)
 {

Tools/AP_Periph/rc_in.cpp

@@ -0,0 +1,126 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_RCProtocol/AP_RCProtocol_config.h>
+
+#ifdef HAL_PERIPH_ENABLE_RCIN
+
+#include <AP_RCProtocol/AP_RCProtocol.h>
+#include "AP_Periph.h"
+#include <dronecan_msgs.h>
+
+extern const AP_HAL::HAL &hal;
+
+void AP_Periph_FW::rcin_init()
+{
+    if (g.rcin1_port == 0) {
+        return;
+    }
+
+    // init uart for serial RC
+    auto *uart = hal.serial(g.rcin1_port);
+    if (uart == nullptr) {
+        return;
+    }
+
+    serial_manager.set_protocol_and_baud(
+        g.rcin1_port,
+        AP_SerialManager::SerialProtocol_RCIN,
+        115200  // baud doesn't matter; RC Protocol autobauds
+        );
+
+    auto &rc = AP::RC();
+    rc.init();
+    rc.set_rc_protocols(g.rcin_protocols);
+    rc.add_uart(uart);
+
+    rcin_initialised = true;
+}
+
+void AP_Periph_FW::rcin_update()
+{
+    if (!rcin_initialised) {
+        return;
+    }
+
+    auto &rc = AP::RC();
+    if (!rc.new_input()) {
+        return;
+    }
+
+    // log discovered protocols:
+    auto new_rc_protocol = rc.protocol_name();
+    if (new_rc_protocol != rcin_rc_protocol) {
+        can_printf("Decoding (%s)", new_rc_protocol);
+        rcin_rc_protocol = new_rc_protocol;
+    }
+
+    // decimate the input to a parameterized rate
+    const uint8_t rate_hz = g.rcin_rate_hz;
+    if (rate_hz == 0) {
+        return;
+    }
+
+    const auto now_ms = AP_HAL::millis();
+    const auto interval_ms = 1000U / rate_hz;
+    if (now_ms - rcin_last_sent_RCInput_ms < interval_ms) {
+        return;
+    }
+    rcin_last_sent_RCInput_ms = now_ms;
+
+    // extract data and send CAN packet:
+    const uint8_t num_channels = rc.num_channels();
+    uint16_t channels[MAX_RCIN_CHANNELS];
+    rc.read(channels, num_channels);
+    const int16_t rssi = rc.get_RSSI();
+
+    can_send_RCInput((uint8_t)rssi, channels, num_channels, rc.failsafe_active(), rssi > 0 && rssi <256);
+}
+
+/*
+  send an RCInput CAN message
+ */
+void AP_Periph_FW::can_send_RCInput(uint8_t quality, uint16_t *values, uint8_t nvalues, bool in_failsafe, bool quality_valid)
+{
+    uint16_t status = 0;
+    if (quality_valid) {
+        status |= DRONECAN_SENSORS_RC_RCINPUT_STATUS_QUALITY_VALID;
+    }
+    if (in_failsafe) {
+        status |= DRONECAN_SENSORS_RC_RCINPUT_STATUS_FAILSAFE;
+    }
+
+    // assemble packet
+    dronecan_sensors_rc_RCInput pkt {};
+    pkt.quality = quality;
+    pkt.status = status;
+    pkt.rcin.len = nvalues;
+    for (uint8_t i=0; i<nvalues; i++) {
+        pkt.rcin.data[i] = values[i];
+    }
+
+    // encode and send message:
+    uint8_t buffer[DRONECAN_SENSORS_RC_RCINPUT_MAX_SIZE] {};
+
+    uint16_t total_size = dronecan_sensors_rc_RCInput_encode(&pkt, buffer, !periph.canfdout());
+
+    canard_broadcast(DRONECAN_SENSORS_RC_RCINPUT_SIGNATURE,
+                     DRONECAN_SENSORS_RC_RCINPUT_ID,
+                     CANARD_TRANSFER_PRIORITY_HIGH,
+                     buffer,
+                     total_size);
+}
+
+#endif  // HAL_PERIPH_ENABLE_RCIN

Tools/AP_Periph/wscript

@@ -72,6 +72,7 @@ def build(bld):
                      'AP_CheckFirmware',
                      'AP_RPM',
                      'AP_Proximity',
+                     'AP_RCProtocol',
                      ]
     bld.ap_stlib(
         name= 'AP_Periph_libs',