vehicle_attitude only based yaw control fix (#10803)

* Add attitude sub to mc_pos_control for yaw usage.

* Initialize pos control _states struct.

* Remove unnecessary init for struct in mc pos ctrl.

* Only use att topic for yaw setting in FlightTask.

src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp

@@ -90,6 +90,11 @@ void FlightTask::_evaluateVehicleLocalPosition()
 	_yaw = NAN;
 	_dist_to_bottom = NAN;
 
+	if ((_time_stamp_current - _sub_attitude->get().timestamp) < _timeout) {
+		// yaw
+		_yaw = matrix::Eulerf(matrix::Quatf(_sub_attitude->get().q)).psi();
+	}
+
 	// Only use vehicle-local-position topic fields if the topic is received within a certain timestamp
 	if ((_time_stamp_current - _sub_vehicle_local_position->get().timestamp) < _timeout) {
 
@@ -113,9 +118,6 @@ void FlightTask::_evaluateVehicleLocalPosition()
 			_velocity(2) = _sub_vehicle_local_position->get().vz;
 		}
 
-		// yaw
-		_yaw = _sub_vehicle_local_position->get().yaw;
-
 		// distance to bottom
 		if (_sub_vehicle_local_position->get().dist_bottom_valid
 		    && PX4_ISFINITE(_sub_vehicle_local_position->get().dist_bottom)) {

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -110,6 +110,7 @@ private:
 	int		_control_mode_sub{-1};		/**< vehicle control mode subscription */
 	int		_params_sub{-1};			/**< notification of parameter updates */
 	int		_local_pos_sub{-1};			/**< vehicle local position */
+	int		_att_sub{-1};				/**< vehicle attitude */
 	int		_home_pos_sub{-1}; 			/**< home position */
 	int		_traj_wp_avoidance_sub{-1};	/**< trajectory waypoint */
 
@@ -149,7 +150,7 @@ private:
 
 	FlightTasks _flight_tasks; /**< class that generates position controller tracking setpoints*/
 	PositionControl _control; /**< class that handles the core PID position controller */
-	PositionControlStates _states; /**< structure that contains required state information for position control */
+	PositionControlStates _states{}; /**< structure that contains required state information for position control */
 
 	hrt_abstime _last_warn = 0; /**< timer when the last warn message was sent out */
 
@@ -433,6 +434,15 @@ MulticopterPositionControl::poll_subscriptions()
 		orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
 	}
 
+	orb_check(_att_sub, &updated);
+
+	if (updated) {
+		vehicle_attitude_s att;
+		if (orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att) == PX4_OK && PX4_ISFINITE(att.q[0])) {
+			_states.yaw = Eulerf(Quatf(att.q)).psi();
+		}
+	}
+
 	orb_check(_home_pos_sub, &updated);
 
 	if (updated) {
@@ -536,9 +546,6 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
 
 	}
 
-	if (PX4_ISFINITE(_local_pos.yaw)) {
-		_states.yaw = _local_pos.yaw;
-	}
 }
 
 void
@@ -550,6 +557,7 @@ MulticopterPositionControl::run()
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
 	_traj_wp_avoidance_sub = orb_subscribe(ORB_ID(vehicle_trajectory_waypoint));
 
@@ -611,7 +619,7 @@ MulticopterPositionControl::run()
 				_wv_controller->deactivate();
 			}
 
-			_wv_controller->update(matrix::Quatf(_att_sp.q_d), _local_pos.yaw);
+			_wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw);
 		}
 
 		if (_control_mode.flag_armed) {
@@ -787,7 +795,7 @@ MulticopterPositionControl::run()
 		} else {
 			// no flighttask is active: set attitude setpoint to idle
 			_att_sp.roll_body = _att_sp.pitch_body = 0.0f;
-			_att_sp.yaw_body = _local_pos.yaw;
+			_att_sp.yaw_body = _states.yaw;
 			_att_sp.yaw_sp_move_rate = 0.0f;
 			_att_sp.fw_control_yaw = false;
 			_att_sp.apply_flaps = false;
@@ -803,6 +811,7 @@ MulticopterPositionControl::run()
 	orb_unsubscribe(_control_mode_sub);
 	orb_unsubscribe(_params_sub);
 	orb_unsubscribe(_local_pos_sub);
+	orb_unsubscribe(_att_sub);
 	orb_unsubscribe(_home_pos_sub);
 	orb_unsubscribe(_traj_wp_avoidance_sub);
 }