dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

add fmuv4 safety switch handling

This commit is contained in:
Mark Whitehorn 2016-02-07 17:21:38 -07:00 committed by Lorenz Meier
parent 5a7d31f7a9
commit ef2a7bda53
3 changed files with 132 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/drivers/boards/px4fmu-v4/board_config.h
View File

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

2
src/drivers/boards/px4fmu-v4/px4fmu_init.c
View File

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

133
src/drivers/px4fmu/fmu.cpp
View File

@ -86,6 +86,7 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/safety.h>
#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;