AP_HAL_SITL: act on safety switch being enabled in SITL by zeroing outputs

libraries/AP_HAL_SITL/RCOutput.cpp

@@ -4,6 +4,9 @@
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 
+#include <AP_BoardConfig/AP_BoardConfig.h>
+#include <SITL/SITL.h>
+
 #include "RCOutput.h"
 
 #define ENABLE_DEBUG 0
@@ -48,6 +51,14 @@ void RCOutput::disable_ch(uint8_t ch)
 
 void RCOutput::write(uint8_t ch, uint16_t period_us)
 {
+    if (safety_state == AP_HAL::Util::SAFETY_DISARMED) {
+        const uint32_t safety_mask = AP_BoardConfig::get_singleton()->get_safety_mask();
+        if (!(safety_mask & (1U<<ch))) {
+            // implement safety pwm value
+            period_us = 0;
+        }
+    }
+
     _sitlState->output_ready = true;
     // FIXME: something in sitl is expecting to be able to read and write disabled channels
     if (ch < SITL_NUM_CHANNELS /*&& (_enable_mask & (1U<<ch))*/) {
@@ -147,22 +158,4 @@ void RCOutput::serial_led_send(const uint16_t chan)
 
 #endif //CONFIG_HAL_BOARD == HAL_BOARD_SITL
 
-void RCOutput::force_safety_off(void)
-{
-    SITL::SIM *sitl = AP::sitl();
-    if (sitl == nullptr) {
-        return;
-    }
-    sitl->force_safety_off();
-}
-
-bool RCOutput::force_safety_on(void)
-{
-    SITL::SIM *sitl = AP::sitl();
-    if (sitl == nullptr) {
-        return false;
-    }
-    return sitl->force_safety_on();
-}
-
 #endif //!defined(HAL_BUILD_AP_PERIPH)

libraries/AP_HAL_SITL/RCOutput.h

@@ -22,11 +22,21 @@ public:
     /*
       force the safety switch on, disabling PWM output from the IO board
      */
-    bool force_safety_on(void) override;
+    bool force_safety_on(void) override {
+        safety_state = AP_HAL::Util::SAFETY_DISARMED;
+        return true;
+    }
     /*
       force the safety switch off, enabling PWM output from the IO board
      */
-    void force_safety_off(void) override;
+    void force_safety_off(void) override {
+        safety_state = AP_HAL::Util::SAFETY_ARMED;
+    }
+
+    /*
+      get safety switch state, used by Util.cpp
+    */
+    AP_HAL::Util::safety_state _safety_switch_state(void) { return safety_state; }
 
     /*
       Serial LED emulation
@@ -43,6 +53,8 @@ private:
     uint32_t _enable_mask;
     bool _corked;
     uint16_t _pending[SITL_NUM_CHANNELS];
+
+    AP_HAL::Util::safety_state safety_state;
 };
 
 #endif

libraries/AP_HAL_SITL/Util.cpp

@@ -1,6 +1,9 @@
 #include "Util.h"
 #include <sys/time.h>
 #include <AP_Param/AP_Param.h>
+#include "RCOutput.h"
+
+extern const AP_HAL::HAL& hal;
 
 #ifdef WITH_SITL_TONEALARM
 HALSITL::ToneAlarm_SF HALSITL::Util::_toneAlarm;
@@ -143,11 +146,12 @@ void *HALSITL::Util::heap_realloc(void *heap_ptr, void *ptr, size_t old_size, si
 #if !defined(HAL_BUILD_AP_PERIPH)
 enum AP_HAL::Util::safety_state HALSITL::Util::safety_switch_state(void)
 {
-    const SITL::SIM *sitl = AP::sitl();
+#define HAL_USE_PWM 1
-    if (sitl == nullptr) {
+#if HAL_USE_PWM
-        return AP_HAL::Util::SAFETY_NONE;
+    return ((RCOutput *)hal.rcout)->_safety_switch_state();
-    }
+#else
-    return sitl->safety_switch_state();
+    return SAFETY_NONE;
+#endif
 }
 
 void HALSITL::Util::set_cmdline_parameters()