From 907977b2f2e73d782580924d4abe8f579639b955 Mon Sep 17 00:00:00 2001 From: Peter Barker 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;