aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/drivers/drv_pwm_output.h7
-rw-r--r--src/drivers/px4io/px4io.cpp22
-rw-r--r--src/modules/px4iofirmware/mixer.cpp14
-rw-r--r--src/modules/px4iofirmware/protocol.h3
-rw-r--r--src/modules/px4iofirmware/registers.c18
-rw-r--r--src/systemcmds/pwm/pwm.c21
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;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-11 06:00:04 +0000