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

px4io: added OVERRIDE_IMMEDIATE arming flag 

this allows the flight code to choose whether FMU failure gives
immediate manual pilot control, or waits for the mode switch to go
past the override threshold
This commit is contained in:
Andrew Tridgell 2014-11-05 21:20:03 +11:00 committed by Lorenz Meier
parent 6406e235d6
commit ba81125453
5 changed files with 31 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/drivers/drv_pwm_output.h
View File

@ -242,6 +242,9 @@ ORB_DECLARE(output_pwm);
/** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
#define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29)
/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */
#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _IOC(_PWM_SERVO_BASE, 30)
/*
*
*

18
src/drivers/px4io/px4io.cpp
View File

@ -2057,7 +2057,7 @@ PX4IO::print_status(bool extended_status)
((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
);
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s%s%s%s%s\n",
printf("arming 0x%04x%s%s%s%s%s%s%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"),
@ -2067,7 +2067,8 @@ PX4IO::print_status(bool extended_status)
((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""),
((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""),
((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""),
((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "")
((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""),
((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "")
);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
@ -2307,6 +2308,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
}
break;
case PWM_SERVO_SET_OVERRIDE_IMMEDIATE:
/* control whether override on FMU failure is
immediate or waits for override threshold on mode
switch */
if (arg == 0) {
/* clear override immediate flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0);
} else {
/* set override immediate flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE);
}
break;
case DSM_BIND_START:
/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */

9
src/modules/px4iofirmware/controls.c
View File

@ -417,6 +417,15 @@ controls_tick() {
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
override = true;
/*
if the FMU is dead then enable override if we have a
mixer and OVERRIDE_IMMEDIATE is set
*/
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
override = true;
if (override) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;

1
src/modules/px4iofirmware/protocol.h
View File

@ -183,6 +183,7 @@
#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */
#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */
#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 10) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */

3
src/modules/px4iofirmware/registers.c
View File

@ -191,7 +191,8 @@ volatile uint16_t r_page_setup[] =
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
PX4IO_P_SETUP_ARMING_LOCKDOWN | \
PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \
PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)