From 71e2a43790f1baca06e9fa2de0956bd37115d3d0 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 9 Jun 2016 17:26:59 +0200 Subject: [PATCH] consider scale parameters in rc calibration code --- src/modules/commander/rc_calibration.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 5423c03a82..fa99b4da27 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -79,18 +79,26 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub) float yaw_trim_active; param_get(param_find("TRIM_YAW"), &yaw_trim_active); + /* get manual control scale values */ + float roll_scale; + param_get(param_find("FW_MAN_R_SC"), &roll_scale); + float pitch_scale; + param_get(param_find("FW_MAN_P_SC"), &pitch_scale); + float yaw_scale; + param_get(param_find("FW_MAN_Y_SC"), &yaw_scale); + /* set parameters: the new trim values are the combination of active trim values and the values coming from the remote control of the user */ - float p = sp.y + roll_trim_active; + float p = sp.y * roll_scale + roll_trim_active; int p1r = param_set(param_find("TRIM_ROLL"), &p); /* we explicitly swap sign here because the trim is added to the actuator controls which are moving in an inverse sense to manual pitch inputs */ - p = -sp.x + pitch_trim_active; + p = -sp.x * pitch_scale + pitch_trim_active; int p2r = param_set(param_find("TRIM_PITCH"), &p); - p = sp.r + yaw_trim_active; + p = sp.r * yaw_scale + yaw_trim_active; int p3r = param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */