aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/drivers/drv_pwm_output.h7
-rw-r--r--src/drivers/px4io/px4io.cpp22
-rw-r--r--src/modules/px4iofirmware/mixer.cpp62
-rw-r--r--src/modules/px4iofirmware/protocol.h4
-rw-r--r--src/modules/px4iofirmware/registers.c18
-rw-r--r--src/systemcmds/pwm/pwm.c21
6 files changed, 106 insertions, 28 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 97919538f..3fdb1dd1f 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1175,6 +1175,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;
@@ -2038,7 +2046,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",
@@ -2262,6 +2271,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..3e19333d8 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -58,7 +58,7 @@ extern "C" {
/*
* Maximum interval in us before FMU signal is considered lost
*/
-#define FMU_INPUT_DROP_LIMIT_US 200000
+#define FMU_INPUT_DROP_LIMIT_US 500000
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
#define ROLL 0
@@ -98,7 +98,8 @@ mixer_tick(void)
{
/* check that we are receiving fresh data from the FMU */
- if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
+ if ((system_state.fmu_data_received_time == 0) ||
+ hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
/* too long without FMU input, time to go to failsafe */
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
@@ -109,6 +110,9 @@ mixer_tick(void)
} else {
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
+
+ /* this flag is never cleared once OK */
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED;
}
/* default to failsafe mixing - it will be forced below if flag is set */
@@ -139,7 +143,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,29 +161,11 @@ mixer_tick(void)
}
/*
- * Check if we should force failsafe - and do it if we have to
- */
- if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
- source = MIX_FAILSAFE;
- }
-
- /*
- * Set failsafe status flag depending on mixing source
- */
- if (source == MIX_FAILSAFE) {
- r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
- } else {
- r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
- }
-
- /*
* Decide whether the servos should be armed right now.
*
* We must be armed, and we must have a PWM source; either raw from
* FMU or from the mixer.
*
- * XXX correct behaviour for failsafe may require an additional case
- * here.
*/
should_arm = (
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
@@ -195,6 +183,38 @@ mixer_tick(void)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
/*
+ * Check if failsafe termination is set - if yes,
+ * set the force failsafe flag once entering the first
+ * failsafe condition.
+ */
+ if ( /* if we have requested flight termination style failsafe (noreturn) */
+ (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) &&
+ /* and we ended up in a failsafe condition */
+ (source == MIX_FAILSAFE) &&
+ /* and we should be armed, so we intended to provide outputs */
+ should_arm &&
+ /* and FMU is initialized */
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) {
+ 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) {
+ source = MIX_FAILSAFE;
+ }
+
+ /*
+ * Set failsafe status flag depending on mixing source
+ */
+ if (source == MIX_FAILSAFE) {
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
+ } else {
+ r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
+ }
+
+ /*
* Run the mixers.
*/
if (source == MIX_FAILSAFE) {
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 050783687..4739f6e40 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
@@ -111,6 +111,7 @@
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
+#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */
@@ -180,6 +181,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;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-08 20:26:29 +0000