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 8c6e70f83b..83f50050fb 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -975,6 +975,19 @@ FixedwingAttitudeControl::task_main()
 				float yaw_manual = 0.0f;
 				float throttle_sp = 0.0f;
 
+				// in STABILIZED mode we need to generate the attitude setpoint
+				// from manual user inputs
+				if (!_vcontrol_mode.flag_control_climb_rate_enabled) {
+					_att_sp.roll_body = _manual.y * _parameters.man_roll_max + _parameters.rollsp_offset_rad;
+					_att_sp.roll_body = math::constrain(_att_sp.roll_body, -_parameters.man_roll_max, _parameters.man_roll_max);
+					_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad;
+					_att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max);
+					_att_sp.yaw_body = 0.0f;
+					_att_sp.thrust = _manual.z;
+					int instance;
+					orb_publish_auto(ORB_ID(vehicle_attitude_setpoint), &_attitude_sp_pub, &_att_sp, &instance, ORB_PRIO_DEFAULT);
+				}
+
 				roll_sp = _att_sp.roll_body;
 				pitch_sp = _att_sp.pitch_body;
 				yaw_sp = _att_sp.yaw_body;
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 98d67fd365..31c6a12877 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -1942,7 +1942,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				}
 			}
 
-		} else {
+		}
+
+		if (!_yaw_lock_engaged || fabsf(_manual.y) > HDG_HOLD_MAN_INPUT_THRESH ||
+		    fabsf(_manual.r) > HDG_HOLD_MAN_INPUT_THRESH) {
 			_hdg_hold_enabled = false;
 			_yaw_lock_engaged = false;
 			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
@@ -2004,23 +2007,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	} else {
 		_control_mode_current = FW_POSCTRL_MODE_OTHER;
 
-		if (_control_mode.flag_control_attitude_enabled) {
-			/** STABILIZED FLIGHT **/
-			_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
-			_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max_rad;
-			_att_sp.yaw_body = 0.0f;
-			_att_sp.thrust = _manual.z;
-
-		} else {
-			/** MANUAL FLIGHT **/
-			_att_sp.roll_body = 0.0f;
-			_att_sp.pitch_body = 0.0f;
-			_att_sp.yaw_body = 0.0f;
-			_att_sp.thrust = 0.0f;
-
-			/* do not publish the setpoint */
-			setpoint = false;
-		}
+		/* do not publish the setpoint */
+		setpoint = false;
 
 		// reset hold altitude
 		_hold_alt = _global_pos.alt;