From e93b3722c97d7469834f94ba7ecf133ca2198012 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 16 Jul 2024 08:06:10 +1000
Subject: [PATCH] HAL_QURT: implement safety switch

this ensures no outputs during initialisation
---
 libraries/AP_HAL_QURT/RCOutput.cpp | 17 ++++++++++++++++-
 libraries/AP_HAL_QURT/RCOutput.h   | 14 ++++++++++++++
 libraries/AP_HAL_QURT/Util.cpp     | 13 +++++++++++++
 libraries/AP_HAL_QURT/Util.h       |  5 +++++
 4 files changed, 48 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_QURT/RCOutput.cpp b/libraries/AP_HAL_QURT/RCOutput.cpp
index 6b9124fcfa..eb509d0709 100644
--- a/libraries/AP_HAL_QURT/RCOutput.cpp
+++ b/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[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;
diff --git a/libraries/AP_HAL_QURT/RCOutput.h b/libraries/AP_HAL_QURT/RCOutput.h
index 3e4075a306..ee3f95aac2 100644
--- a/libraries/AP_HAL_QURT/RCOutput.h
+++ b/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 @@ public:
         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;
@@ -68,4 +79,7 @@ private:
 
     float esc_voltage;
     float esc_current;
+
+    // start with safety on, gets disabled by AP_BoardConfig
+    bool safety_on = true;
 };
diff --git a/libraries/AP_HAL_QURT/Util.cpp b/libraries/AP_HAL_QURT/Util.cpp
index 164d301b54..a0891c3cc3 100644
--- a/libraries/AP_HAL_QURT/Util.cpp
+++ b/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)
 {
diff --git a/libraries/AP_HAL_QURT/Util.h b/libraries/AP_HAL_QURT/Util.h
index 7c389703da..f1eea1493c 100644
--- a/libraries/AP_HAL_QURT/Util.h
+++ b/libraries/AP_HAL_QURT/Util.h
@@ -21,6 +21,11 @@ public:
 
     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;