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

HAL_ChibiOS: use safety button logic from AP_BoardConfig

Browse Source
This commit is contained in:
Andrew Tridgell 2019-09-03 12:24:34 +10:00
parent 5a96e374ba
commit 03c1a8bfd9
2 changed files with 13 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -1506,35 +1506,21 @@ void RCOutput::safety_update(void)
#ifdef HAL_GPIO_PIN_SAFETY_IN
// handle safety button
uint16_t safety_options = 0;
if (boardconfig) {
safety_options = boardconfig->get_safety_button_options();
}
bool safety_pressed = palReadLine(HAL_GPIO_PIN_SAFETY_IN);
if (!(safety_options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) &&
hal.util->get_soft_armed()) {
safety_pressed = false;
}
if (safety_state==AP_HAL::Util::SAFETY_DISARMED &&
!(safety_options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF)) {
safety_pressed = false;
}
if (safety_state==AP_HAL::Util::SAFETY_ARMED &&
!(safety_options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)) {
safety_pressed = false;
}
if (safety_pressed) {
safety_button_counter++;
} else {
safety_button_counter = 0;
}
if (safety_button_counter == 10) {
// safety has been pressed for 1 second, change state
if (safety_state==AP_HAL::Util::SAFETY_ARMED) {
safety_state = AP_HAL::Util::SAFETY_DISARMED;
} else {
safety_state = AP_HAL::Util::SAFETY_ARMED;
AP_BoardConfig *brdconfig = AP_BoardConfig::get_singleton();
if (safety_press_count < 255) {
safety_press_count++;
}
if (brdconfig && brdconfig->safety_button_handle_pressed(safety_press_count)) {
if (safety_state ==AP_HAL::Util::SAFETY_ARMED) {
safety_state = AP_HAL::Util::SAFETY_DISARMED;
} else {
safety_state = AP_HAL::Util::SAFETY_ARMED;
}
}
} else {
safety_press_count = 0;
}
#elif HAL_WITH_IO_MCU
safety_state = _safety_switch_state();

1
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -314,6 +314,7 @@ private:
uint32_t safety_update_ms;
uint8_t led_counter;
int8_t safety_button_counter;
uint8_t safety_press_count; // 0.1s units
// mask of channels to allow when safety on
uint16_t safety_mask;