Merge branch 'master' into mpc_track

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

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

src/modules/dataman/dataman.c

@@ -797,7 +797,7 @@ start(void)
 	sem_init(&g_init_sema, 1, 0);
 
 	/* start the worker thread */
-	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) {
+	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) {
 		warn("task start failed");
 		return -1;
 	}

src/modules/navigator/geofence.cpp

@@ -76,11 +76,7 @@ Geofence::~Geofence()
 
 bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
 {
-	double lat = vehicle->lat / 1e7d;
-	double lon = vehicle->lon / 1e7d;
-	//float	alt = vehicle->alt;
-
-	return inside(lat, lon, vehicle->alt);
+	return inside(vehicle->lat, vehicle->lon, vehicle->alt);
 }
 
 bool Geofence::inside(double lat, double lon, float altitude)

src/modules/navigator/mission_feasibility_checker.cpp

@@ -84,7 +84,12 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
 bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
 {
 
-	return  (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
+	/* Perform checks and issue feedback to the user for all checks */
+	bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
+	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
+
+	/* Mission is only marked as feasible if all checks return true */
+	return (resGeofence && resHomeAltitude);
 }
 
 bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
@@ -93,7 +98,13 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
 	updateNavigationCapabilities();
 //	warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
 
-	return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
+	/* Perform checks and issue feedback to the user for all checks */
+	bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
+	bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
+	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
+
+	/* Mission is only marked as feasible if all checks return true */
+	return (resLanding && resGeofence && resHomeAltitude);
 }
 
 bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
@@ -216,9 +227,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
 		}
 	}
 
-
-//	float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
-	return false;
+	/* No landing waypoints or no waypoints */
+	return true;
 }
 
 void MissionFeasibilityChecker::updateNavigationCapabilities()

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;
@@ -154,30 +160,12 @@ 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)
@@ -194,6 +182,38 @@ mixer_tick(void)
 						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
 						&& (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.
 	 */

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

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;

src/modules/systemlib/mixer/mixer.h

@@ -130,6 +130,9 @@
 
 #include <nuttx/config.h>
 #include "drivers/drv_mixer.h"
+
+#include <uORB/topics/multirotor_motor_limits.h>
+
 #include "mixer_load.h"
 
 /**
@@ -531,6 +534,9 @@ private:
 	float				_yaw_scale;
 	float				_idle_speed;
 
+	orb_advert_t			_limits_pub;
+	multirotor_motor_limits_s 	_limits;
+
 	unsigned			_rotor_count;
 	const Rotor			*_rotors;
 

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -36,7 +36,8 @@
  *
  * Multi-rotor mixers.
  */
-
+#include <uORB/uORB.h>
+#include <uORB/topics/multirotor_motor_limits.h>
 #include <nuttx/config.h>
 
 #include <sys/types.h>
@@ -302,6 +303,11 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 	float		min_out = 0.0f;
 	float		max_out = 0.0f;
 
+	_limits.roll_pitch = false;
+	_limits.yaw = false;
+	_limits.throttle_upper = false;
+	_limits.throttle_lower = false;
+
 	/* perform initial mix pass yielding unbounded outputs, ignore yaw */
 	for (unsigned i = 0; i < _rotor_count; i++) {
 		float out = roll * _rotors[i].roll_scale +
@@ -311,6 +317,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 		/* limit yaw if it causes outputs clipping */
 		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
 			yaw = -out / _rotors[i].yaw_scale;
+			_limits.yaw = true;
 		}
 
 		/* calculate min and max output values */
@@ -332,6 +339,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 		for (unsigned i = 0; i < _rotor_count; i++) {
 			outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust;
 		}
+		_limits.roll_pitch = true;
 
 	} else {
 		/* roll/pitch mixed without limiting, add yaw control */
@@ -344,6 +352,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 	float scale_out;
 	if (max_out > 1.0f) {
 		scale_out = 1.0f / max_out;
+		_limits.throttle_upper = true;
 
 	} else {
 		scale_out = 1.0f;
@@ -351,9 +360,20 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 
 	/* scale outputs to range _idle_speed..1, and do final limiting */
 	for (unsigned i = 0; i < _rotor_count; i++) {
+		if (outputs[i] < _idle_speed) {
+			_limits.throttle_lower = true;
+		}
 		outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f);
 	}
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+        /* publish/advertise motor limits if running on FMU */
+        if (_limits_pub > 0) {
+            orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits);
+        } else {
+            _limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits);
+        }
+#endif
 	return _rotor_count;
 }
 

src/modules/uORB/objects_common.cpp

@@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
 
+#include "topics/multirotor_motor_limits.h"
+ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s);
+
 #include "topics/telemetry_status.h"
 ORB_DEFINE(telemetry_status_0, struct telemetry_status_s);
 ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);

src/modules/uORB/topics/multirotor_motor_limits.h

@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file multirotor_motor_limits.h
+ *
+ * Definition of multirotor_motor_limits  topic
+ */
+
+#ifndef MULTIROTOR_MOTOR_LIMITS_H_
+#define MULTIROTOR_MOTOR_LIMITS_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Motor limits
+ */
+struct multirotor_motor_limits_s {
+        uint8_t roll_pitch	: 1; // roll/pitch limit reached
+        uint8_t yaw		: 1; // yaw limit reached
+        uint8_t throttle_lower	: 1; // lower throttle limit reached
+        uint8_t throttle_upper	: 1; // upper throttle limit reached
+        uint8_t reserved	: 4;
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(multirotor_motor_limits);
+
+#endif

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;
 }