IO: Allow to pre-arm the non-throttle channels with the safety switch

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