diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 08261c9890..c9a8343b66 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -68,16 +68,26 @@ int do_trim_calibration(int mavlink_fd) orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); - /* set parameters */ - float p = sp.y; + /* load trim values which are active */ + float roll_trim_active; + param_get(param_find("TRIM_ROLL"), &roll_trim_active); + float pitch_trim_active; + param_get(param_find("TRIM_PITCH"), &pitch_trim_active); + float yaw_trim_active; + param_get(param_find("TRIM_YAW"), &yaw_trim_active); + + /* 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; 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; + p = -sp.x + pitch_trim_active; int p2r = param_set(param_find("TRIM_PITCH"), &p); - p = sp.r; + p = sp.r + yaw_trim_active; int p3r = param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */