mc stabilized mode: move from mc_pos_control to mc_att_control

- better in terms of dependencies:
  - the position controller code depended on position topics for yaw
  - mc_pos_control does not have to be run for Stabilized mode
- the code path is much simpler, and thus less error prone. This is
  especially important since Stabilized is often used as a fail-safe flight
  mode.

src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -43,19 +43,23 @@ using namespace matrix;
 
 bool FlightTaskManualAltitude::updateInitialize()
 {
-	bool ret = FlightTaskManualStabilized::updateInitialize();
-	// in addition to stabilized require valid position and velocity in D-direction
-	return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2));
+	bool ret = FlightTaskManual::updateInitialize();
+	// in addition to manual require valid position and velocity in D-direction and valid yaw
+	return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw);
 }
 
 bool FlightTaskManualAltitude::activate()
 {
-	bool ret = FlightTaskManualStabilized::activate();
-	_thrust_setpoint(2) = NAN; // altitude is controlled from position/velocity
+	bool ret = FlightTaskManual::activate();
+	_yaw_setpoint = _yaw;
+	_yawspeed_setpoint = 0.0f;
+	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // altitude is controlled from position/velocity
 	_position_setpoint(2) = _position(2);
 	_velocity_setpoint(2) = 0.0f;
 	_setDefaultConstraints();
 
+	_constraints.tilt = math::radians(MPC_MAN_TILT_MAX.get());
+
 	if (PX4_ISFINITE(_sub_vehicle_local_position->get().hagl_min)) {
 		_constraints.min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min;
 
@@ -78,8 +82,9 @@ bool FlightTaskManualAltitude::activate()
 
 void FlightTaskManualAltitude::_scaleSticks()
 {
-	// reuse same scaling as for stabilized
-	FlightTaskManualStabilized::_scaleSticks();
+	/* Scale sticks to yaw and thrust using
+	 * linear scale for yaw and piecewise linear map for thrust. */
+	_yawspeed_setpoint = _sticks_expo(3) * math::radians(MPC_MAN_Y_MAX.get());
 
 	// scale horizontal velocity with expo curve stick input
 	const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
@@ -249,9 +254,47 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 	}
 }
 
+void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v)
+{
+	float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
+	Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f));
+	v(0) = v_r(0);
+	v(1) = v_r(1);
+}
+
+void FlightTaskManualAltitude::_updateHeadingSetpoints()
+{
+	/* Yaw-lock depends on stick input. If not locked,
+	 * yaw_sp is set to NAN.
+	 * TODO: add yawspeed to get threshold.*/
+	if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) {
+		// no fixed heading when rotating around yaw by stick
+		_yaw_setpoint = NAN;
+
+	} else {
+		// hold the current heading when no more rotation commanded
+		if (!PX4_ISFINITE(_yaw_setpoint)) {
+			_yaw_setpoint = _yaw;
+
+		} else {
+			// check reset counter and update yaw setpoint if necessary
+			if (_sub_attitude->get().quat_reset_counter != _heading_reset_counter) {
+				_yaw_setpoint += matrix::Eulerf(matrix::Quatf(_sub_attitude->get().delta_q_reset)).psi();
+				_heading_reset_counter = _sub_attitude->get().quat_reset_counter;
+			}
+		}
+	}
+
+	// check if an external yaw handler is active and if yes, let it compute the yaw setpoints
+	if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) {
+		_yaw_setpoint = NAN;
+		_yawspeed_setpoint += _ext_yaw_handler->get_weathervane_yawrate();
+	}
+}
+
 void FlightTaskManualAltitude::_updateSetpoints()
 {
-	FlightTaskManualStabilized::_updateHeadingSetpoints(); // get yaw setpoint
+	_updateHeadingSetpoints(); // get yaw setpoint
 
 	// Thrust in xy are extracted directly from stick inputs. A magnitude of
 	// 1 means that maximum thrust along xy is demanded. A magnitude of 0 means no
@@ -271,3 +314,11 @@ void FlightTaskManualAltitude::_updateSetpoints()
 
 	_updateAltitudeLock();
 }
+
+bool FlightTaskManualAltitude::update()
+{
+	_scaleSticks();
+	_updateSetpoints();
+
+	return true;
+}