diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2cfdf491b..050750d080 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/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;