diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg
index 065fa12314..b2d1fdb64a 100644
--- a/msg/vehicle_attitude_setpoint.msg
+++ b/msg/vehicle_attitude_setpoint.msg
@@ -4,7 +4,8 @@ float32 roll_body		# body angle in NED frame (can be NaN for FW)
 float32 pitch_body		# body angle in NED frame (can be NaN for FW)
 float32 yaw_body		# body angle in NED frame (can be NaN for FW)
 
-float32 yaw_sp_move_rate	# rad/s (commanded by user)
+float32 yaw_sp_move_rate	# feed-forward NED yaw angular rate in rad/s
+bool absolute_heading_valid # True if absolute heading of attitude quaternion should be tracked
 
 # For quaternion-based attitude control
 float32[4] q_d			# Desired quaternion for quaternion control
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 6392da26a7..18f67c97ee 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -246,6 +246,9 @@ MulticopterAttitudeControl::Run()
 		if (_vehicle_attitude_setpoint_sub.updated()) {
 			vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
 			_vehicle_attitude_setpoint_sub.update(&vehicle_attitude_setpoint);
+			const float yaw_weight = vehicle_attitude_setpoint.absolute_heading_valid ? _param_mc_yaw_weight.get() : 0.f;
+			_attitude_control.setProportionalGain(Vector3f(_param_mc_roll_p.get(), _param_mc_pitch_p.get(), _param_mc_yaw_p.get()),
+							      yaw_weight);
 			_attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate);
 			_thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body);
 		}
diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
index 70d23c7885..72dc0cb1f9 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
@@ -120,7 +120,6 @@ bool PositionControl::update(const float dt)
 	_velocityControl(dt);
 
 	_yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f;
-	_yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control
 
 	return valid && _updateSuccessful();
 }
@@ -240,6 +239,7 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
 
 void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
 {
-	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
+	ControlMath::thrustToAttitude(_thr_sp, PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw, attitude_setpoint);
+	attitude_setpoint.absolute_heading_valid = PX4_ISFINITE(_yaw_sp);
 	attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
 }