fw_attitude_control: calculate attitude setpoint for STAB mode

- attitude setpoint generation for stabilized mode was shifted back
to the fw attitude controller. since the fw position controller is polling
on global position attitude setpoints were not generated when global
position was not published.

Signed-off-by: Roman <bapstroman@gmail.com>

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;

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;