diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-27 21:34:07 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-27 21:34:07 +0200 |
commit | 92c3e52fec8945cede585da1e1236ab2ec5183f4 (patch) | |
tree | 660bfec8d26dec556a8063b3b5869eed5e4b0e10 /src | |
parent | 05027617996c86005fb8ec4d68fae798b9fbef35 (diff) | |
parent | 91d50301c61cf495e83cab59621ef83cff24da3a (diff) | |
download | px4-firmware-92c3e52fec8945cede585da1e1236ab2ec5183f4.tar.gz px4-firmware-92c3e52fec8945cede585da1e1236ab2ec5183f4.tar.bz2 px4-firmware-92c3e52fec8945cede585da1e1236ab2ec5183f4.zip |
Merge remote-tracking branch 'upstream/termination_failsafe' into obcfailsafe
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/drv_pwm_output.h | 7 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 22 | ||||
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 14 | ||||
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 3 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 18 | ||||
-rw-r--r-- | src/systemcmds/pwm/pwm.c | 21 |
6 files changed, 77 insertions, 8 deletions
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 5aff6825b..bc586f395 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -205,10 +205,13 @@ ORB_DECLARE(output_pwm); #define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22) /** force safety switch off (to disable use of safety switch) */ -#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) +#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) /** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */ -#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24) +#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24) + +/** make failsafe non-recoverable (termination) if it occurs */ +#define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25) /* * diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b5c5ef17c..6f8d6f5f6 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1176,6 +1176,14 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } + // XXX this is for future support in the commander + // but can be removed if unneeded + // if (armed.termination_failsafe) { + // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } else { + // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -2039,7 +2047,8 @@ PX4IO::print_status(bool extended_status) ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), ((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_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""), + ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "") ); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", @@ -2263,6 +2272,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) } break; + case PWM_SERVO_SET_TERMINATION_FAILSAFE: + /* if failsafe occurs, do not allow the system to recover */ + if (arg == 0) { + /* clear termination failsafe flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, 0); + } else { + /* set termination failsafe flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE); + } + break; + case DSM_BIND_START: /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 606c639f9..0c65b7642 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -139,7 +139,9 @@ mixer_tick(void) (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && - !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + /* do not enter manual override if we asked for termination failsafe and FMU is lost */ + !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; @@ -155,6 +157,16 @@ mixer_tick(void) } /* + * Check if failsafe termination is set - if yes, + * set the force failsafe flag once entering the first + * failsafe condition. + */ + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && + (source == MIX_FAILSAFE)) { + r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } + + /* * Check if we should force failsafe - and do it if we have to */ if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) { diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 050783687..eae7f9567 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -180,6 +180,7 @@ #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ #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_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 0da778b6f..7a5a5e484 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -190,7 +190,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ PX4IO_P_SETUP_ARMING_LOCKDOWN | \ - PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) + PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -518,6 +519,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; } + /* + * If the failsafe termination flag is set, do not allow the autopilot to unset it + */ + value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE); + + /* + * If failsafe termination is enabled and force failsafe bit is set, do not allow + * the autopilot to clear it. + */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) { + value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); + } + r_setup_arming = value; break; diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index c8d698b86..478c2a772 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -654,9 +654,28 @@ pwm_main(int argc, char *argv[]) } } exit(0); + } else if (!strcmp(argv[1], "terminatefail")) { + + if (argc < 3) { + errx(1, "arg missing [on|off]"); + } else { + + if (!strcmp(argv[2], "on")) { + /* force failsafe */ + ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 1); + } else { + /* force failsafe */ + ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 0); + } + + if (ret != OK) { + warnx("FAILED setting termination failsafe %s", argv[2]); + } + } + exit(0); } - usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info|forcefail"); + usage("specify arm|disarm|rate|failsafe\n\t\tdisarmed|min|max|test|info|forcefail|terminatefail"); return 0; } |