From bb0cbd15a4c75a3e185d942ebad19e730a8927ce Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 14 Apr 2018 13:55:03 +1000 Subject: [PATCH] HAL_ChibiOS: implement fmuv4 safety state and added safety_mask and safety_pwm support for both fmuv3 and fmuv4 --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 185 +++++++++++++++--- libraries/AP_HAL_ChibiOS/RCOutput.h | 24 +++ libraries/AP_HAL_ChibiOS/Util.cpp | 10 +- .../AP_HAL_ChibiOS/hwdef/fmuv4/hwdef.dat | 2 +- 4 files changed, 184 insertions(+), 37 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 9d5d3a65420..94d8ae761b9 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -81,6 +81,10 @@ void RCOutput::init() // setup default output rate of 50Hz set_freq(0xFFFF, 50); + +#ifdef HAL_GPIO_PIN_SAFETY_IN + safety_state = AP_HAL::Util::SAFETY_DISARMED; +#endif } /* @@ -314,6 +318,12 @@ void RCOutput::write(uint8_t chan, uint16_t period_us) if (chan < chan_offset) { return; } + + if (safety_state == AP_HAL::Util::SAFETY_DISARMED && !(safety_mask & (1U<safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED; for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { pwm_group &group = pwm_group_list[i]; @@ -352,6 +363,12 @@ void RCOutput::push_local(void) } if (outmask & (1UL<safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED; + for (uint8_t i=0; i<4; i++) { uint8_t chan = group.chan[i]; if (chan != CHAN_DISABLED) { uint16_t pwm = period[chan]; + + if (safety_on && !(safety_mask & (1U<<(chan+chan_offset)))) { + // safety is on, overwride pwm + pwm = safe_pwm[chan+chan_offset]; + } + pwm = constrain_int16(pwm, _esc_pwm_min, _esc_pwm_max); uint16_t value = 2000UL * uint32_t(pwm - _esc_pwm_min) / uint32_t(_esc_pwm_max - _esc_pwm_min); if (value != 0) { @@ -1259,4 +1258,128 @@ void RCOutput::serial_end(void) serial_group = nullptr; } +/* + get safety switch state for Util.cpp + */ +AP_HAL::Util::safety_state RCOutput::_safety_switch_state(void) +{ +#if HAL_WITH_IO_MCU + if (AP_BoardConfig::io_enabled()) { + return iomcu.get_safety_switch_state(); + } +#endif + return safety_state; +} + +/* + force the safety switch on, disabling PWM output from the IO board +*/ +bool RCOutput::force_safety_on(void) +{ +#if HAL_WITH_IO_MCU + if (AP_BoardConfig::io_enabled()) { + return iomcu.force_safety_on(); + } + return false; +#else + safety_state = AP_HAL::Util::SAFETY_DISARMED; + return true; +#endif +} + +/* + force the safety switch off, enabling PWM output from the IO board +*/ +void RCOutput::force_safety_off(void) +{ +#if HAL_WITH_IO_MCU + if (AP_BoardConfig::io_enabled()) { + iomcu.force_safety_off(); + } +#else + safety_state = AP_HAL::Util::SAFETY_ARMED; +#endif +} + +/* + set PWM to send to a set of channels when the safety switch is + in the safe state +*/ +void RCOutput::set_safety_pwm(uint32_t chmask, uint16_t period_us) +{ +#if HAL_WITH_IO_MCU + if (AP_BoardConfig::io_enabled()) { + iomcu.set_safety_pwm(chmask, period_us); + } +#endif + for (uint8_t i=0; i<16; i++) { + if (chmask & (1U<get_safety_mask(); + } + +#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_ON)) { + safety_pressed = false; + } + if (safety_state==AP_HAL::Util::SAFETY_ARMED && + !(safety_options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF)) { + 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; + } + } +#elif HAL_WITH_IO_MCU + safety_state = _safety_switch_state(); + iomcu.set_safety_mask(safety_mask); +#endif + +#ifdef HAL_GPIO_PIN_LED_SAFETY + led_counter = (led_counter+1) % 16; + const uint16_t led_pattern = safety_state==AP_HAL::Util::SAFETY_DISARMED?0x5500:0xFFFF; + palWriteLine(HAL_GPIO_PIN_LED_SAFETY, (led_pattern & (1U << led_counter))?0:1); +#endif +} + #endif // HAL_USE_PWM diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 21f1fa9ac4f..3a4e26fc380 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -63,6 +63,12 @@ public: */ void force_safety_off(void) override; + /* + set PWM to send to a set of channels when the safety switch is + in the safe state + */ + void set_safety_pwm(uint32_t chmask, uint16_t period_us) override; + bool enable_px4io_sbus_out(uint16_t rate_hz) override; /* @@ -120,6 +126,11 @@ public: with DShot to get telemetry feedback */ void set_telem_request_mask(uint16_t mask) { telem_request_mask = (mask >> chan_offset); } + + /* + get safety switch state, used by Util.cpp + */ + AP_HAL::Util::safety_state _safety_switch_state(void); private: struct pwm_group { @@ -212,6 +223,7 @@ private: // these values are for the local channels. Non-local channels are handled by IOMCU uint32_t en_mask; uint16_t period[16]; + uint16_t safe_pwm[16]; // pwm to use when safety is on uint8_t num_channels; bool corked; // mask of channels that are running in high speed @@ -240,6 +252,18 @@ private: // setup output frequency for a group void set_freq_group(pwm_group &group); + + // safety switch state + AP_HAL::Util::safety_state safety_state; + uint32_t safety_update_ms; + uint8_t led_counter; + int8_t safety_button_counter; + + // mask of channels to allow when safety on + uint16_t safety_mask; + + // update safety switch and LED + void safety_update(void); /* DShot handling diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 92c758fb6fb..82c9d442df9 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -20,6 +20,7 @@ #include "Util.h" #include #include "ToneAlarm.h" +#include "RCOutput.h" #if HAL_WITH_IO_MCU #include @@ -85,12 +86,11 @@ void* Util::try_alloc_from_ccm_ram(size_t size) */ Util::safety_state Util::safety_switch_state(void) { -#if HAL_WITH_IO_MCU - if (AP_BoardConfig::io_enabled()) { - return iomcu.get_safety_switch_state(); - } -#endif +#if HAL_USE_PWM == TRUE + return ((RCOutput *)hal.rcout)->_safety_switch_state(); +#else return SAFETY_NONE; +#endif } void Util::set_imu_temp(float current) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv4/hwdef.dat index 62f32bd2135..b72bba09cb1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv4/hwdef.dat @@ -103,7 +103,7 @@ PC0 VBUS_VALID INPUT PC1 RSSI_IN ADC1 PC2 MPU9250_CS CS PC3 LED_SAFETY OUTPUT -PC4 SAFETY_IN INPUT +PC4 SAFETY_IN INPUT PULLDOWN PC5 VDD_PERIPH_EN OUTPUT HIGH PC7 TIM8_CH2 TIM8 RCIN FLOAT LOW # also USART6_RX for serial RC