AP_RCProtocol: add and use AP_RCProtocol_UDP_Packed16BitValues

libraries/AP_RCProtocol/AP_RCProtocol.cpp

@@ -36,6 +36,7 @@
 #include "AP_RCProtocol_GHST.h"
 #include "AP_RCProtocol_MAVLinkRadio.h"
 #include "AP_RCProtocol_Joystick_SFML.h"
+#include "AP_RCProtocol_UDP_Packed16BitValues.h"
 #include <AP_Math/AP_Math.h>
 #include <RC_Channel/RC_Channel.h>
 
@@ -96,6 +97,9 @@ void AP_RCProtocol::init()
 #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
     backend[AP_RCProtocol::JOYSTICK_SFML] = new AP_RCProtocol_Joystick_SFML(*this);
 #endif
+#if AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED
+    backend[AP_RCProtocol::UDP_PACKED16BITVALUES] = new AP_RCProtocol_UDP_Packed16BitValues(*this);
+#endif
 }
 
 AP_RCProtocol::~AP_RCProtocol()
@@ -444,6 +448,9 @@ bool AP_RCProtocol::new_input()
 #endif
 #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
         AP_RCProtocol::JOYSTICK_SFML,
+#endif
+#if AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED
+        AP_RCProtocol::UDP_PACKED16BITVALUES,
 #endif
     };
     for (const auto protocol : pollable) {
@@ -584,6 +591,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol)
 #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
     case JOYSTICK_SFML:
         return "SFML";
+#endif
+#if AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED
+    case UDP_PACKED16BITVALUES:
+        return "UDP-Packed16BitValues";
 #endif
     case NONE:
         break;

libraries/AP_RCProtocol/AP_RCProtocol.h

@@ -83,6 +83,9 @@ class AP_RCProtocol {
 #endif
 #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
         JOYSTICK_SFML = 16,
+#endif
+#if AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED
+        UDP_PACKED16BITVALUES = 17,
 #endif
         NONE    //last enum always is None
     };
@@ -173,6 +176,9 @@ class AP_RCProtocol {
 #endif
 #if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
         case JOYSTICK_SFML:
+#endif
+#if AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED
+        case UDP_PACKED16BITVALUES:
 #endif
         case NONE:
             return false;

libraries/AP_RCProtocol/AP_RCProtocol_UDP_Packed16BitValues.cpp

@@ -0,0 +1,169 @@
+#include "AP_RCProtocol_config.h"
+
+#if AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED
+
+#include "AP_RCProtocol_UDP_Packed16BitValues.h"
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Vehicle/AP_Vehicle_Type.h>
+#include <SITL/SITL.h>
+
+extern const AP_HAL::HAL& hal;
+
+void AP_RCProtocol_UDP_Packed16BitValues::set_default_pwm_input_values()
+{
+    pwm_input[0] = 1500;
+    pwm_input[1] = 1500;
+    pwm_input[2] = 1000;
+    pwm_input[3] = 1500;
+    pwm_input[4] = 1800;
+    pwm_input[5] = 1000;
+    pwm_input[6] = 1000;
+    pwm_input[7] = 1800;
+
+#if APM_BUILD_TYPE(APM_BUILD_Rover)
+    // set correct default throttle for rover (allowing for reverse)
+    pwm_input[2] = 1500;
+#elif APM_BUILD_TYPE(APM_BUILD_ArduSub) || APM_BUILD_TYPE(APM_BUILD_Blimp)
+    for(uint8_t i = 0; i < 8; i++) {
+        pwm_input[i] = 1500;
+    }
+#endif
+
+    num_channels = 8;
+}
+
+bool AP_RCProtocol_UDP_Packed16BitValues::init()
+{
+    const auto sitl = AP::sitl();
+    if (sitl == nullptr) {
+        return false;
+    }
+    if (!rc_in.reuseaddress()) {
+        return false;
+    }
+    if (!rc_in.bind("0.0.0.0", sitl->rcin_port)) {
+        return false;
+    }
+    if (!rc_in.set_blocking(false)) {
+        return false;
+    }
+    if (!rc_in.set_cloexec()) {
+        return false;
+    }
+
+    set_default_pwm_input_values();
+
+    return true;
+}
+
+void AP_RCProtocol_UDP_Packed16BitValues::update()
+{
+    if (!init_done) {
+        if (!init()) {
+            return;
+        }
+        init_done = true;
+    }
+
+    read_all_socket_input();
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    const auto sitl = AP::sitl();
+    if (sitl == nullptr) {
+        return;
+    }
+
+    if (sitl->rc_fail == SITL::SIM::SITL_RCFail_NoPulses) {
+        return;
+    }
+#endif
+
+    // simulate RC input at 50Hz
+    if (AP_HAL::millis() - last_input_ms < 20) {
+        return;
+    }
+    last_input_ms = AP_HAL::millis();
+
+    add_input(
+        num_channels,
+        pwm_input,
+        false,  // failsafe
+        0, // check me
+        0  // link quality
+        );
+}
+
+/*
+  check for a SITL RC input packet
+ */
+void AP_RCProtocol_UDP_Packed16BitValues::read_all_socket_input(void)
+{
+    struct pwm_packet {
+        uint16_t pwm[16];
+    } pwm_pkt;
+    uint8_t pwm_pkt_num_channels = 0;
+
+    ssize_t receive_size = 1;  // lies!
+    uint16_t count = 0;
+    while (receive_size > 0) {
+        receive_size = rc_in.recv(&pwm_pkt, sizeof(pwm_pkt), 0);
+
+        switch (receive_size) {
+        case -1:
+            break;
+        case 8*2:
+        case 16*2:
+            pwm_pkt_num_channels = receive_size/2;
+            break;
+        default:
+            fprintf(stderr, "Malformed SITL RC input (%ld)", (long)receive_size);
+            return;
+        }
+        count++;
+    }
+
+    if (count > 100) {
+        ::fprintf(stderr, "Read %u rc inputs\n", count);
+    }
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    const auto sitl = AP::sitl();
+    if (sitl == nullptr) {
+        return;
+    }
+
+    // convert last packet received into pwm values
+    switch (sitl->rc_fail) {
+    case SITL::SIM::SITL_RCFail_Throttle950:
+        // discard anything we just read from the "receiver" and set
+        // values to bind values:
+        for (uint8_t i=0; i<ARRAY_SIZE(pwm_input); i++) {
+            pwm_input[i] = 1500;  // centre all inputs
+        }
+        pwm_input[2] = 950;  // reset throttle (assumed to be on channel 3...)
+        return;
+    case SITL::SIM::SITL_RCFail_NoPulses:
+        // see also code in ::update
+        return;
+    case SITL::SIM::SITL_RCFail_None:
+        break;
+    }
+#endif
+
+    if (pwm_pkt_num_channels == 0) {
+        return;
+    }
+    for (uint8_t i=0; i<pwm_pkt_num_channels; i++) {
+        // setup the pwm input for the RC channel inputs
+        const uint16_t pwm = pwm_pkt.pwm[i];
+        if (pwm == 0) {
+            // 0 means "ignore this value"
+            continue;
+        }
+        pwm_input[i] = pwm;
+    }
+    num_channels = pwm_pkt_num_channels;  // or ARRAY_SIZE(pwm_input)?
+}
+
+#endif // AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED

libraries/AP_RCProtocol/AP_RCProtocol_UDP_Packed16BitValues.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include "AP_RCProtocol_config.h"
+
+#if AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED
+
+#include "AP_RCProtocol_Backend.h"
+
+#include <AP_Common/missing/endian.h>
+#include <AP_HAL/utility/Socket_native.h>
+
+class AP_RCProtocol_UDP_Packed16BitValues : public AP_RCProtocol_Backend {
+public:
+
+    using AP_RCProtocol_Backend::AP_RCProtocol_Backend;
+
+    void update() override;
+
+private:
+
+    bool init();
+    bool init_done;
+
+    uint32_t last_input_ms;
+
+    void read_all_socket_input(void);
+    SocketAPM_native rc_in{true};  // "true" means "datagram"
+
+    // these are the values that will be fed into the autopilot.
+    // Packets we receive usually only contain a subset of channel
+    // values to insert into here:
+    uint16_t pwm_input[16];
+    uint8_t num_channels;
+
+    void set_default_pwm_input_values();
+};
+
+
+#endif  // AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED

libraries/AP_RCProtocol/AP_RCProtocol_config.h

@@ -79,3 +79,6 @@
 #define AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024 && HAL_GCS_ENABLED
 #endif
 
+#ifndef AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED
+#define AP_RCPROTOCOL_UDP_PACKED16BITVALUES_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#endif