From af8cd3f880a14864e57284a5f1242796a71a103d Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 12 Jul 2016 14:33:44 +0200 Subject: [PATCH] correctly scale and trim outputs in IO --- src/modules/px4iofirmware/mixer.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index f41a80ef66..9c2dfdc5aa 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -373,15 +373,18 @@ mixer_callback(uintptr_t handle, if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && control_index == actuator_controls_s::INDEX_ROLL) { - control *= REG_TO_FLOAT(r_setup_scale_roll) + REG_TO_FLOAT(r_setup_trim_roll); + control *= REG_TO_FLOAT(r_setup_scale_roll); + control += REG_TO_FLOAT(r_setup_trim_roll); } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && control_index == actuator_controls_s::INDEX_PITCH) { - control *= REG_TO_FLOAT(r_setup_scale_pitch) + REG_TO_FLOAT(r_setup_trim_pitch); + control *= REG_TO_FLOAT(r_setup_scale_pitch); + control += REG_TO_FLOAT(r_setup_trim_pitch); } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && control_index == actuator_controls_s::INDEX_YAW) { - control *= REG_TO_FLOAT(r_setup_scale_yaw) + REG_TO_FLOAT(r_setup_trim_yaw); + control *= REG_TO_FLOAT(r_setup_scale_yaw); + control += REG_TO_FLOAT(r_setup_trim_yaw); } }