From 907977b2f2e73d782580924d4abe8f579639b955 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 25 Nov 2019 18:02:11 +1100
Subject: [PATCH] AP_HAL_SITL: support for virtual hardware safety switch

---
 libraries/AP_HAL_SITL/RCOutput.cpp | 18 ++++++++++++++++++
 libraries/AP_HAL_SITL/RCOutput.h   |  9 +++++++++
 libraries/AP_HAL_SITL/Util.cpp     |  9 +++++++++
 libraries/AP_HAL_SITL/Util.h       |  4 +++-
 4 files changed, 39 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_SITL/RCOutput.cpp b/libraries/AP_HAL_SITL/RCOutput.cpp
index f9169a5f9f..1e5e92193f 100644
--- a/libraries/AP_HAL_SITL/RCOutput.cpp
+++ b/libraries/AP_HAL_SITL/RCOutput.cpp
@@ -126,3 +126,21 @@ void RCOutput::neopixel_send(void)
 }
 
 #endif
+
+void RCOutput::force_safety_off(void)
+{
+    SITL::SITL *sitl = AP::sitl();
+    if (sitl == nullptr) {
+        return;
+    }
+    sitl->force_safety_off();
+}
+
+bool RCOutput::force_safety_on(void)
+{
+    SITL::SITL *sitl = AP::sitl();
+    if (sitl == nullptr) {
+        return false;
+    }
+    return sitl->force_safety_on();
+}
diff --git a/libraries/AP_HAL_SITL/RCOutput.h b/libraries/AP_HAL_SITL/RCOutput.h
index 8d7ba99e30..35fe2ae970 100644
--- a/libraries/AP_HAL_SITL/RCOutput.h
+++ b/libraries/AP_HAL_SITL/RCOutput.h
@@ -18,6 +18,15 @@ public:
     void cork(void) override;
     void push(void) override;
 
+    /*
+      force the safety switch on, disabling PWM output from the IO board
+     */
+    bool force_safety_on(void) override;
+    /*
+      force the safety switch off, enabling PWM output from the IO board
+     */
+    void force_safety_off(void) override;
+
     /*
       Serial LED emulation
      */
diff --git a/libraries/AP_HAL_SITL/Util.cpp b/libraries/AP_HAL_SITL/Util.cpp
index 812f2eebc7..17230a490e 100644
--- a/libraries/AP_HAL_SITL/Util.cpp
+++ b/libraries/AP_HAL_SITL/Util.cpp
@@ -124,3 +124,12 @@ void *HALSITL::Util::heap_realloc(void *heap_ptr, void *ptr, size_t new_size)
 }
 
 #endif // ENABLE_HEAP
+
+enum AP_HAL::Util::safety_state HALSITL::Util::safety_switch_state(void)
+{
+    const SITL::SITL *sitl = AP::sitl();
+    if (sitl == nullptr) {
+        return AP_HAL::Util::SAFETY_NONE;
+    }
+    return sitl->safety_switch_state();
+}
diff --git a/libraries/AP_HAL_SITL/Util.h b/libraries/AP_HAL_SITL/Util.h
index c209e657d4..97f7010656 100644
--- a/libraries/AP_HAL_SITL/Util.h
+++ b/libraries/AP_HAL_SITL/Util.h
@@ -49,7 +49,9 @@ public:
 
     // return true if the reason for the reboot was a watchdog reset
     bool was_watchdog_reset() const override { return getenv("SITL_WATCHDOG_RESET") != nullptr; }
-    
+
+    enum safety_state safety_switch_state(void) override;
+
 private:
     SITL_State *sitlState;