Merge pull request #2536 from PX4/safety_prearm

Safety prearm

ROMFS/px4fmu_common/init.d/3032_skywalker_x5

@@ -14,18 +14,16 @@ then
 	param set FW_AIRSPD_MAX 40
 	param set FW_ATT_TC 0.3
 	param set FW_L1_DAMPING 0.74
-	param set FW_L1_PERIOD 15
-	param set FW_PR_FF 0.3
-	param set FW_PR_I 0
-	param set FW_PR_IMAX 0.2
-	param set FW_PR_P 0.03
-	param set FW_P_ROLLFF 1
-	param set FW_RR_FF 0.3
-	param set FW_RR_I 0
-	param set FW_RR_IMAX 0.2
-	param set FW_RR_P 0.03
-	param set FW_R_LIM 60
-	param set FW_R_RMAX 0
+	param set FW_L1_PERIOD 16
+	param set FW_LND_ANG 15
+	param set FW_LND_FLALT 5
+	param set FW_LND_HHDIST 15
+	param set FW_LND_HVIRT 13
+	param set FW_LND_TLALT 5
+	param set FW_THR_LND_MAX 0
+	param set FW_PR_FF 0.35
+	param set FW_RR_FF 0.6
+	param set FW_RR_P 0.04
 fi
 
 set MIXER X5

ROMFS/px4fmu_common/init.d/rc.mc_defaults

@@ -20,3 +20,11 @@ set PWM_RATE 400
 set PWM_DISARMED 900
 set PWM_MIN 1075
 set PWM_MAX 2000
+
+# This is the gimbal pass mixer
+set MIXER_AUX pass
+set PWM_AUX_RATE 50
+set PWM_AUX_OUT 1234
+set PWM_AUX_DISARMED 1500
+set PWM_AUX_MIN 1000
+set PWM_AUX_MAX 2000

src/drivers/px4fmu/fmu.cpp

@@ -90,6 +90,7 @@
  */
 
 #define CONTROL_INPUT_DROP_LIMIT_MS		20
+#define NAN_VALUE	(0.0f/0.0f)
 
 class PX4FMU : public device::CDev
 {
@@ -136,7 +137,6 @@ private:
 	int		_armed_sub;
 	int		_param_sub;
 	orb_advert_t	_outputs_pub;
-	actuator_armed_s	_armed;
 	unsigned	_num_outputs;
 	int		_class_instance;
 
@@ -156,6 +156,7 @@ private:
 	unsigned	_poll_fds_num;
 
 	static pwm_limit_t	_pwm_limit;
+	static actuator_armed_s	_armed;
 	uint16_t	_failsafe_pwm[_max_actuators];
 	uint16_t	_disarmed_pwm[_max_actuators];
 	uint16_t	_min_pwm[_max_actuators];
@@ -164,6 +165,8 @@ private:
 	unsigned	_num_failsafe_set;
 	unsigned	_num_disarmed_set;
 
+	static bool	arm_nothrottle() { return (_armed.ready_to_arm && !_armed.armed); }
+
 	static void	task_main_trampoline(int argc, char *argv[]);
 	void		task_main();
 
@@ -240,8 +243,9 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
 #endif
 };
 
-const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
-pwm_limit_t	PX4FMU::_pwm_limit;
+const unsigned		PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
+pwm_limit_t		PX4FMU::_pwm_limit;
+actuator_armed_s	PX4FMU::_armed = {};
 
 namespace
 {
@@ -261,7 +265,6 @@ PX4FMU::PX4FMU() :
 	_armed_sub(-1),
 	_param_sub(-1),
 	_outputs_pub(-1),
-	_armed{},
 	_num_outputs(0),
 	_class_instance(0),
 	_task_should_exit(false),
@@ -695,24 +698,17 @@ PX4FMU::task_main()
 				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
 				outputs.timestamp = hrt_absolute_time();
 
-				/* iterate actuators */
+				/* disable unused ports by setting their output to NaN */
 				for (unsigned i = 0; i < num_outputs; i++) {
-					/* last resort: catch NaN and INF */
-					if ((i >= outputs.noutputs) ||
-						!isfinite(outputs.output[i])) {
-						/*
-						 * Value is NaN, INF or out of band - set to the minimum value.
-						 * This will be clearly visible on the servo status and will limit the risk of accidentally
-						 * spinning motors. It would be deadly in flight.
-						 */
-						outputs.output[i] = -1.0f;
+					if (i >= outputs.noutputs) {
+						outputs.output[i] = NAN_VALUE;
 					}
 				}
 
 				uint16_t pwm_limited[num_outputs];
 
-				/* the PWM limit call takes care of out of band errors and constrains */
-				pwm_limit_calc(_servo_armed, num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
+				/* the PWM limit call takes care of out of band errors, NaN and constrains */
+				pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
 
 				/* output to the servos */
 				for (unsigned i = 0; i < num_outputs; i++) {
@@ -737,13 +733,14 @@ PX4FMU::task_main()
 			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 
 			/* update the armed status and check that we're not locked down */
-			bool set_armed = _armed.armed && !_armed.lockdown;
+			bool set_armed = (_armed.armed || _armed.ready_to_arm) && !_armed.lockdown;
 
-			if (_servo_armed != set_armed)
+			if (_servo_armed != set_armed) {
 				_servo_armed = set_armed;
+			}
 
 			/* update PWM status if armed or if disarmed PWM values are set */
-			bool pwm_on = (_armed.armed || _num_disarmed_set > 0);
+			bool pwm_on = (set_armed || _num_disarmed_set > 0);
 
 			if (_pwm_on != pwm_on) {
 				_pwm_on = pwm_on;
@@ -828,6 +825,13 @@ PX4FMU::control_callback(uintptr_t handle,
 
 	input = controls[control_group].control[control_index];
 
+	/* limit control input */
+	if (input > 1.0f) {
+		input = 1.0f;
+	} else if (input < -1.0f) {
+		input = -1.0f;
+	}
+
 	/* motor spinup phase - lock throttle to zero */
 	if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
 		if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
@@ -839,6 +843,15 @@ PX4FMU::control_callback(uintptr_t handle,
 		}
 	}
 
+	/* throttle not arming - mark throttle input as invalid */
+	if (arm_nothrottle()) {
+		if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
+			control_index == actuator_controls_s::INDEX_THROTTLE) {
+			/* set the throttle to an invalid value */
+			input = NAN_VALUE;
+		}
+	}
+
 	return 0;
 }
 
@@ -847,14 +860,12 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret;
 
-	// XXX disabled, confusing users
-	//debug("ioctl 0x%04x 0x%08x", cmd, arg);
-
 	/* try it as a GPIO ioctl first */
 	ret = gpio_ioctl(filp, cmd, arg);
 
-	if (ret != -ENOTTY)
+	if (ret != -ENOTTY) {
 		return ret;
+	}
 
 	/* if we are in valid PWM mode, try it as a PWM ioctl as well */
 	switch (_mode) {
@@ -873,8 +884,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
 	}
 
 	/* if nobody wants it, let CDev have it */
-	if (ret == -ENOTTY)
+	if (ret == -ENOTTY) {
 		ret = CDev::ioctl(filp, cmd, arg);
+	}
 
 	return ret;
 }

src/modules/px4iofirmware/mixer.cpp

@@ -62,6 +62,7 @@ extern "C" {
  * Maximum interval in us before FMU signal is considered lost
  */
 #define FMU_INPUT_DROP_LIMIT_US		500000
+#define NAN_VALUE	(0.0f/0.0f)
 
 /* current servo arm/disarm state */
 static bool mixer_servos_armed = false;
@@ -243,12 +244,12 @@ mixer_tick(void)
 		in_mixer = false;
 
 		/* the pwm limit call takes care of out of band errors */
-		pwm_limit_calc(should_arm, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
+		pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
 		/* clamp unused outputs to zero */
 		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
 			r_page_servos[i] = 0;
-			outputs[i] = 0;
+			outputs[i] = 0.0f;
 		}
 
 		/* store normalized outputs */
@@ -280,7 +281,7 @@ mixer_tick(void)
 		isr_debug(5, "> PWM disabled");
 	}
 
-	if (mixer_servos_armed && should_arm) {
+	if (mixer_servos_armed && (should_arm || should_arm_nothrottle)) {
 		/* update the servo outputs. */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
 			up_pwm_servo_set(i, r_page_servos[i]);
@@ -369,7 +370,14 @@ mixer_callback(uintptr_t handle,
 		}
 	}
 
-	/* motor spinup phase or only safety off, but not armed - lock throttle to zero */
+	/* limit output */
+	if (control > 1.0f) {
+		control = 1.0f;
+	} else if (control < -1.0f) {
+		control = -1.0f;
+	}
+
+	/* motor spinup phase - lock throttle to zero */
 	if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) {
 		if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
 			control_index == actuator_controls_s::INDEX_THROTTLE) {
@@ -380,11 +388,13 @@ mixer_callback(uintptr_t handle,
 		}
 	}
 
-	/* limit output */
-	if (control > 1.0f) {
-		control = 1.0f;
-	} else if (control < -1.0f) {
-		control = -1.0f;
+	/* only safety off, but not armed - set throttle as invalid */
+	if (should_arm_nothrottle && !should_arm) {
+		if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
+			control_index == actuator_controls_s::INDEX_THROTTLE) {
+			/* mark the throttle as invalid */
+			control = NAN_VALUE;
+		}
 	}
 
 	return 0;

src/modules/systemlib/pwm_limit/pwm_limit.c

@@ -37,6 +37,7 @@
  * Library for PWM output limiting
  *
  * @author Julian Oes <julian@px4.io>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 #include "pwm_limit.h"
@@ -54,7 +55,7 @@ void pwm_limit_init(pwm_limit_t *limit)
 	return;
 }
 
-void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t reverse_mask,
+void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask,
 	const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm,
 	const float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
 {
@@ -99,10 +100,23 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 			break;
 	}
 
+	/* if the system is pre-armed, the limit state is temporarily on,
+	 * as some outputs are valid and the non-valid outputs have been
+	 * set to NaN. This is not stored in the state machine though,
+	 * as the throttle channels need to go through the ramp at
+	 * regular arming time.
+	 */
+
+	 unsigned local_limit_state = limit->state;
+
+	if (pre_armed) {
+		local_limit_state = PWM_LIMIT_STATE_ON;
+	}
+
 	unsigned progress;
 
 	/* then set effective_pwm based on state */
-	switch (limit->state) {
+	switch (local_limit_state) {
 		case PWM_LIMIT_STATE_OFF:
 		case PWM_LIMIT_STATE_INIT:
 			for (unsigned i=0; i<num_channels; i++) {
@@ -120,6 +134,14 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 				}
 
 				for (unsigned i=0; i<num_channels; i++) {
+
+					float control_value = output[i];
+
+					/* check for invalid / disabled channels */
+					if (!isfinite(control_value)) {
+						effective_pwm[i] = disarmed_pwm[i];
+						continue;
+					}
 	                
 					uint16_t ramp_min_pwm;
 	                
@@ -141,8 +163,6 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 						ramp_min_pwm = min_pwm[i];
 					}
 
-					float control_value = output[i];
-
 					if (reverse_mask & (1 << i)) {
 						control_value = -1.0f * control_value;
 					}
@@ -163,6 +183,12 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 
 				float control_value = output[i];
 
+				/* check for invalid / disabled channels */
+				if (!isfinite(control_value)) {
+					effective_pwm[i] = disarmed_pwm[i];
+					continue;
+				}
+
 				if (reverse_mask & (1 << i)) {
 					control_value = -1.0f * control_value;
 				}

src/modules/systemlib/pwm_limit/pwm_limit.h

@@ -71,7 +71,7 @@ typedef struct {
 
 __EXPORT void pwm_limit_init(pwm_limit_t *limit);
 
-__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_pwm,
+__EXPORT void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_pwm,
 			     const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
 
 __END_DECLS

src/systemcmds/tests/test_mixer.cpp

@@ -56,6 +56,8 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
 
+#include <uORB/topics/actuator_controls.h>
+
 #include "tests.h"
 
 static int	mixer_callback(uintptr_t handle,
@@ -65,6 +67,9 @@ static int	mixer_callback(uintptr_t handle,
 
 const unsigned output_max = 8;
 static float actuator_controls[output_max];
+static bool should_prearm = false;
+
+#define NAN_VALUE 0.0f/0.0f
 
 int test_mixer(int argc, char *argv[])
 {
@@ -72,7 +77,7 @@ int test_mixer(int argc, char *argv[])
 	 * PWM limit structure
 	 */
 	pwm_limit_t pwm_limit;
-	static bool should_arm = false;
+	bool should_arm = false;
 	uint16_t r_page_servo_disarmed[output_max];
 	uint16_t r_page_servo_control_min[output_max];
 	uint16_t r_page_servo_control_max[output_max];
@@ -184,7 +189,6 @@ int test_mixer(int argc, char *argv[])
 	const int jmax = 5;
 
 	pwm_limit_init(&pwm_limit);
-	should_arm = true;
 
 	/* run through arming phase */
 	for (unsigned i = 0; i < output_max; i++) {
@@ -194,6 +198,40 @@ int test_mixer(int argc, char *argv[])
 		r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
 	}
 
+	warnx("PRE-ARM TEST: DISABLING SAFETY");
+	/* mix */
+	should_prearm = true;
+	mixed = mixer_group.mix(&outputs[0], output_max, NULL);
+
+	pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
+		r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
+
+	//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
+	for (unsigned i = 0; i < mixed; i++) {
+
+		warnx("pre-arm:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]);
+
+		if (i != actuator_controls_s::INDEX_THROTTLE) {
+			if (r_page_servos[i] < r_page_servo_control_min[i]) {
+				warnx("active servo < min");
+				return 1;
+			}
+		} else {
+			if (r_page_servos[i] != r_page_servo_disarmed[i]) {
+				warnx("throttle output != 0 (this check assumed the IO pass mixer!)");
+				return 1;
+			}
+		}
+	}
+
+	should_arm = true;
+	should_prearm = false;
+
+	/* simulate another orb_copy() from actuator controls */
+	for (unsigned i = 0; i < output_max; i++) {
+		actuator_controls[i] = 0.1f;
+	}
+
 	warnx("ARMING TEST: STARTING RAMP");
 	unsigned sleep_quantum_us = 10000;
 
@@ -205,15 +243,18 @@ int test_mixer(int argc, char *argv[])
 		/* mix */
 		mixed = mixer_group.mix(&outputs[0], output_max, NULL);
 
-		pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
+		pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
 			r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
 		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
 		for (unsigned i = 0; i < mixed; i++) {
+
+			warnx("ramp:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]);
+
 			/* check mixed outputs to be zero during init phase */
 			if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
 			    r_page_servos[i] != r_page_servo_disarmed[i]) {
-				warnx("disarmed servo value mismatch");
+				warnx("disarmed servo value mismatch: %d vs %d", r_page_servos[i], r_page_servo_disarmed[i]);
 				return 1;
 			}
 
@@ -222,8 +263,6 @@ int test_mixer(int argc, char *argv[])
 				warnx("ramp servo value mismatch");
 				return 1;
 			}
-
-			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]);
 		}
 
 		usleep(sleep_quantum_us);
@@ -251,7 +290,7 @@ int test_mixer(int argc, char *argv[])
 		/* mix */
 		mixed = mixer_group.mix(&outputs[0], output_max, NULL);
 
-		pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
+		pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
 			       r_page_servos, &pwm_limit);
 
 		warnx("mixed %d outputs (max %d)", mixed, output_max);
@@ -278,18 +317,19 @@ int test_mixer(int argc, char *argv[])
 		/* mix */
 		mixed = mixer_group.mix(&outputs[0], output_max, NULL);
 
-		pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
+		pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
 			       r_page_servos, &pwm_limit);
 
 		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
 		for (unsigned i = 0; i < mixed; i++) {
+
+			warnx("disarmed:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]);
+
 			/* check mixed outputs to be zero during init phase */
 			if (r_page_servos[i] != r_page_servo_disarmed[i]) {
 				warnx("disarmed servo value mismatch");
 				return 1;
 			}
-
-			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
 		}
 
 		usleep(sleep_quantum_us);
@@ -314,7 +354,7 @@ int test_mixer(int argc, char *argv[])
 		/* mix */
 		mixed = mixer_group.mix(&outputs[0], output_max, NULL);
 
-		pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
+		pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
 			       r_page_servos, &pwm_limit);
 
 		//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
@@ -324,6 +364,8 @@ int test_mixer(int argc, char *argv[])
 
 			/* check ramp */
 
+			warnx("ramp:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]);
+
 			if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
 			    (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
 			     r_page_servos[i] > servo_predicted[i])) {
@@ -338,8 +380,6 @@ int test_mixer(int argc, char *argv[])
 				warnx("mixer violated predicted value");
 				return 1;
 			}
-
-			//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
 		}
 
 		usleep(sleep_quantum_us);
@@ -397,5 +437,10 @@ mixer_callback(uintptr_t handle,
 
 	control = actuator_controls[control_index];
 
+	if (should_prearm && control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
+		control_index == actuator_controls_s::INDEX_THROTTLE) {
+		control = NAN_VALUE;
+	}
+
 	return 0;
 }