fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 11:08:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

don't update servos if mixer_mix_threadsafe() failed to run

Browse Source
This commit is contained in:
Mark Whitehorn
2017-03-23 08:28:15 +01:00
committed by Lorenz Meier
parent dd25366b86
commit 9e3488af46
1 changed files with 22 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/px4iofirmware/mixer.cpp
+22 -19
View File
@@ -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);
/* if mixer_mix_threadsafe returns zero, it did nothing */
if (mixed != 0) {
/* clamp unused outputs to zero */
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = 0;
outputs[i] = 0.0f;
/* 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;
}
/* 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();
}
}
/* 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 */