From f2449496cc8cf1cd3ef83fc117a84266424509fe Mon Sep 17 00:00:00 2001
From: Oleksiy Protas <elfy.ua@gmail.com>
Date: Sat, 6 Apr 2024 20:38:25 +0300
Subject: [PATCH] AP_HAL_Linux: Basic safety switch infrastructure

---
 libraries/AP_HAL/GPIO.h                    |  1 +
 libraries/AP_HAL/board/linux.h             |  4 +++-
 libraries/AP_HAL_Linux/HAL_Linux_Class.cpp | 11 +++++++++
 libraries/AP_HAL_Linux/Util.cpp            | 17 ++++++++++++++
 libraries/AP_HAL_Linux/Util.h              | 26 ++++++++++++++++++++++
 5 files changed, 58 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL/GPIO.h b/libraries/AP_HAL/GPIO.h
index f32a1eb250ede..a052fcf77be40 100644
--- a/libraries/AP_HAL/GPIO.h
+++ b/libraries/AP_HAL/GPIO.h
@@ -10,6 +10,7 @@
 
 class AP_HAL::DigitalSource {
 public:
+    virtual ~DigitalSource() {};
     virtual void    mode(uint8_t output) = 0;
     virtual uint8_t read() = 0;
     virtual void    write(uint8_t value) = 0;
diff --git a/libraries/AP_HAL/board/linux.h b/libraries/AP_HAL/board/linux.h
index 80238c3a56eb9..fee0ecc26fe88 100644
--- a/libraries/AP_HAL/board/linux.h
+++ b/libraries/AP_HAL/board/linux.h
@@ -334,7 +334,9 @@
 #endif
 
 #define HAL_HAVE_BOARD_VOLTAGE 1
-#define HAL_HAVE_SAFETY_SWITCH 0
+#ifndef HAL_HAVE_SAFETY_SWITCH
+    #define HAL_HAVE_SAFETY_SWITCH 0
+#endif
 
 
 #ifndef HAL_HAVE_SERVO_VOLTAGE
diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
index a0be63c44ab90..e5290fbb66a60 100644
--- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
+++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
@@ -481,6 +481,17 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const
     analogin->init();
     utilInstance.init(argc+gopt.optind-1, &argv[gopt.optind-1]);
 
+#if HAL_HAVE_SAFETY_SWITCH
+    #ifndef HAL_SAFETY_SWITCH_PIN
+        #error HAL_HAVE_SAFETY_SWITCH set but no HAL_SAFETY_SWITCH_PIN defined
+    #endif // HAL_SAFETY_SWITCH_PIN
+
+    auto safety_switch = gpio->channel(HAL_SAFETY_SWITCH_PIN);
+    safety_switch->mode(HAL_GPIO_INPUT);
+    utilInstance.set_safety_switch(safety_switch);
+#endif // HAL_HAVE_SAFETY_SWITCH
+
+
     // NOTE: See commit 9f5b4ffca ("AP_HAL_Linux_Class: Correct
     // deadlock, and infinite loop in setup()") for details about the
     // order of scheduler initialize and setup on Linux.
diff --git a/libraries/AP_HAL_Linux/Util.cpp b/libraries/AP_HAL_Linux/Util.cpp
index dc9d8083a17b1..f7a0bdff19cec 100644
--- a/libraries/AP_HAL_Linux/Util.cpp
+++ b/libraries/AP_HAL_Linux/Util.cpp
@@ -108,6 +108,23 @@ uint32_t Util::available_memory(void)
 #define HAL_LINUX_DEFAULT_SYSTEM_ID "linux-unknown"
 #endif
 
+#if HAL_HAVE_SAFETY_SWITCH
+
+Util::safety_state Util::safety_switch_state(void)
+{
+    return _safety_switch->read() ? SAFETY_ARMED : SAFETY_DISARMED;
+}
+
+void Util::set_safety_switch(AP_HAL::DigitalSource* source)
+{
+    if (_safety_switch) {
+        delete _safety_switch;
+    }
+    _safety_switch = source;
+}
+
+#endif // HAL_HAVE_SAFETY_SWITCH
+
 /*
   get a (hopefully unique) machine ID
  */
diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h
index 9aa0a82314678..a871fd033c9ce 100644
--- a/libraries/AP_HAL_Linux/Util.h
+++ b/libraries/AP_HAL_Linux/Util.h
@@ -24,6 +24,13 @@ enum hw_type {
 
 class Util : public AP_HAL::Util {
 public:
+    virtual ~Util() {
+#if HAL_HAVE_SAFETY_SWITCH
+        delete _safety_switch;
+        _safety_switch = nullptr;
+#endif // HAL_HAVE_SAFETY_SWITCH
+    }
+
     static Util *from(AP_HAL::Util *util) {
         return static_cast<Util*>(util);
     }
@@ -69,6 +76,21 @@ class Util : public AP_HAL::Util {
 
     uint32_t available_memory(void) override;
 
+#if HAL_HAVE_SAFETY_SWITCH
+    /*
+     * Read a GPIO as safety swtich state
+     * Implementation implies a pulled up pin with the switch pulling low when inserted
+     * set_safety_switch() is used to set the source during start-up
+     */
+    enum safety_state safety_switch_state(void) override;
+
+    /*
+     * Set the digital pin to be watched as a safety switch
+     * NOTE: takes ownership of the object
+     */
+    void set_safety_switch(AP_HAL::DigitalSource* source);
+#endif // HAL_HAVE_SAFETY_SWITCH
+
     bool get_system_id(char buf[50]) override;
     bool get_system_id_unformatted(uint8_t buf[], uint8_t &len) override;
 
@@ -128,6 +150,10 @@ class Util : public AP_HAL::Util {
     };
 #endif // ENABLE_HEAP
 
+#if HAL_HAVE_SAFETY_SWITCH
+    AP_HAL::DigitalSource* _safety_switch = nullptr;
+#endif // HAL_HAVE_SAFETY_SWITCH
+
 };
 
 }