Merged beta into master

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_L1_PERIOD 16
-	param set FW_PR_FF 0.3
+	param set FW_LND_ANG 15
-	param set FW_PR_I 0
+	param set FW_LND_FLALT 5
-	param set FW_PR_IMAX 0.2
+	param set FW_LND_HHDIST 15
-	param set FW_PR_P 0.03
+	param set FW_LND_HVIRT 13
-	param set FW_P_ROLLFF 1
+	param set FW_LND_TLALT 5
-	param set FW_RR_FF 0.3
+	param set FW_THR_LND_MAX 0
-	param set FW_RR_I 0
+	param set FW_PR_FF 0.35
-	param set FW_RR_IMAX 0.2
+	param set FW_RR_FF 0.6
-	param set FW_RR_P 0.03
+	param set FW_RR_P 0.04
-	param set FW_R_LIM 60
-	param set FW_R_RMAX 0
 fi
 
 set MIXER X5

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -23,3 +23,11 @@ then
 	param set PE_GBIAS_PNOISE 0.000001
 	param set PE_ABIAS_PNOISE 0.0002
 fi
+
+# 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

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();
 
@@ -242,6 +245,7 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
 
 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(nullptr),
-	_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) {
-					if ((i >= outputs.noutputs) ||
+						outputs.output[i] = NAN_VALUE;
-						!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;
 					}
 				}
 
 				uint16_t pwm_limited[num_outputs];
 
-				/* the PWM limit call takes care of out of band errors and constrains */
+				/* the PWM limit call takes care of out of band errors, NaN and constrains */
-				pwm_limit_calc(_servo_armed, num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
+				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/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -97,7 +97,7 @@ static int	_control_task = -1;			/**< task handle for sensor task */
 #define HDG_HOLD_DIST_NEXT 			3000.0f 	// initial distance of waypoint in front of plane in heading hold mode
 #define HDG_HOLD_REACHED_DIST 		1000.0f 	// distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
 #define HDG_HOLD_SET_BACK_DIST 		100.0f 		// distance by which previous waypoint is set behind the plane
-#define HDG_HOLD_YAWRATE_THRESH 	0.1f 		// max yawrate at which plane locks yaw for heading hold mode
+#define HDG_HOLD_YAWRATE_THRESH 	0.15f 		// max yawrate at which plane locks yaw for heading hold mode
 #define HDG_HOLD_MAN_INPUT_THRESH 	0.01f 		// max manual roll input from user which does not change the locked heading
 #define TAKEOFF_IDLE				0.2f 		// idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0)
 

src/modules/mavlink/mavlink.c

@@ -60,7 +60,7 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
  * @min 1
  * @max 250
  */
-PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
+PARAM_DEFINE_INT32(MAV_COMP_ID, 1);
 
 /**
  * MAVLink Radio ID

src/modules/px4iofirmware/mixer.cpp

@@ -35,6 +35,8 @@
  * @file mixer.cpp
  *
  * Control channel input/output mixer and failsafe.
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 #include <px4_config.h>
@@ -60,10 +62,12 @@ 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;
 static bool should_arm = false;
+static bool should_arm_nothrottle = false;
 static bool should_always_enable_pwm = false;
 static volatile bool in_mixer = false;
 
@@ -172,6 +176,11 @@ mixer_tick(void)
 						     )
 	);
 
+	should_arm_nothrottle = (
+		/* IO initialised without error */   (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+		/* and IO is armed */ 		  && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+		/* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK));
+
 	should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
 						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
 						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
@@ -235,26 +244,27 @@ 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);
 
-		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
+		/* clamp unused outputs to zero */
+		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
 			r_page_servos[i] = 0;
+			outputs[i] = 0.0f;
+		}
 
+		/* store normalized outputs */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
 			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
 		}
 	}
 
 	/* set arming */
-	bool needs_to_arm = (should_arm || should_always_enable_pwm);
+	bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm);
 
 	/* check any conditions that prevent arming */
 	if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
 		needs_to_arm = false;
 	}
-	if (!should_arm && !should_always_enable_pwm) {
-		needs_to_arm = false;
-	}
 
 	if (needs_to_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
@@ -271,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]);
@@ -308,8 +318,9 @@ mixer_callback(uintptr_t handle,
 	       uint8_t control_index,
 	       float &control)
 {
-	if (control_group >= PX4IO_CONTROL_GROUPS)
+	if (control_group >= PX4IO_CONTROL_GROUPS) {
 		return -1;
+	}
 
 	switch (source) {
 	case MIX_FMU:
@@ -359,8 +370,15 @@ mixer_callback(uintptr_t handle,
 		}
 	}
 
+	/* 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) {
+	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) {
 			/* limit the throttle output to zero during motor spinup,
@@ -370,11 +388,13 @@ mixer_callback(uintptr_t handle,
 		}
 	}
 
-	/* limit output */
+	/* only safety off, but not armed - set throttle as invalid */
-	if (control > 1.0f) {
+	if (should_arm_nothrottle && !should_arm) {
-		control = 1.0f;
+		if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
-	} else if (control < -1.0f) {
+			control_index == actuator_controls_s::INDEX_THROTTLE) {
-		control = -1.0f;
+			/* mark the throttle as invalid */
+			control = NAN_VALUE;
+		}
 	}
 
 	return 0;
@@ -458,8 +478,9 @@ mixer_handle_text(const void *buffer, size_t length)
 			isr_debug(2, "used %u", mixer_text_length - resid);
 
 			/* copy any leftover text to the base of the buffer for re-use */
-			if (resid > 0)
+			if (resid > 0) {
 				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+			}
 
 			mixer_text_length = resid;
 
@@ -482,8 +503,9 @@ mixer_set_failsafe()
 	 */
 
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
-		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
+		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 		return;
+	}
 
 	/* set failsafe defaults to the values for all inputs = 0 */
 	float	outputs[PX4IO_SERVO_COUNT];
@@ -501,7 +523,8 @@ mixer_set_failsafe()
 	}
 
 	/* disable the rest of the outputs */
-	for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
+	for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
 		r_page_servo_failsafe[i] = 0;
+	}
 
 }

src/modules/systemlib/mixer/mixer.cpp

@@ -98,20 +98,25 @@ Mixer::scale(const mixer_scaler_s &scaler, float input)
 int
 Mixer::scale_check(struct mixer_scaler_s &scaler)
 {
-	if (scaler.offset > 1.001f)
+	if (scaler.offset > 1.001f) {
 		return 1;
+	}
 
-	if (scaler.offset < -1.001f)
+	if (scaler.offset < -1.001f) {
 		return 2;
+	}
 
-	if (scaler.min_output > scaler.max_output)
+	if (scaler.min_output > scaler.max_output) {
 		return 3;
+	}
 
-	if (scaler.min_output < -1.001f)
+	if (scaler.min_output < -1.001f) {
 		return 4;
+	}
 
-	if (scaler.max_output > 1.001f)
+	if (scaler.max_output > 1.001f) {
 		return 5;
+	}
 
 	return 0;
 }
@@ -120,11 +125,14 @@ const char *
 Mixer::findtag(const char *buf, unsigned &buflen, char tag)
 {
 	while (buflen >= 2) {
-		if ((buf[0] == tag) && (buf[1] == ':'))
+		if ((buf[0] == tag) && (buf[1] == ':')) {
 			return buf;
+		}
+
 		buf++;
 		buflen--;
 	}
+
 	return nullptr;
 }
 
@@ -174,13 +182,15 @@ NullMixer::from_text(const char *buf, unsigned &buflen)
 
 	/* enforce that the mixer ends with space or a new line */
 	for (int i = buflen - 1; i >= 0; i--) {
-		if (buf[i] == '\0')
+		if (buf[i] == '\0') {
 			continue;
+		}
 
 		/* require a space or newline at the end of the buffer, fail on printable chars */
 		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
 			/* found a line ending or space, so no split symbols / numbers. good. */
 			break;
+
 		} else {
 			return nm;
 		}

src/modules/systemlib/mixer/mixer.h

@@ -222,7 +222,7 @@ protected:
 	 * @param buflen		length of the buffer.
 	 * @param tag			character to search for.
 	 */
-	static const char *		findtag(const char *buf, unsigned &buflen, char tag);
+	static const char 		*findtag(const char *buf, unsigned &buflen, char tag);
 
 	/**
 	 * Skip a line
@@ -231,13 +231,13 @@ protected:
 	 * @param buflen		length of the buffer.
 	 * @return			0 / OK if a line could be skipped, 1 else
 	 */
-	static const char *		skipline(const char *buf, unsigned &buflen);
+	static const char 		*skipline(const char *buf, unsigned &buflen);
 
 private:
 
 	/* do not allow to copy due to prt data members */
-	Mixer(const Mixer&);
+	Mixer(const Mixer &);
-	Mixer& operator=(const Mixer&);
+	Mixer &operator=(const Mixer &);
 };
 
 /**
@@ -316,8 +316,8 @@ private:
 	Mixer				*_first;	/**< linked list of mixers */
 
 	/* do not allow to copy due to pointer data members */
-	MixerGroup(const MixerGroup&);
+	MixerGroup(const MixerGroup &);
-	MixerGroup operator=(const MixerGroup&);
+	MixerGroup operator=(const MixerGroup &);
 };
 
 /**
@@ -437,8 +437,8 @@ private:
 			uint8_t &control_index);
 
 	/* do not allow to copy due to ptr data members */
-	SimpleMixer(const SimpleMixer&);
+	SimpleMixer(const SimpleMixer &);
-	SimpleMixer operator=(const SimpleMixer&);
+	SimpleMixer operator=(const SimpleMixer &);
 };
 
 /**
@@ -449,13 +449,13 @@ private:
 typedef unsigned int MultirotorGeometryUnderlyingType;
 enum class MultirotorGeometry : MultirotorGeometryUnderlyingType;
 
-/**
+	/**
 	 * Multi-rotor mixer for pre-defined vehicle geometries.
 	 *
 	 * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to
 	 * a set of outputs based on the configured geometry.
 	 */
-class __EXPORT MultirotorMixer : public Mixer
+	class __EXPORT MultirotorMixer : public Mixer
 {
 public:
 	/**
@@ -531,8 +531,8 @@ private:
 	const Rotor			*_rotors;
 
 	/* do not allow to copy due to ptr data members */
-	MultirotorMixer(const MultirotorMixer&);
+	MultirotorMixer(const MultirotorMixer &);
-	MultirotorMixer operator=(const MultirotorMixer&);
+	MultirotorMixer operator=(const MultirotorMixer &);
 };
 
 #endif

src/modules/systemlib/mixer/mixer_load.c

@@ -52,6 +52,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 
 	/* open the mixer definition file */
 	fp = fopen(fname, "r");
+
 	if (fp == NULL) {
 		warnx("file not found");
 		return -1;
@@ -59,29 +60,38 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 
 	/* read valid lines from the file into a buffer */
 	buf[0] = '\0';
+
 	for (;;) {
 
 		/* get a line, bail on error/EOF */
 		line[0] = '\0';
-		if (fgets(line, sizeof(line), fp) == NULL)
+
+		if (fgets(line, sizeof(line), fp) == NULL) {
 			break;
+		}
 
 		/* if the line doesn't look like a mixer definition line, skip it */
-		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
+		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) {
 			continue;
+		}
 
 		/* compact whitespace in the buffer */
 		char *t, *f;
+
 		for (f = line; *f != '\0'; f++) {
 			/* scan for space characters */
 			if (*f == ' ') {
 				/* look for additional spaces */
 				t = f + 1;
-				while (*t == ' ')
+
+				while (*t == ' ') {
 					t++;
+				}
+
 				if (*t == '\0') {
 					/* strip trailing whitespace */
 					*f = '\0';
+
 				} else if (t > (f + 1)) {
 					memmove(f + 1, t, strlen(t) + 1);
 				}

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -109,15 +109,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 
 	/* enforce that the mixer ends with space or a new line */
 	for (int i = buflen - 1; i >= 0; i--) {
-		if (buf[i] == '\0')
+		if (buf[i] == '\0') {
 			continue;
+		}
 
 		/* require a space or newline at the end of the buffer, fail on printable chars */
 		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
 			/* found a line ending or space, so no split symbols / numbers. good. */
 			break;
+
 		} else {
-			debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
+			debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen - 1, buf[i]);
 			return nullptr;
 		}
 
@@ -134,6 +136,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	}
 
 	buf = skipline(buf, buflen);
+
 	if (buf == nullptr) {
 		debug("no line ending, line is incomplete");
 		return nullptr;
@@ -222,6 +225,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 	if (status_reg != NULL) {
 		(*status_reg) = 0;
 	}
+
 	// thrust boost parameters
 	float thrust_increase_factor = 1.5f;
 	float thrust_decrease_factor = 0.6f;
@@ -238,6 +242,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 		if (out < min_out) {
 			min_out = out;
 		}
+
 		if (out > max_out) {
 			max_out = out;
 		}
@@ -248,54 +253,59 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 	float boost = 0.0f;				// value added to demanded thrust (can also be negative)
 	float roll_pitch_scale = 1.0f;	// scale for demanded roll and pitch
 
-	if(min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) {
+	if (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) {
 		float max_thrust_diff = thrust * thrust_increase_factor - thrust;
-		if(max_thrust_diff >= -min_out) {
+
+		if (max_thrust_diff >= -min_out) {
 			boost = -min_out;
-		}
+
-		else {
+		} else {
 			boost = max_thrust_diff;
-			roll_pitch_scale = (thrust + boost)/(thrust - min_out);
+			roll_pitch_scale = (thrust + boost) / (thrust - min_out);
 		}
-	}
+
-	else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) {
+	} else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) {
-		float max_thrust_diff = thrust - thrust_decrease_factor*thrust;
+		float max_thrust_diff = thrust - thrust_decrease_factor * thrust;
-		if(max_thrust_diff >= max_out - 1.0f) {
+
+		if (max_thrust_diff >= max_out - 1.0f) {
 			boost = -(max_out - 1.0f);
+
 		} else {
 			boost = -max_thrust_diff;
-			roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust);
+			roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust);
 		}
-	}
+
-	else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) {
+	} else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) {
 		float max_thrust_diff = thrust * thrust_increase_factor - thrust;
-		boost = constrain(-min_out - (1.0f - max_out)/2.0f,0.0f, max_thrust_diff);
+		boost = constrain(-min_out - (1.0f - max_out) / 2.0f, 0.0f, max_thrust_diff);
-		roll_pitch_scale = (thrust + boost)/(thrust - min_out);
+		roll_pitch_scale = (thrust + boost) / (thrust - min_out);
-	}
+
-	else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f ) {
+	} else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f) {
-		float max_thrust_diff = thrust - thrust_decrease_factor*thrust;
+		float max_thrust_diff = thrust - thrust_decrease_factor * thrust;
-		boost = constrain(-(max_out - 1.0f - min_out)/2.0f, -max_thrust_diff, 0.0f);
+		boost = constrain(-(max_out - 1.0f - min_out) / 2.0f, -max_thrust_diff, 0.0f);
-		roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust);
+		roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust);
-	}
+
-	else if (min_out < 0.0f && max_out > 1.0f) {
+	} else if (min_out < 0.0f && max_out > 1.0f) {
-		boost = constrain(-(max_out - 1.0f + min_out)/2.0f, thrust_decrease_factor*thrust - thrust, thrust_increase_factor*thrust - thrust);
+		boost = constrain(-(max_out - 1.0f + min_out) / 2.0f, thrust_decrease_factor * thrust - thrust,
-		roll_pitch_scale = (thrust + boost)/(thrust - min_out);
+				  thrust_increase_factor * thrust - thrust);
+		roll_pitch_scale = (thrust + boost) / (thrust - min_out);
 	}
 
 	// notify if saturation has occurred
-	if(min_out < 0.0f) {
+	if (min_out < 0.0f) {
-		if(status_reg != NULL) {
+		if (status_reg != NULL) {
 			(*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT;
 		}
 	}
-	if(max_out > 0.0f) {
+
-		if(status_reg != NULL) {
+	if (max_out > 0.0f) {
+		if (status_reg != NULL) {
 			(*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT;
 		}
 	}
 
 	// mix again but now with thrust boost, scale roll/pitch and also add yaw
-	for(unsigned i = 0; i < _rotor_count; i++) {
+	for (unsigned i = 0; i < _rotor_count; i++) {
 		float out = (roll * _rotors[i].roll_scale +
 			     pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
 			    yaw * _rotors[i].yaw_scale +
@@ -304,26 +314,28 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 		out *= _rotors[i].out_scale;
 
 		// scale yaw if it violates limits. inform about yaw limit reached
-		if(out < 0.0f) {
+		if (out < 0.0f) {
 			yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
-				roll_pitch_scale + thrust + boost)/_rotors[i].yaw_scale;
+				roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale;
-			if(status_reg != NULL) {
+
+			if (status_reg != NULL) {
 				(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
 			}
-		}
+
-		else if(out > 1.0f) {
+		} else if (out > 1.0f) {
 			// allow to reduce thrust to get some yaw response
 			float thrust_reduction = fminf(0.15f, out - 1.0f);
 			thrust -= thrust_reduction;
 			yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
-				roll_pitch_scale + thrust + boost))/_rotors[i].yaw_scale;
+				       roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale;
-			if(status_reg != NULL) {
+
+			if (status_reg != NULL) {
 				(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
 			}
 		}
 	}
 
-	/* last mix, add yaw and scale outputs to range idle_speed...1 */
+	/* add yaw and scale outputs to range idle_speed...1 */
 	for (unsigned i = 0; i < _rotor_count; i++) {
 		outputs[i] = (roll * _rotors[i].roll_scale +
 			      pitch * _rotors[i].pitch_scale) * roll_pitch_scale +

src/modules/systemlib/mixer/mixer_simple.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -67,8 +67,9 @@ SimpleMixer::SimpleMixer(ControlCallback control_cb,
 
 SimpleMixer::~SimpleMixer()
 {
-	if (_info != nullptr)
+	if (_info != nullptr) {
 		free(_info);
+	}
 }
 
 int
@@ -79,6 +80,7 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
 	int n = -1;
 
 	buf = findtag(buf, buflen, 'O');
+
 	if ((buf == nullptr) || (buflen < 12)) {
 		debug("output parser failed finding tag, ret: '%s'", buf);
 		return -1;
@@ -91,6 +93,7 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
 	}
 
 	buf = skipline(buf, buflen);
+
 	if (buf == nullptr) {
 		debug("no line ending, line is incomplete");
 		return -1;
@@ -106,12 +109,14 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
 }
 
 int
-SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index)
+SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group,
+				  uint8_t &control_index)
 {
 	unsigned u[2];
 	int s[5];
 
 	buf = findtag(buf, buflen, 'S');
+
 	if ((buf == nullptr) || (buflen < 16)) {
 		debug("control parser failed finding tag, ret: '%s'", buf);
 		return -1;
@@ -124,6 +129,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 	}
 
 	buf = skipline(buf, buflen);
+
 	if (buf == nullptr) {
 		debug("no line ending, line is incomplete");
 		return -1;
@@ -156,6 +162,7 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 	}
 
 	buf = skipline(buf, buflen);
+
 	if (buf == nullptr) {
 		debug("no line ending, line is incomplete");
 		goto out;
@@ -198,14 +205,16 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 
 out:
 
-	if (mixinfo != nullptr)
+	if (mixinfo != nullptr) {
 		free(mixinfo);
+	}
 
 	return sm;
 }
 
 SimpleMixer *
-SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max)
+SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min,
+		       uint16_t mid, uint16_t max)
 {
 	SimpleMixer *sm = nullptr;
 	mixer_simple_s *mixinfo = nullptr;
@@ -258,8 +267,9 @@ SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, u
 
 out:
 
-	if (mixinfo != nullptr)
+	if (mixinfo != nullptr) {
 		free(mixinfo);
+	}
 
 	return sm;
 }
@@ -269,11 +279,13 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 {
 	float		sum = 0.0f;
 
-	if (_info == nullptr)
+	if (_info == nullptr) {
 		return 0;
+	}
 
-	if (space < 1)
+	if (space < 1) {
 		return 0;
+	}
 
 	for (unsigned i = 0; i < _info->control_count; i++) {
 		float input;
@@ -293,8 +305,9 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 void
 SimpleMixer::groups_required(uint32_t &groups)
 {
-	for (unsigned i = 0; i < _info->control_count; i++)
+	for (unsigned i = 0; i < _info->control_count; i++) {
 		groups |= 1 << _info->controls[i].control_group;
+	}
 }
 
 int
@@ -305,14 +318,16 @@ SimpleMixer::check()
 
 	/* sanity that presumes that a mixer includes a control no more than once */
 	/* max of 32 groups due to groups_required API */
-	if (_info->control_count > 32)
+	if (_info->control_count > 32) {
 		return -2;
+	}
 
 	/* validate the output scaler */
 	ret = scale_check(_info->output_scaler);
 
-	if (ret != 0)
+	if (ret != 0) {
 		return ret;
+	}
 
 	/* validate input scalers */
 	for (unsigned i = 0; i < _info->control_count; i++) {
@@ -328,9 +343,10 @@ SimpleMixer::check()
 		/* validate the scaler */
 		ret = scale_check(_info->controls[i].scaler);
 
-		if (ret != 0)
+		if (ret != 0) {
 			return (10 * i + ret);
 		}
+	}
 
 	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++) {
@@ -121,6 +135,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;
 	                
 					/* if a disarmed pwm value was set, blend between disarmed and min */
@@ -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;
 	}
 
+	PX4_INFO("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;
+	}
+
 	PX4_INFO("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]) {
-				PX4_ERR("disarmed servo value mismatch");
+				PX4_ERR("disarmed servo value mismatch: %d vs %d", r_page_servos[i], r_page_servo_disarmed[i]);
 				return 1;
 			}
 
@@ -222,21 +263,16 @@ int test_mixer(int argc, char *argv[])
 				PX4_ERR("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);
 		sleepcount++;
 
 		if (sleepcount % 10 == 0) {
-			printf(".");
 			fflush(stdout);
 		}
 	}
 
-	printf("\n");
-
 	PX4_INFO("ARMING TEST: NORMAL OPERATION");
 
 	for (int j = -jmax; j <= jmax; j++) {
@@ -251,7 +287,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);
 
 		PX4_INFO("mixed %d outputs (max %d)", mixed, output_max);
@@ -278,18 +314,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]) {
 				PX4_ERR("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 +351,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 +361,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 +377,6 @@ int test_mixer(int argc, char *argv[])
 				PX4_ERR("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 +434,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;
 }