Merge pull request #2346 from tumbili/fix_trim

fix: take current trim values into account when doing trim calibration

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 */