diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 0cdf639d85..d4484e36b5 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -295,27 +295,30 @@ mixer_tick(void) /* mix */ mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits); - /* the pwm limit call takes care of out of band errors */ - 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.0f; - } + /* if mixer_mix_threadsafe returns zero, it did nothing */ + if (mixed != 0) { - /* store normalized outputs */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { - r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - } + /* the pwm limit call takes care of out of band errors */ + 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); - if (new_fmu_data) { - new_fmu_data = false; + /* clamp unused outputs to zero */ + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { + r_page_servos[i] = 0; + outputs[i] = 0.0f; + } - up_pwm_force_update(); - } + /* store normalized outputs */ + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); + } + + if (new_fmu_data) { + new_fmu_data = false; + up_pwm_force_update(); + } + } } /* set arming */