From b1d59c8817538009d21ed3c7033b818064310f79 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 24 Aug 2019 17:14:17 -0400 Subject: [PATCH] px4fmu split safety button into new driver --- boards/mro/ctrl-zero-f7/default.cmake | 1 + .../mro/ctrl-zero-f7/init/rc.board_defaults | 2 + boards/nxp/fmuk66-v3/default.cmake | 1 + boards/nxp/fmuk66-v3/init/rc.board_defaults | 2 + boards/px4/fmu-v4/default.cmake | 1 + boards/px4/fmu-v4/init/rc.board_defaults | 2 + boards/px4/fmu-v4/rtps.cmake | 1 + boards/px4/fmu-v4/stackcheck.cmake | 1 + boards/px4/fmu-v5/critmonitor.cmake | 1 + boards/px4/fmu-v5/default.cmake | 1 + boards/px4/fmu-v5/fixedwing.cmake | 1 + boards/px4/fmu-v5/init/rc.board_defaults | 2 + boards/px4/fmu-v5/irqmonitor.cmake | 1 + boards/px4/fmu-v5/multicopter.cmake | 1 + boards/px4/fmu-v5/rover.cmake | 1 + boards/px4/fmu-v5/rtps.cmake | 1 + boards/px4/fmu-v5/stackcheck.cmake | 1 + boards/px4/fmu-v5x/default.cmake | 1 + boards/px4/fmu-v5x/fixedwing.cmake | 1 + boards/px4/fmu-v5x/init/rc.board_defaults | 2 + boards/px4/fmu-v5x/multicopter.cmake | 1 + boards/px4/fmu-v5x/rover.cmake | 1 + boards/px4/fmu-v5x/rtps.cmake | 1 + boards/px4/fmu-v5x/stackcheck.cmake | 1 + src/drivers/px4fmu/fmu.cpp | 192 +------------- src/drivers/safety_button/CMakeLists.txt | 42 ++++ src/drivers/safety_button/SafetyButton.cpp | 236 ++++++++++++++++++ src/drivers/safety_button/SafetyButton.hpp | 86 +++++++ 28 files changed, 394 insertions(+), 191 deletions(-) create mode 100644 src/drivers/safety_button/CMakeLists.txt create mode 100644 src/drivers/safety_button/SafetyButton.cpp create mode 100644 src/drivers/safety_button/SafetyButton.hpp diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index 7b37b65cfb..26dfedb27c 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -48,6 +48,7 @@ px4_add_board( px4fmu rc_input roboclaw + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/mro/ctrl-zero-f7/init/rc.board_defaults b/boards/mro/ctrl-zero-f7/init/rc.board_defaults index de65d26ffb..2930b72d65 100644 --- a/boards/mro/ctrl-zero-f7/init/rc.board_defaults +++ b/boards/mro/ctrl-zero-f7/init/rc.board_defaults @@ -11,3 +11,5 @@ fi set LOGGER_BUF 64 set MIXER_AUX none + +safety_button start diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index 469e90e971..247087e455 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -46,6 +46,7 @@ px4_add_board( pwm_out_sim px4fmu rc_input + safety_button tap_esc telemetry # all available telemetry drivers #test_ppm # NOT Portable YET diff --git a/boards/nxp/fmuk66-v3/init/rc.board_defaults b/boards/nxp/fmuk66-v3/init/rc.board_defaults index 52739b0d63..2d81e88031 100644 --- a/boards/nxp/fmuk66-v3/init/rc.board_defaults +++ b/boards/nxp/fmuk66-v3/init/rc.board_defaults @@ -9,3 +9,5 @@ then fi +safety_button start + diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 5599773bec..04cbea663d 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -38,6 +38,7 @@ px4_add_board( pwm_out_sim px4fmu rc_input + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v4/init/rc.board_defaults b/boards/px4/fmu-v4/init/rc.board_defaults index 186da5ee37..716e0ddf2e 100644 --- a/boards/px4/fmu-v4/init/rc.board_defaults +++ b/boards/px4/fmu-v4/init/rc.board_defaults @@ -11,3 +11,5 @@ then fi +safety_button start + diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index bd423dc2f2..b9c5968540 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -40,6 +40,7 @@ px4_add_board( pwm_out_sim px4fmu rc_input + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index 6c853978b4..0427e928d5 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -38,6 +38,7 @@ px4_add_board( pwm_out_sim px4fmu rc_input + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index 481a304de2..5bb06eee4f 100644 --- a/boards/px4/fmu-v5/critmonitor.cmake +++ b/boards/px4/fmu-v5/critmonitor.cmake @@ -52,6 +52,7 @@ px4_add_board( px4io rc_input roboclaw + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 797c6e01e0..97b9c79586 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -52,6 +52,7 @@ px4_add_board( px4io rc_input roboclaw + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index 8eff52df13..7f5fb493f0 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -36,6 +36,7 @@ px4_add_board( px4fmu px4io rc_input + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v5/init/rc.board_defaults b/boards/px4/fmu-v5/init/rc.board_defaults index df97fadc5c..0a44f6e6da 100644 --- a/boards/px4/fmu-v5/init/rc.board_defaults +++ b/boards/px4/fmu-v5/init/rc.board_defaults @@ -10,3 +10,5 @@ then fi set LOGGER_BUF 64 + +safety_button start diff --git a/boards/px4/fmu-v5/irqmonitor.cmake b/boards/px4/fmu-v5/irqmonitor.cmake index db3911f611..664d493931 100644 --- a/boards/px4/fmu-v5/irqmonitor.cmake +++ b/boards/px4/fmu-v5/irqmonitor.cmake @@ -52,6 +52,7 @@ px4_add_board( px4io rc_input roboclaw + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index 4fdb8867e1..6b0bce74ce 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -40,6 +40,7 @@ px4_add_board( px4io rc_input roboclaw + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index 03b0fef0ea..f72467f4e7 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -40,6 +40,7 @@ px4_add_board( px4io rc_input roboclaw + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index b35172fb1b..6f337e4c36 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -50,6 +50,7 @@ px4_add_board( px4io rc_input roboclaw + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index db3018de8e..1335d2b863 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -50,6 +50,7 @@ px4_add_board( px4io rc_input roboclaw + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index 9529c26ae1..3bab45afd2 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -52,6 +52,7 @@ px4_add_board( px4io rc_input roboclaw + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v5x/fixedwing.cmake b/boards/px4/fmu-v5x/fixedwing.cmake index e05bd5f446..436f509c8d 100644 --- a/boards/px4/fmu-v5x/fixedwing.cmake +++ b/boards/px4/fmu-v5x/fixedwing.cmake @@ -46,6 +46,7 @@ px4_add_board( px4fmu px4io rc_input + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v5x/init/rc.board_defaults b/boards/px4/fmu-v5x/init/rc.board_defaults index 9343ceebce..e818caaf97 100644 --- a/boards/px4/fmu-v5x/init/rc.board_defaults +++ b/boards/px4/fmu-v5x/init/rc.board_defaults @@ -10,3 +10,5 @@ then fi set LOGGER_BUF 64 + +safety_button start diff --git a/boards/px4/fmu-v5x/multicopter.cmake b/boards/px4/fmu-v5x/multicopter.cmake index 49eba6150c..46fdec308e 100644 --- a/boards/px4/fmu-v5x/multicopter.cmake +++ b/boards/px4/fmu-v5x/multicopter.cmake @@ -47,6 +47,7 @@ px4_add_board( px4io rc_input roboclaw + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v5x/rover.cmake b/boards/px4/fmu-v5x/rover.cmake index d695514e97..dd6a4186c6 100644 --- a/boards/px4/fmu-v5x/rover.cmake +++ b/boards/px4/fmu-v5x/rover.cmake @@ -50,6 +50,7 @@ px4_add_board( px4io rc_input roboclaw + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v5x/rtps.cmake b/boards/px4/fmu-v5x/rtps.cmake index c3126a0222..f69c8bdc2e 100644 --- a/boards/px4/fmu-v5x/rtps.cmake +++ b/boards/px4/fmu-v5x/rtps.cmake @@ -52,6 +52,7 @@ px4_add_board( px4io rc_input roboclaw + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/boards/px4/fmu-v5x/stackcheck.cmake b/boards/px4/fmu-v5x/stackcheck.cmake index 32799b48f8..8264d38d8d 100644 --- a/boards/px4/fmu-v5x/stackcheck.cmake +++ b/boards/px4/fmu-v5x/stackcheck.cmake @@ -52,6 +52,7 @@ px4_add_board( px4io rc_input roboclaw + safety_button stm32 stm32/adc stm32/tone_alarm diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 85656e5515..07af2605af 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -68,21 +68,9 @@ #include #include #include -#include using namespace time_literals; -static constexpr uint8_t CYCLE_COUNT = 10; /* safety switch must be held for 1 second to activate */ - -/* - * Define the various LED flash sequences for each system state. - */ -#define LED_PATTERN_FMU_OK_TO_ARM 0x0003 /**< slow blinking */ -#define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */ -#define LED_PATTERN_IO_ARMED 0x5050 /**< long off, then double blink */ -#define LED_PATTERN_FMU_ARMED 0x5500 /**< long off, then quad blink */ -#define LED_PATTERN_IO_FMU_ARMED 0xffff /**< constantly on */ - /** Mode given via CLI */ enum PortMode { PORT_MODE_UNSET = 0, @@ -184,8 +172,6 @@ private: Betaflight = 1 }; - hrt_abstime _cycle_timestamp = 0; - hrt_abstime _last_safety_check = 0; hrt_abstime _time_last_mix = 0; static constexpr uint8_t MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS; @@ -200,9 +186,7 @@ private: uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; uORB::Subscription _param_sub{ORB_ID(parameter_update)}; - uORB::Subscription _safety_sub{ORB_ID(safety)}; - uORB::Publication _to_safety{ORB_ID(safety)}; uORB::PublicationMulti _outputs_pub{ORB_ID(actuator_outputs), ORB_PRIO_DEFAULT}; uORB::PublicationMulti _to_mixer_status{ORB_ID(multirotor_motor_limits), ORB_PRIO_DEFAULT}; ///< mixer status flags @@ -241,10 +225,6 @@ private: unsigned _num_failsafe_set{0}; unsigned _num_disarmed_set{0}; - bool _safety_off{false}; ///< State of the safety button from the subscribed safety topic - bool _safety_btn_off{false}; ///< State of the safety button read from the HW button - bool _safety_disabled{false}; - float _mot_t_max{0.0f}; ///< maximum rise time for motor (slew rate limiting) float _thr_mdl_fac{0.0f}; ///< thrust to pwm modelling factor Mixer::Airmode _airmode{Mixer::Airmode::disabled}; ///< multicopter air-mode @@ -281,9 +261,6 @@ private: PX4FMU(const PX4FMU &) = delete; PX4FMU operator=(const PX4FMU &) = delete; - void safety_check_button(void); - void flash_safety_button(void); - /** * Reorder PWM outputs according to _motor_ordering * @param values PWM values to reorder @@ -313,12 +290,6 @@ PX4FMU::PX4FMU() : _armed.lockdown = false; _armed.force_failsafe = false; _armed.in_esc_calibration_mode = false; - - // If there is no safety button, disable it on boot. -#ifndef GPIO_BTN_SAFETY - _safety_off = true; - _safety_btn_off = true; -#endif } PX4FMU::~PX4FMU() @@ -355,13 +326,6 @@ PX4FMU::init() PX4_ERR("FAILED registering class device"); } - _safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY); - - if (_safety_disabled) { - _safety_off = true; - _safety_btn_off = true; - } - /* force a reset of the update rate */ _current_update_rate = 0; @@ -371,102 +335,9 @@ PX4FMU::init() // Getting initial parameter values update_params(); - ScheduleOnInterval(100_ms); // run at 10 Hz until mixers loaded - return 0; } -void -PX4FMU::safety_check_button(void) -{ -#ifdef GPIO_BTN_SAFETY - - if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { - - static int counter = 0; - /* - * Debounce the safety button, change state if it has been held for long enough. - * - */ - bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY); - - /* - * Keep pressed for a while to arm. - * - * Note that the counting sequence has to be same length - * for arming / disarming in order to end up as proper - * state machine, keep ARM_COUNTER_THRESHOLD the same - * length in all cases of the if/else struct below. - */ - if (safety_button_pressed && !_safety_btn_off) { - - if (counter < CYCLE_COUNT) { - counter++; - - } else if (counter == CYCLE_COUNT) { - /* switch to armed state */ - _safety_btn_off = true; - counter++; - } - - } else if (safety_button_pressed && _safety_btn_off) { - - if (counter < CYCLE_COUNT) { - counter++; - - } else if (counter == CYCLE_COUNT) { - /* change to disarmed state and notify the FMU */ - _safety_btn_off = false; - counter++; - } - - } else { - counter = 0; - } - } - -#endif -} - -void -PX4FMU::flash_safety_button() -{ -#if defined(GPIO_BTN_SAFETY) && defined(GPIO_LED_SAFETY) - - if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { - /* Select the appropriate LED flash pattern depending on the current arm state */ - uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; - - /* cycle the blink state machine at 10Hz */ - static int blink_counter = 0; - - if (_safety_btn_off) { - if (_armed.armed) { - pattern = LED_PATTERN_IO_FMU_ARMED; - - } else { - pattern = LED_PATTERN_IO_ARMED; - } - - } else if (_armed.armed) { - pattern = LED_PATTERN_FMU_ARMED; - - } else { - pattern = LED_PATTERN_FMU_OK_TO_ARM; - - } - - /* Turn the LED on if we have a 1 at the current bit position */ - px4_arch_gpiowrite(GPIO_LED_SAFETY, !(pattern & (1 << blink_counter++))); - - if (blink_counter > 15) { - blink_counter = 0; - } - } - -#endif -} - int PX4FMU::set_mode(Mode mode) { @@ -826,11 +697,6 @@ PX4FMU::subscribe() } } - // if nothing required keep periodic schedule for safety button - if (_groups_required == 0) { - ScheduleOnInterval(100_ms); - } - PX4_DEBUG("_groups_required 0x%08x", _groups_required); PX4_DEBUG("_groups_subscribed 0x%08x", _groups_subscribed); @@ -985,7 +851,7 @@ PX4FMU::Run() if (_armed_sub.update(&_armed)) { /* Update the armed status and check that we're not locked down. * We also need to arm throttle for the ESC calibration. */ - _throttle_armed = (_safety_off && _armed.armed && !_armed.lockdown) || (_safety_off && _armed.in_esc_calibration_mode); + _throttle_armed = (_armed.armed && !_armed.lockdown) || _armed.in_esc_calibration_mode; } unsigned n_updates = 0; @@ -1106,48 +972,6 @@ PX4FMU::Run() } } - _cycle_timestamp = hrt_absolute_time(); - -#ifdef GPIO_BTN_SAFETY - - if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { - - if (_cycle_timestamp - _last_safety_check >= (unsigned int)1e5) { - _last_safety_check = _cycle_timestamp; - - /** - * Get and handle the safety status at 10Hz - */ - struct safety_s safety = {}; - - if (!_safety_disabled) { - /* read safety switch input and control safety switch LED at 10Hz */ - safety_check_button(); - } - - /* Make the safety button flash anyway, no matter if it's used or not. */ - flash_safety_button(); - - safety.timestamp = hrt_absolute_time(); - safety.safety_switch_available = true; - safety.safety_off = _safety_btn_off; - - /* lazily publish the safety status */ - _to_safety.publish(safety); - } - } - -#endif - - // check safety button state - if (_safety_sub.updated()) { - safety_s safety; - - if (_safety_sub.copy(&safety)) { - _safety_off = !safety.safety_switch_available || safety.safety_off; - } - } - /* update PWM status if armed or if disarmed PWM values are set */ bool pwm_on = _armed.armed || (_num_disarmed_set > 0) || _armed.in_esc_calibration_mode; @@ -1313,16 +1137,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET_ARM_OK: case PWM_SERVO_CLEAR_ARM_OK: - break; - case PWM_SERVO_SET_FORCE_SAFETY_OFF: - /* force safety switch off */ - _safety_btn_off = true; - break; - case PWM_SERVO_SET_FORCE_SAFETY_ON: - /* force safety switch on */ - _safety_btn_off = false; break; case PWM_SERVO_DISARM: @@ -2851,12 +2667,6 @@ int PX4FMU::print_status() { PX4_INFO("Max update rate: %i Hz", _current_update_rate); -#ifdef GPIO_BTN_SAFETY - if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { - PX4_INFO("Safety State (from button): %s", _safety_btn_off ? "off" : "on"); - } -#endif - const char *mode_str = nullptr; switch (_mode) { diff --git a/src/drivers/safety_button/CMakeLists.txt b/src/drivers/safety_button/CMakeLists.txt new file mode 100644 index 0000000000..f1fb9c3e6b --- /dev/null +++ b/src/drivers/safety_button/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE drivers__safety_button + MAIN safety_button + SRCS + SafetyButton.cpp + DEPENDS + circuit_breaker + px4_work_queue + ) diff --git a/src/drivers/safety_button/SafetyButton.cpp b/src/drivers/safety_button/SafetyButton.cpp new file mode 100644 index 0000000000..2d1a2a202e --- /dev/null +++ b/src/drivers/safety_button/SafetyButton.cpp @@ -0,0 +1,236 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "SafetyButton.hpp" + +using namespace time_literals; + +static constexpr uint8_t CYCLE_COUNT{10}; /* safety switch must be held for 1 second to activate */ + +// Define the various LED flash sequences for each system state. +enum class LED_PATTERN : uint16_t { + FMU_OK_TO_ARM = 0x0003, /**< slow blinking */ + FMU_REFUSE_TO_ARM = 0x5555, /**< fast blinking */ + IO_ARMED = 0x5050, /**< long off, then double blink */ + FMU_ARMED = 0x5500, /**< long off, then quad blink */ + IO_FMU_ARMED = 0xffff, /**< constantly on */ +}; + +SafetyButton::~SafetyButton() +{ + ScheduleClear(); +} + +void +SafetyButton::CheckButton() +{ + // Debounce the safety button, change state if it has been held for long enough. + bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY); + + /* + * Keep pressed for a while to arm. + * + * Note that the counting sequence has to be same length + * for arming / disarming in order to end up as proper + * state machine, keep ARM_COUNTER_THRESHOLD the same + * length in all cases of the if/else struct below. + */ + if (safety_button_pressed && !_safety_btn_off) { + if (_button_counter < CYCLE_COUNT) { + _button_counter++; + + } else if (_button_counter == CYCLE_COUNT) { + // switch to armed state + _safety_btn_off = true; + _button_counter++; + } + + } else if (safety_button_pressed && _safety_btn_off) { + + if (_button_counter < CYCLE_COUNT) { + _button_counter++; + + } else if (_button_counter == CYCLE_COUNT) { + // change to disarmed state and notify + _safety_btn_off = false; + _button_counter++; + } + + } else { + _button_counter = 0; + } +} + +void +SafetyButton::FlashButton() +{ +#if defined(GPIO_LED_SAFETY) + actuator_armed_s armed; + + if (_armed_sub.copy(&armed)) { + // Select the appropriate LED flash pattern depending on the current arm state + LED_PATTERN pattern = LED_PATTERN::FMU_REFUSE_TO_ARM; + + // cycle the blink state machine at 10Hz + if (_safety_btn_off) { + if (armed.armed) { + pattern = LED_PATTERN::IO_FMU_ARMED; + + } else { + pattern = LED_PATTERN::IO_ARMED; + } + + } else if (armed.armed) { + pattern = LED_PATTERN::FMU_ARMED; + + } else { + pattern = LED_PATTERN::FMU_OK_TO_ARM; + } + + // Turn the LED on if we have a 1 at the current bit position + px4_arch_gpiowrite(GPIO_LED_SAFETY, !((uint16_t)pattern & (1 << _blink_counter++))); + + if (_blink_counter > 15) { + _blink_counter = 0; + } + } + +#endif // GPIO_LED_SAFETY +} + +void +SafetyButton::Run() +{ + if (should_exit()) { + exit_and_cleanup(); + } + + // read safety switch input and control safety switch LED at 10Hz + CheckButton(); + + // Make the safety button flash anyway, no matter if it's used or not. + FlashButton(); + + safety_s safety{}; + safety.timestamp = hrt_absolute_time(); + safety.safety_switch_available = true; + safety.safety_off = _safety_btn_off; + + // publish the safety status + _to_safety.publish(safety); +} + +int +SafetyButton::task_spawn(int argc, char *argv[]) +{ + if (PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { + PX4_ERR("not starting (use px4io for safety button)"); + + return PX4_ERROR; + + } else if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) { + PX4_WARN("disabled by CBRK_IO_SAFETY, exiting"); + return PX4_ERROR; + + } else { + SafetyButton *instance = new SafetyButton(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->Start() == PX4_OK) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + } + + return PX4_ERROR; +} + +int +SafetyButton::Start() +{ + ScheduleOnInterval(100_ms); // run at 10 Hz + + return PX4_OK; +} + +int +SafetyButton::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int +SafetyButton::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module is responsible for the safety button. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("safety_button", "driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the safety button driver"); + + return 0; +} + +int +SafetyButton::print_status() +{ + PX4_INFO("Safety State (from button): %s", _safety_btn_off ? "off" : "on"); + + return 0; +} + +extern "C" __EXPORT int safety_button_main(int argc, char *argv[]); + +int +safety_button_main(int argc, char *argv[]) +{ + return SafetyButton::main(argc, argv); +} diff --git a/src/drivers/safety_button/SafetyButton.hpp b/src/drivers/safety_button/SafetyButton.hpp new file mode 100644 index 0000000000..da59bca09b --- /dev/null +++ b/src/drivers/safety_button/SafetyButton.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class SafetyButton : public ModuleBase, public px4::ScheduledWorkItem +{ +public: + SafetyButton() : ScheduledWorkItem(px4::wq_configurations::hp_default) {} + virtual ~SafetyButton(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + void Run() override; + + int Start(); + +private: + + void CheckButton(); + void FlashButton(); + + + uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; + uORB::Publication _to_safety{ORB_ID(safety)}; + + uint8_t _button_counter{0}; + uint8_t _blink_counter{0}; + bool _safety_off{false}; ///< State of the safety button from the subscribed safety topic + bool _safety_btn_off{false}; ///< State of the safety button read from the HW button + +};