AP_HAL_Linux: Basic safety switch infrastructure

ArduPilot · Apr 6, 2024 · f244949 · f244949
1 parent 01b0e0c
commit f244949
Show file tree

Hide file tree

Showing 5 changed files with 58 additions and 1 deletion.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
+
 };
 
 }