QURT: update motor order and add safety state

Tools/Frame_params/ModalAI/AutonomyDevKit.parm

@@ -6,6 +6,12 @@ FLTMODE1         5
 FLTMODE4         2
 FLTMODE_CH       6
 
+# motor ordering
+SERVO1_FUNCTION 34
+SERVO2_FUNCTION 35
+SERVO3_FUNCTION 33
+SERVO4_FUNCTION 36
+
 # enable PID logging
 LOG_BITMASK      65535
 

libraries/AP_HAL_QURT/RCOutput.cpp

@@ -2,6 +2,7 @@
 
 #include "RCOutput.h"
 #include <AP_Math/AP_Math.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -114,10 +115,24 @@ void RCOutput::send_receive(void)
         return;
     }
 
+    AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
+    uint32_t safety_mask = 0;
+
+    if (boardconfig != nullptr) {
+        // mask of channels to allow with safety on
+        safety_mask = boardconfig->get_safety_mask();
+    }
+
     int16_t data[5] {};
 
     for (uint8_t i=0; i<4; i++) {
-        data[esc_map[i]] = pwm_to_esc(period[i]);
+        uint16_t v = period[i];
+        if (safety_on && (safety_mask & (1U<<i)) == 0) {
+            // when safety is on we send 0, which allows us to still
+            // get feedback telemetry data, including battery voltage
+            v = 0;
+        }
+        data[i] = pwm_to_esc(v);
     }
 
     need_write = false;
@@ -140,7 +155,7 @@ void RCOutput::send_receive(void)
  */
 void RCOutput::handle_esc_feedback(const struct esc_response_v2 &pkt)
 {
-    const uint8_t idx = esc_map_rev[pkt.id_state>>4];
+    const uint8_t idx = pkt.id_state>>4;
     if (idx >= ARRAY_SIZE(period)) {
         return;
     }

libraries/AP_HAL_QURT/RCOutput.h

@@ -7,6 +7,8 @@
 class QURT::RCOutput : public AP_HAL::RCOutput, AP_ESC_Telem_Backend
 {
 public:
+    friend class QURT::Util;
+
     void init() override;
     void set_freq(uint32_t chmask, uint16_t freq_hz) override;
     uint16_t get_freq(uint8_t ch) override;
@@ -27,6 +29,15 @@ class QURT::RCOutput : public AP_HAL::RCOutput, AP_ESC_Telem_Backend
         return esc_current;
     }
 
+    /*
+      force the safety switch on, disabling output from the ESCs/servos
+     */
+    bool force_safety_on(void) override { safety_on = true; return true; }
+
+    /*
+      force the safety switch off, enabling output from the ESCs/servos
+     */
+    void force_safety_off(void) override { safety_on = false; }
 
 private:
     const uint32_t baudrate = 2000000;
@@ -56,10 +67,6 @@ class QURT::RCOutput : public AP_HAL::RCOutput, AP_ESC_Telem_Backend
     void handle_esc_feedback(const struct esc_response_v2 &pkt);
     void handle_power_status(const struct esc_power_status &pkt);
 
-    // order is RL, FL, FR, RR, map to X frame
-    const uint8_t esc_map[4] {2, 0, 1, 3};
-    const uint8_t esc_map_rev[4] {1, 2, 0, 3};
-
     int fd = -1;
     uint16_t enable_mask;
     static const uint8_t channel_count = 4;
@@ -72,4 +79,7 @@ class QURT::RCOutput : public AP_HAL::RCOutput, AP_ESC_Telem_Backend
 
     float esc_voltage;
     float esc_current;
+
+    // start with safety on, gets disabled by AP_BoardConfig
+    bool safety_on = true;
 };

libraries/AP_HAL_QURT/Util.cpp

@@ -5,6 +5,7 @@
 extern const AP_HAL::HAL& hal;
 
 #include "Util.h"
+#include "RCOutput.h"
 
 using namespace QURT;
 
@@ -20,6 +21,18 @@ uint32_t Util::available_memory(void)
     return fc_heap_size() - fc_heap_usage();
 }
 
+/*
+  return state of safety switch, if applicable
+*/
+Util::safety_state Util::safety_switch_state(void)
+{
+    const auto *rcout = (QURT::RCOutput *)hal.rcout;
+    if (rcout != nullptr && rcout->safety_on) {
+        return SAFETY_DISARMED;
+    }
+    return SAFETY_ARMED;
+}
+
 #if ENABLE_HEAP
 void *Util::allocate_heap_memory(size_t size)
 {

libraries/AP_HAL_QURT/Util.h

@@ -21,6 +21,11 @@ class QURT::Util : public AP_HAL::Util
 
     uint32_t available_memory(void) override;
 
+    /*
+      return state of safety switch, if applicable
+     */
+    enum safety_state safety_switch_state(void) override;
+
 #if ENABLE_HEAP
     // heap functions, note that a heap once alloc'd cannot be dealloc'd
     virtual void *allocate_heap_memory(size_t size) override;