diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index b7194461e6..c44f29a404 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -861,8 +861,8 @@ FixedwingAttitudeControl::task_main()
 					if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
 						roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
 					} else {
-						roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
-											+ _parameters.rollsp_offset_rad;
+						roll_sp = (_manual.y * _parameters.man_roll_max)
+								+ _parameters.rollsp_offset_rad;
 					}
 
 					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
@@ -883,8 +883,7 @@ FixedwingAttitudeControl::task_main()
  					/*
 					 * Velocity should be controlled and manual is enabled.
 					*/
-					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
-											+ _parameters.rollsp_offset_rad;
+					roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
 					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
 					throttle_sp = _att_sp.thrust;
 
@@ -911,10 +910,8 @@ FixedwingAttitudeControl::task_main()
 					 * the intended attitude setpoint. Later, after the rate control step the
 					 * trim is added again to the control signal.
 					 */
-					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
-						+ _parameters.rollsp_offset_rad;
-					pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
-						+ _parameters.pitchsp_offset_rad;
+					roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
+					pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad;
 					/* allow manual control of rudder deflection */
 					yaw_manual = _manual.r;
 					throttle_sp = _manual.z;
@@ -1081,9 +1078,9 @@ FixedwingAttitudeControl::task_main()
 
 			} else {
 				/* manual/direct control */
-				_actuators.control[0] = _manual.y;
-				_actuators.control[1] = -_manual.x;
-				_actuators.control[2] = _manual.r;
+				_actuators.control[0] = _manual.y + _parameters.trim_roll;
+				_actuators.control[1] = -_manual.x + _parameters.trim_pitch;
+				_actuators.control[2] = _manual.r + _parameters.trim_yaw;
 				_actuators.control[3] = _manual.z;
 				_actuators.control[4] = _manual.flaps;
 			}