Add simulated hardware safety switch to SITL

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();
+}

libraries/AP_HAL_SITL/RCOutput.h

@@ -18,6 +18,15 @@ class HALSITL::RCOutput : public AP_HAL::RCOutput {
     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
      */

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();
+}

libraries/AP_HAL_SITL/Util.h

@@ -49,7 +49,9 @@ class HALSITL::Util : public AP_HAL::Util {
 
     // 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;
 

libraries/SITL/SITL.cpp

@@ -200,6 +200,8 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
 
     AP_GROUPINFO("EFI_TYPE",    58, SITL,  efi_type,  SITL::EFI_TYPE_NONE),
 
+    AP_GROUPINFO("SAFETY_STATE",    59, SITL,  _safety_switch_state, 0),
+
     AP_GROUPEND
 
 };

libraries/SITL/SITL.h

@@ -292,6 +292,19 @@ class SITL {
         AP_Float hdg; // 0 to 360
     } opos;
 
+    AP_Int8 _safety_switch_state;
+
+    AP_HAL::Util::safety_state safety_switch_state() const {
+        return (AP_HAL::Util::safety_state)_safety_switch_state.get();
+    }
+    void force_safety_off() {
+        _safety_switch_state = (uint8_t)AP_HAL::Util::SAFETY_ARMED;
+    }
+    bool force_safety_on() {
+        _safety_switch_state = (uint8_t)AP_HAL::Util::SAFETY_DISARMED;
+        return true;
+    }
+
     uint16_t irlock_port;
 
     void simstate_send(mavlink_channel_t chan);