Create AP_RCProtocol_SITL

Tools/autotest/arduplane.py

@@ -1334,6 +1334,7 @@ def ThrottleFailsafe(self):
         self.reboot_sitl()
 
     def ThrottleFailsafeFence(self):
+
         '''Fly fence survives throttle failsafe'''
         fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
 

Tools/autotest/vehicle_test_suite.py

@@ -7822,6 +7822,12 @@ def assert_arm_failure(self, expected_statustext, timeout=5, ignore_prearm_failu
             if self.mav.motors_armed():
                 raise NotAchievedException("Armed when we shouldn't have")
 
+    def ready_to_arm(self):
+        '''returns true if the vehicle thinks it is ready to arm.  Note that
+        currently this implementation is *significantly* different to
+        wait_ready_to_arm'''
+        return self.sensor_has_state(mavutil.mavlink.MAV_SYS_STATUS_PREARM_CHECK, True, True, True)
+
     def wait_ready_to_arm(self, timeout=120, require_absolute=True, check_prearm_bit=True):
         # wait for EKF checks to pass
         self.progress("Waiting for ready to arm")
@@ -7839,6 +7845,7 @@ def wait_ready_to_arm(self, timeout=120, require_absolute=True, check_prearm_bit
         self.waiting_to_arm_count += 1
 
     def wait_heartbeat(self, drain_mav=True, quiet=False, *args, **x):
+
         '''as opposed to mav.wait_heartbeat, raises an exception on timeout.
 Also, ignores heartbeats not from our target system'''
         if drain_mav:

libraries/AP_HAL_SITL/HAL_SITL_Class.cpp

@@ -306,6 +306,11 @@ void HAL_SITL::actually_reboot()
     AP_HAL::panic("PANIC: REBOOT FAILED: %s", strerror(errno));
 }
 
+uint16_t *HAL_SITL::get_pwm_input() const
+{
+    return _sitl_state->pwm_input;
+}
+
 static HAL_SITL hal_sitl_inst;
 
 const AP_HAL::HAL& AP_HAL::get_HAL() {

libraries/AP_HAL_SITL/HAL_SITL_Class.h

@@ -41,6 +41,8 @@ class HAL_SITL : public AP_HAL::HAL {
     bool run_in_maintenance_mode() const;
 #endif
 
+    uint16_t *get_pwm_input() const;
+
 private:
     HALSITL::SITL_State *_sitl_state;
 
@@ -53,6 +55,7 @@ class HAL_SITL : public AP_HAL::HAL {
 
     // set to true if simulation is to wipe storage as it is opened:
     bool wipe_storage;
+
 };
 
 #if HAL_NUM_CAN_IFACES

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -25,6 +25,8 @@
 #include <SITL/SIM_JSBSim.h>
 #include <AP_HAL/utility/Socket_native.h>
 
+#include <AP_RCProtocol/AP_RCProtocol.h>
+
 extern const AP_HAL::HAL& hal;
 
 using namespace HALSITL;
@@ -242,6 +244,7 @@ void SITL_State::_check_rc_input(void)
 
 bool SITL_State::_read_rc_sitl_input()
 {
+
     struct pwm_packet {
         uint16_t pwm[16];
     } pwm_pkt;
@@ -266,24 +269,19 @@ bool SITL_State::_read_rc_sitl_input()
         return size != -1;  // we must continue to drain _sitl_rc
     }
 
+    // consider transforming the data in the packet into a different
+    // protocol here.
+
     switch (size) {
     case -1:
         return false;
     case 8*2:
     case 16*2: {
         // a packet giving the receiver PWM inputs
-        for (uint8_t i=0; i<size/2; i++) {
-            // setup the pwm input for the RC channel inputs
-            if (i < _sitl->state.rcin_chan_count) {
-                // we're using rc from simulator
-                continue;
-            }
-            uint16_t pwm = pwm_pkt.pwm[i];
-            if (pwm != 0) {
-                pwm_input[i] = pwm;
-            }
-        }
-        return true;
+        const uint32_t baudrate = 0;
+        AP::RC().process_bytes((uint8_t*)pwm_pkt.pwm, size, baudrate);
+
+        return false;
     }
     default:
         fprintf(stderr, "Malformed SITL RC input (%ld)", (long)size);

libraries/AP_RCProtocol/AP_RCProtocol.cpp

@@ -34,6 +34,7 @@
 #include "AP_RCProtocol_FPort2.h"
 #include "AP_RCProtocol_DroneCAN.h"
 #include "AP_RCProtocol_GHST.h"
+#include "AP_RCProtocol_SITL.h"
 #include <AP_Math/AP_Math.h>
 #include <RC_Channel/RC_Channel.h>
 
@@ -88,6 +89,9 @@ void AP_RCProtocol::init()
 #if AP_RCPROTOCOL_GHST_ENABLED
     backend[AP_RCProtocol::GHST] = new AP_RCProtocol_GHST(*this);
 #endif
+#if AP_RCPROTOCOL_SITL_ENABLED
+    backend[AP_RCProtocol::SITL] = new AP_RCProtocol_SITL(*this);
+#endif
 }
 
 AP_RCProtocol::~AP_RCProtocol()
@@ -199,6 +203,27 @@ void AP_RCProtocol::process_pulse_list(const uint32_t *widths, uint16_t n, bool
     }
 }
 
+void AP_RCProtocol::process_bytes(uint8_t *bytes, uint16_t count, uint32_t baudrate)
+{
+    if (_detected_protocol != AP_RCProtocol::NONE &&
+        !protocol_enabled(_detected_protocol)) {
+        _detected_protocol = AP_RCProtocol::NONE;
+    }
+
+    uint32_t now = AP_HAL::millis();
+    if (!should_search(now) &&
+        _detected_protocol != AP_RCProtocol::NONE) {
+        // pass all the bytes to the backend directly:
+        backend[_detected_protocol]->process_bytes(bytes, count, baudrate);
+        return;
+    }
+
+    // we're searching, so process bytes one-at-a-time:
+    for (uint8_t i=0; i<count; i++) {
+        process_byte(bytes[i], baudrate);
+    }
+}
+
 bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
 {
     uint32_t now = AP_HAL::millis();
@@ -422,6 +447,9 @@ bool AP_RCProtocol::new_input()
     const rcprotocol_t pollable[] {
 #if AP_RCPROTOCOL_DRONECAN_ENABLED
         AP_RCProtocol::DRONECAN,
+#endif
+#if AP_RCPROTOCOL_SITL_ENABLED
+        AP_RCProtocol::SITL,
 #endif
     };
     for (const auto protocol : pollable) {
@@ -554,6 +582,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol)
 #if AP_RCPROTOCOL_GHST_ENABLED
     case GHST:
         return "GHST";
+#endif
+#if AP_RCPROTOCOL_SITL_ENABLED
+    case SITL:
+        return "SITL";
 #endif
     case NONE:
         break;

libraries/AP_RCProtocol/AP_RCProtocol.h

@@ -74,6 +74,9 @@ class AP_RCProtocol {
 #endif
 #if AP_RCPROTOCOL_GHST_ENABLED
         GHST       = 14,
+#endif
+#if AP_RCPROTOCOL_SITL_ENABLED  // 8 or 16 packed integers
+        SITL = 16,
 #endif
         NONE    //last enum always is None
     };
@@ -96,6 +99,7 @@ class AP_RCProtocol {
     void process_pulse(uint32_t width_s0, uint32_t width_s1);
     void process_pulse_list(const uint32_t *widths, uint16_t n, bool need_swap);
     bool process_byte(uint8_t byte, uint32_t baudrate);
+    void process_bytes(uint8_t *byte, uint16_t count, uint32_t baudrate);
     void process_handshake(uint32_t baudrate);
     void update(void);
 
@@ -158,6 +162,9 @@ class AP_RCProtocol {
 #endif
 #if AP_RCPROTOCOL_DRONECAN_ENABLED
         case DRONECAN:
+#endif
+#if AP_RCPROTOCOL_SITL_ENABLED
+        case SITL:
 #endif
         case NONE:
             return false;
@@ -236,6 +243,8 @@ class AP_RCProtocol {
     // allowed RC protocols mask (first bit means "all")
     uint32_t rc_protocols_mask;
 
+    bool _process_bytes(uint8_t *byte, uint16_t count, uint32_t baudrate);
+
 #endif  // AP_RCPROTCOL_ENABLED
 
 };

libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp

@@ -71,7 +71,7 @@ void AP_RCProtocol_Backend::read(uint16_t *pwm, uint8_t n)
 /*
   provide input from a backend
  */
-void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool in_failsafe, int16_t _rssi, int16_t _rx_link_quality)
+void AP_RCProtocol_Backend::add_input(uint8_t num_values, const uint16_t *values, bool in_failsafe, int16_t _rssi, int16_t _rx_link_quality)
 {
     num_values = MIN(num_values, MAX_RCIN_CHANNELS);
     memcpy(_pwm_values, values, num_values*sizeof(uint16_t));
@@ -173,6 +173,13 @@ void AP_RCProtocol_Backend::configure_vtx(uint8_t band, uint8_t channel, uint8_t
 }
 #endif  // AP_VIDEOTX_ENABLED
 
+void AP_RCProtocol_Backend::process_bytes(uint8_t *bytes, uint16_t count, uint32_t baudrate)
+{
+    for (uint8_t i=0; i<count; i++) {
+         process_byte(bytes[i], baudrate);
+    }
+}
+
 /*
   optionally log RC input data
  */

libraries/AP_RCProtocol/AP_RCProtocol_Backend.h

@@ -32,6 +32,7 @@ class AP_RCProtocol_Backend {
     virtual ~AP_RCProtocol_Backend() {}
     virtual void process_pulse(uint32_t width_s0, uint32_t width_s1) {}
     virtual void process_byte(uint8_t byte, uint32_t baudrate) {}
+    virtual void process_bytes(uint8_t *bytes, uint16_t count, uint32_t baudrate);
     virtual void process_handshake(uint32_t baudrate) {}
     uint16_t read(uint8_t chan);
     void read(uint16_t *pwm, uint8_t n);
@@ -120,7 +121,7 @@ class AP_RCProtocol_Backend {
         uint32_t ch7 : 11;
     } PACKED;
 
-    void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe, int16_t rssi=-1, int16_t rx_link_quality=-1);
+    void add_input(uint8_t num_channels, const uint16_t *values, bool in_failsafe, int16_t rssi=-1, int16_t rx_link_quality=-1);
     AP_RCProtocol &frontend;
 
     void log_data(AP_RCProtocol::rcprotocol_t prot, uint32_t timestamp, const uint8_t *data, uint8_t len) const;

libraries/AP_RCProtocol/AP_RCProtocol_SITL.cpp

@@ -0,0 +1,66 @@
+#include "AP_RCProtocol_config.h"
+
+#if AP_RCPROTOCOL_SITL_ENABLED
+
+#include "AP_RCProtocol_SITL.h"
+
+#include <AP_HAL_SITL/AP_HAL_SITL.h>
+#include <AP_HAL_SITL/SITL_State.h>
+
+#include <SITL/SITL.h>
+extern const HAL_SITL& hal_sitl;
+
+void AP_RCProtocol_SITL::process_bytes(uint8_t *bytes, uint16_t count, uint32_t baudrate)
+{
+    const uint32_t now = AP_HAL::millis();
+    if (now - last_byte_ms > 10) {
+        // could be a framing failure.  Might also just be first byte
+        // in packet Discard.
+        search_buf_ofs = 0;
+    }
+    last_byte_ms = now;
+
+    // if we get 32 bytes that's 16 values. 16 is 8 values.  1 byte is
+    // "the RCProtocol library is searching"
+    switch (count) {
+    case 32:
+    case 16:
+        add_input(count/2, (uint16_t*)bytes, false);
+        search_success_correct_size_start_ms = 0;
+        return;
+    case 1:
+        // constant shift of bytes down
+        if (search_buf_ofs >= ARRAY_SIZE(search_buf)) {
+            memmove(&search_buf[1], &search_buf[0], search_buf_ofs-1);
+        }
+        search_buf[search_buf_ofs++] = bytes[0];
+        if (search_buf_ofs == 8 || search_buf_ofs == 16) {
+            search_success_correct_size_start_ms = now;
+        } else {
+            search_success_correct_size_start_ms = 0;
+        }
+        break;
+    default:
+        AP_HAL::panic("unexpected SITL byte count");
+    }
+}
+
+void AP_RCProtocol_SITL::update()
+{
+    const auto *sitl = AP::sitl();
+    if (sitl->state.rcin_chan_count) {
+        const uint16_t *pwm_input = hal_sitl.get_pwm_input();
+        add_input(sitl->state.rcin_chan_count, pwm_input, false);
+        return;
+    }
+
+    // frame-gap protection:
+    const uint32_t now = AP_HAL::millis();
+    if (now - search_success_correct_size_start_ms) {
+        add_input(search_buf_ofs/2, (uint16_t*)search_buf, false);
+        search_success_correct_size_start_ms = 0;
+    }
+}
+
+#endif // AP_RCPROTOCOL_SITL_ENABLED
+

libraries/AP_RCProtocol/AP_RCProtocol_SITL.h

@@ -0,0 +1,30 @@
+
+#pragma once
+
+#include "AP_RCProtocol_config.h"
+
+#if AP_RCPROTOCOL_SITL_ENABLED
+
+#include "AP_RCProtocol.h"
+
+
+class AP_RCProtocol_SITL : public AP_RCProtocol_Backend {
+public:
+
+    using AP_RCProtocol_Backend::AP_RCProtocol_Backend;
+
+    void process_bytes(uint8_t *bytes, uint16_t count, uint32_t baudrate) override;
+    void update() override;
+
+private:
+
+    uint32_t last_byte_ms;
+
+    // search support:
+    uint8_t search_buf[32];
+    uint8_t search_buf_ofs;
+    uint32_t search_success_correct_size_start_ms;
+};
+
+#endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED
+

libraries/AP_RCProtocol/AP_RCProtocol_config.h

@@ -69,3 +69,7 @@
 #ifndef AP_RCPROTOCOL_GHST_ENABLED
 #define AP_RCPROTOCOL_GHST_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED
 #endif
+
+#ifndef AP_RCPROTOCOL_SITL_ENABLED
+#define AP_RCPROTOCOL_SITL_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#endif