aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Peter Barker 2023-07-07 13:27:26 +10:00 committed by Andrew Tridgell
parent f5a26495d3
commit 2c9551a25a
3 changed files with 34 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_HAL_SITL/RCOutput.cpp
View File

@ -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)

16
libraries/AP_HAL_SITL/RCOutput.h
View File

@ -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

14
libraries/AP_HAL_SITL/Util.cpp
View File

@ -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();
if (sitl == nullptr) {
return AP_HAL::Util::SAFETY_NONE;
}
return sitl->safety_switch_state();
#define HAL_USE_PWM 1
#if HAL_USE_PWM
return ((RCOutput *)hal.rcout)->_safety_switch_state();
#else
return SAFETY_NONE;
#endif
}
void HALSITL::Util::set_cmdline_parameters()