From ef2a7bda5315538c60c7b4ef5a98154f66fc11e0 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sun, 7 Feb 2016 17:21:38 -0700 Subject: [PATCH] add fmuv4 safety switch handling --- src/drivers/boards/px4fmu-v4/board_config.h | 2 +- src/drivers/boards/px4fmu-v4/px4fmu_init.c | 2 + src/drivers/px4fmu/fmu.cpp | 133 +++++++++++++++++++- 3 files changed, 132 insertions(+), 5 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index a97096a04e..d2fde3d30c 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -239,7 +239,7 @@ __BEGIN_DECLS #define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 #define GPIO_RSSI_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) -#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3) +#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN3) #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) #define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) /* for R07, this signal is active low */ diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c index 2484cf7347..69c5a00521 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c @@ -228,6 +228,8 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); stm32_configgpio(GPIO_8266_PD); stm32_configgpio(GPIO_8266_RST); + stm32_configgpio(GPIO_BTN_SAFETY); + #ifdef GPIO_RC_OUT stm32_configgpio(GPIO_RC_OUT); /* Serial RC output pin */ stm32_gpiowrite(GPIO_RC_OUT, 1); /* set it high to pull RC input up */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 5db433d352..4901228b50 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -86,6 +86,7 @@ #include #include #include +#include #ifdef HRT_PPM_CHANNEL @@ -94,6 +95,17 @@ #define SCHEDULE_INTERVAL 2000 /**< The schedule interval in usec (500 Hz) */ #define NAN_VALUE (0.0f/0.0f) /**< NaN value for throttle lock mode */ +#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) +#define CYCLE_FREQUENCY 250 /* 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 */ class PX4FMU : public device::CDev { @@ -196,6 +208,8 @@ private: uint16_t _reverse_pwm_mask; unsigned _num_failsafe_set; unsigned _num_disarmed_set; + bool _safety_off; + orb_advert_t _to_safety; static bool arm_nothrottle() { return (_armed.prearmed && !_armed.armed); } @@ -248,6 +262,7 @@ private: void set_rc_scan_state(RC_SCAN _rc_scan_state); void rc_io_invert(); void rc_io_invert(bool invert); + void safety_check_button(void); }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { @@ -344,7 +359,9 @@ PX4FMU::PX4FMU() : _disarmed_pwm{0}, _reverse_pwm_mask(0), _num_failsafe_set(0), - _num_disarmed_set(0) + _num_disarmed_set(0), + _safety_off(false), + _to_safety(nullptr) { for (unsigned i = 0; i < _max_actuators; i++) { _min_pwm[i] = PWM_DEFAULT_MIN; @@ -422,6 +439,88 @@ PX4FMU::init() return OK; } +void +PX4FMU:: safety_check_button(void) +{ + static int counter = 0; + /* + * Debounce the safety button, change state if it has been held for long enough. + * + */ + bool safety_button_pressed = BUTTON_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. + */ +// warnx("b: %u, c: %u", safety_button_pressed, counter); + if (safety_button_pressed && !_safety_off +// && (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) + ) { + + if (counter < CYCLE_FREQUENCY) { + counter++; + + } else if (counter == CYCLE_FREQUENCY) { + /* switch to armed state */ + warnx("safety off"); + _safety_off = true; + counter++; + } + + } else if (safety_button_pressed && _safety_off) { + + if (counter < CYCLE_FREQUENCY) { + counter++; + + } else if (counter == CYCLE_FREQUENCY) { + /* change to disarmed state and notify the FMU */ + warnx("safety on"); + _safety_off = false; + counter++; + } + } else { + counter = 0; + } + + /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ + uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; + + /* cycle the blink state machine at 10Hz */ + static int divider = 0; + static int blink_counter = 0; + if (divider++ >= (CYCLE_FREQUENCY/10)) { + divider = 0; + + if (_safety_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 */ + stm32_gpiowrite(GPIO_LED_SAFETY, !(pattern & (1 << blink_counter++))); + + if (blink_counter > 15) { + blink_counter = 0; + } + } +} + int PX4FMU::set_mode(Mode mode) { @@ -953,6 +1052,32 @@ PX4FMU::cycle() } } + /* check safety switch */ + safety_check_button(); + + /** + * Get and handle the safety status + */ + struct safety_s safety; + safety.timestamp = hrt_absolute_time(); + + if (_safety_off) { + safety.safety_off = true; + safety.safety_switch_available = true; + + } else { + safety.safety_off = false; + safety.safety_switch_available = true; + } + + /* lazily publish the safety status */ + if (_to_safety != nullptr) { + orb_publish(ORB_ID(safety), _to_safety, &safety); + + } else { + _to_safety = orb_advertise(ORB_ID(safety), &safety); + } + /* check arming state */ bool updated = false; orb_check(_armed_sub, &updated); @@ -960,11 +1085,11 @@ PX4FMU::cycle() if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - /* update the armed status */ - _throttle_armed = _armed.armed; + /* update the armed status and check that we're not locked down */ + _throttle_armed = _safety_off && _armed.armed && !_armed.lockdown; /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (_armed.armed || _num_disarmed_set > 0); + bool pwm_on = _safety_off && (_armed.armed || _num_disarmed_set > 0); if (_pwm_on != pwm_on) { _pwm_on = pwm_on;