AP_RCProtocol: allow handshake to initiate RC connection. Support listen-only SRXL2 devices.

only bootstrap when SRXL2 is the only configured protocol (RC_PROTOCOLS=256)
remove spurious 0 initializers

libraries/AP_RCProtocol/AP_RCProtocol.cpp

@@ -23,7 +23,7 @@
 #include "AP_RCProtocol_SBUS.h"
 #include "AP_RCProtocol_SUMD.h"
 #include "AP_RCProtocol_SRXL.h"
-#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
+#ifndef IOMCU_FW
 #include "AP_RCProtocol_SRXL2.h"
 #endif
 #include "AP_RCProtocol_CRSF.h"
@@ -44,7 +44,7 @@ void AP_RCProtocol::init()
     backend[AP_RCProtocol::DSM] = new AP_RCProtocol_DSM(*this);
     backend[AP_RCProtocol::SUMD] = new AP_RCProtocol_SUMD(*this);
     backend[AP_RCProtocol::SRXL] = new AP_RCProtocol_SRXL(*this);
-#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
+#ifndef IOMCU_FW
     backend[AP_RCProtocol::SRXL2] = new AP_RCProtocol_SRXL2(*this);
     backend[AP_RCProtocol::CRSF] = new AP_RCProtocol_CRSF(*this);
 #endif
@@ -165,6 +165,7 @@ bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
         // we're using pulse inputs, discard bytes
         return false;
     }
+
     // first try current protocol
     if (_detected_protocol != AP_RCProtocol::NONE && !searching) {
         backend[_detected_protocol]->process_byte(byte, baudrate);
@@ -207,6 +208,22 @@ bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
     return false;
 }
 
+// handshake if nothing else has succeeded so far
+void AP_RCProtocol::process_handshake( uint32_t baudrate)
+{
+    // if we ever succeeded before then do not handshake
+    if (_detected_protocol != AP_RCProtocol::NONE || _last_input_ms > 0) {
+        return;
+    }
+
+    // otherwise handshake all protocols
+    for (uint8_t i = 0; i < AP_RCProtocol::NONE; i++) {
+        if (backend[i] != nullptr) {
+            backend[i]->process_handshake(baudrate);
+        }
+    }
+}
+
 /*
   check for bytes from an additional uart. This is used to support RC
   protocols from SERIALn_PROTOCOL
@@ -257,6 +274,9 @@ void AP_RCProtocol::check_added_uart(void)
         added.uart->begin(added.baudrate, 128, 128);
         added.last_baud_change_ms = AP_HAL::millis();
     }
+
+    process_handshake(added.baudrate);
+
     uint32_t n = added.uart->available();
     n = MIN(n, 255U);
     for (uint8_t i=0; i<n; i++) {

libraries/AP_RCProtocol/AP_RCProtocol.h

@@ -52,6 +52,7 @@ public:
     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_handshake(uint32_t baudrate);
     void update(void);
 
     void disable_for_pulses(enum rcprotocol_t protocol) {

libraries/AP_RCProtocol/AP_RCProtocol_Backend.h

@@ -28,6 +28,7 @@ public:
     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_handshake(uint32_t baudrate) {}
     uint16_t read(uint8_t chan);
     void read(uint16_t *pwm, uint8_t n);
     bool new_input();
@@ -58,6 +59,10 @@ public:
         return rc_input_count;
     }
 
+    uint32_t get_rc_protocols_mask(void) const {
+        return frontend.rc_protocols_mask;
+    }
+
     // get RSSI
     int16_t get_RSSI(void) const {
         return rssi;

libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp

@@ -39,10 +39,9 @@ AP_RCProtocol_SRXL2* AP_RCProtocol_SRXL2::_singleton;
 
 AP_RCProtocol_SRXL2::AP_RCProtocol_SRXL2(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend)
 {
-    const uint32_t uniqueID = AP_HAL::micros();
 #if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
     if (_singleton != nullptr) {
-        AP_HAL::panic("Duplicate SRXL2 handler");
+        AP_HAL::panic("Duplicate SRXL2 handler\n");
     }
 
     _singleton = this;
@@ -51,16 +50,27 @@ AP_RCProtocol_SRXL2::AP_RCProtocol_SRXL2(AP_RCProtocol &_frontend) : AP_RCProtoc
         _singleton = this;
     }
 #endif
+}
+
+void AP_RCProtocol_SRXL2::_bootstrap(uint8_t device_id)
+{
+    if (_device_id == device_id) {
+        return;
+    }
+
     // Init the local SRXL device
-    if (!srxlInitDevice(SRXL_DEVICE_ID, SRXL_DEVICE_PRIORITY, SRXL_DEVICE_INFO, uniqueID)) {
-        AP_HAL::panic("Failed to initialize SRXL2 device");
+    if (!srxlInitDevice(device_id, SRXL_DEVICE_PRIORITY, SRXL_DEVICE_INFO, device_id)) {
+        AP_HAL::panic("Failed to initialize SRXL2 device\n");
     }
 
     // Init the SRXL bus: The bus index must always be < SRXL_NUM_OF_BUSES -- in this case, it can only be 0
     if (!srxlInitBus(0, 0, SRXL_SUPPORTED_BAUD_RATES)) {
-        AP_HAL::panic("Failed to initialize SRXL2 bus");
+        AP_HAL::panic("Failed to initialize SRXL2 bus\n");
     }
 
+    _device_id = device_id;
+
+    debug("Bootstrapped as 0x%x", _device_id);
 }
 
 AP_RCProtocol_SRXL2::~AP_RCProtocol_SRXL2() {
@@ -121,7 +131,19 @@ void AP_RCProtocol_SRXL2::_process_byte(uint32_t timestamp_us, uint8_t byte)
 
         if (_buflen == _frame_len_full) {
             log_data(AP_RCProtocol::SRXL2, timestamp_us, _buffer, _buflen);
-
+            // we got a full frame but never handshaked before
+            if (!is_bootstrapped()) {
+                if (_buffer[1] == 0x21) {
+                    _bootstrap(SRXL_DEVICE_ID);
+                    _last_handshake_ms = timestamp_us / 1000;
+                } else {
+                    // not a handshake frame so reset without initializing the SRXL2 engine
+                    _decode_state = STATE_IDLE;
+                    _buflen = 0;
+                    _frame_len_full = 0;
+                    return;
+                }
+            }
             // Try to parse SRXL packet -- this internally calls srxlRun() after packet is parsed and resets timeout
             if (srxlParsePacket(0, _buffer, _frame_len_full)) {
                 add_input(MAX_CHANNELS, _channels, _in_failsafe, _new_rssi);
@@ -228,9 +250,45 @@ void AP_RCProtocol_SRXL2::process_byte(uint8_t byte, uint32_t baudrate)
     if (baudrate != 115200) {
         return;
     }
+
     _process_byte(AP_HAL::micros(), byte);
 }
 
+// handshake
+void AP_RCProtocol_SRXL2::process_handshake(uint32_t baudrate)
+{
+    // only bootstrap if only SRXL2 is enabled
+    if (baudrate != 115200 || (get_rc_protocols_mask() & ~(1U<<(uint8_t(AP_RCProtocol::SRXL2)+1)))) {
+        _handshake_start_ms = 0;
+        return;
+    }
+    uint32_t now = AP_HAL::millis();
+
+    // record the time of the first request in this cycle
+    if (_handshake_start_ms == 0) {
+        _handshake_start_ms = now;
+        // it seems the handshake protocol only sets the baudrate after receiving data
+        // since we are sending data unprompted make sure that the uart is set up correctly
+        _change_baud_rate(baudrate);
+    }
+
+    // we have not bootstrapped and attempts to listen first have failed
+    // we should receive a handshake request within the first 250ms
+    if (!is_bootstrapped() && now - _handshake_start_ms > 250) {
+        _bootstrap(SRXL_DEVICE_ID_BASE_RX);
+    }
+
+    // certain RXs (e.g. AR620) are "listen-only" - they require the flight controller to initiate
+    // a handshake in order to switch to SRXL2 mode. This requires that we send data on the UART even
+    // if we have not decoded SRXL2 (recently). We try this every 50ms.
+    if (now - _handshake_start_ms > 250 && (_last_handshake_ms == 0 || (now - _last_run_ms > 50 && now - _last_handshake_ms > 50))) {
+        _in_bootstrap_or_failsafe = true;
+        srxlRun(0, 50); // 50 is a magic number at which the handshake protocol is initiated
+        _in_bootstrap_or_failsafe = false;
+        _last_handshake_ms = now;
+    }
+}
+
 // send data to the uart
 void AP_RCProtocol_SRXL2::send_on_uart(uint8_t* pBuffer, uint8_t length)
 {
@@ -307,7 +365,7 @@ void AP_RCProtocol_SRXL2::_send_on_uart(uint8_t* pBuffer, uint8_t length)
         uint64_t tend = uart->receive_time_constraint_us(1);
         uint64_t now = AP_HAL::micros64();
         uint64_t tdelay = now - tend;
-        if (tdelay > 2000) {
+        if (tdelay > 2000 && !_in_bootstrap_or_failsafe) {
             // we've been too slow in responding
             return;
         }
@@ -338,6 +396,7 @@ void AP_RCProtocol_SRXL2::_change_baud_rate(uint32_t baudrate)
         uart->begin(baudrate);
         uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
         uart->set_unbuffered_writes(true);
+        uart->set_blocking_writes(false);
     }
 }
 

libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h

@@ -29,6 +29,7 @@ public:
     AP_RCProtocol_SRXL2(AP_RCProtocol &_frontend);
     virtual ~AP_RCProtocol_SRXL2();
     void process_byte(uint8_t byte, uint32_t baudrate) override;
+    void process_handshake(uint32_t baudrate) override;
     void start_bind(void) override;
     void update(void) override;
     // get singleton instance
@@ -53,20 +54,26 @@ private:
     void _send_on_uart(uint8_t* pBuffer, uint8_t length);
     void _change_baud_rate(uint32_t baudrate);
     void _capture_scaled_input(const uint8_t *values_p, bool in_failsafe, int16_t rssi);
+    void _bootstrap(uint8_t device_id);
+    bool is_bootstrapped() const { return _device_id != 0; }
 
     uint8_t _buffer[SRXL2_FRAMELEN_MAX];       /* buffer for raw srxl frame data in correct order --> buffer[0]=byte0  buffer[1]=byte1  */
     uint8_t _buflen;                          /* length in number of bytes of received srxl dataframe in buffer  */
     uint32_t _last_run_ms;                    // last time the state machine was run
-    uint16_t _channels[SRXL2_MAX_CHANNELS] = {0};    /* buffer for extracted RC channel data as pulsewidth in microseconds */
+    uint16_t _channels[SRXL2_MAX_CHANNELS];    /* buffer for extracted RC channel data as pulsewidth in microseconds */
+    bool _in_bootstrap_or_failsafe;         // controls whether we allow UART sends outside a receive time constraint
+    uint8_t _device_id;
 
     enum {
         STATE_IDLE,                          /* do nothing */
         STATE_NEW,                           /* get header of frame + prepare for frame reception */
         STATE_COLLECT                        /* collect RC channel data from frame */
     };
-    uint8_t _frame_len_full = 0U;                 /* Length in number of bytes of full srxl datastream */
-    uint8_t _decode_state = STATE_IDLE;           /* Current state of SRXL frame decoding */
-    uint8_t _decode_state_next = STATE_IDLE;      /* State of frame decoding that will be applied when the next byte from dataframe drops in  */
+    uint8_t _frame_len_full;                 /* Length in number of bytes of full srxl datastream */
+    uint8_t _decode_state;           /* Current state of SRXL frame decoding */
+    uint8_t _decode_state_next;      /* State of frame decoding that will be applied when the next byte from dataframe drops in  */
     bool _in_failsafe = false;
     int16_t _new_rssi = -1;
+    uint32_t _last_handshake_ms;
+    uint32_t _handshake_start_ms;
 };

libraries/AP_RCProtocol/spm_srxl_config.h

@@ -50,6 +50,7 @@ extern "C++" {
 //    VTX = 0x81
 // NOTE: This value is not used internally -- it is passed as a parameter to srxlInit() in the example app
 #define SRXL_DEVICE_ID              0x31
+#define SRXL_DEVICE_ID_BASE_RX      0x30
 
 // Set this to the desired priority level for sending telemetry, ranging from 0 to 100.
 // Generally, this number should be 10 times the number of different telemetry packets to regularly send.