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/CMakeLists.txt

@@ -51,7 +51,6 @@ endif()
 
 # add core flight tasks to list
 list(APPEND flight_tasks_all
-	ManualStabilized
 	ManualAltitude
 	ManualAltitudeSmooth
 	ManualPosition

src/lib/FlightTasks/tasks/ManualAltitude/CMakeLists.txt

@@ -35,5 +35,5 @@ px4_add_library(FlightTaskManualAltitude
 	FlightTaskManualAltitude.cpp
 )
 
-target_link_libraries(FlightTaskManualAltitude PUBLIC FlightTaskManualStabilized)
+target_link_libraries(FlightTaskManualAltitude PUBLIC FlightTaskManual)
 target_include_directories(FlightTaskManualAltitude PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

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;
+}

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

@@ -39,19 +39,31 @@
 
 #pragma once
 
-#include "FlightTaskManualStabilized.hpp"
+#include "FlightTaskManual.hpp"
 
-class FlightTaskManualAltitude : public FlightTaskManualStabilized
+class FlightTaskManualAltitude : public FlightTaskManual
 {
 public:
 	FlightTaskManualAltitude() = default;
 	virtual ~FlightTaskManualAltitude() = default;
 	bool activate() override;
 	bool updateInitialize() override;
+	bool update() override;
+
+	/**
+	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
+	 */
+	void setYawHandler(WeatherVane *ext_yaw_handler) override { _ext_yaw_handler = ext_yaw_handler; }
 
 protected:
-	void _updateSetpoints() override; /**< updates all setpoints */
-	void _scaleSticks() override; /**< scales sticks to velocity in z */
+	void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
+	virtual void _updateSetpoints(); /**< updates all setpoints */
+	virtual void _scaleSticks(); /**< scales sticks to velocity in z */
+
+	/**
+	 * rotates vector into local frame
+	 */
+	void _rotateIntoHeadingFrame(matrix::Vector2f &vec);
 
 	/**
 	 *  Check and sets for position lock.
@@ -60,27 +72,15 @@ protected:
 	 */
 	void _updateAltitudeLock();
 
-	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualStabilized,
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) MPC_HOLD_MAX_Z,
 					(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) MPC_HOLD_MAX_XY,
-					(ParamFloat<px4::params::MPC_Z_P>) MPC_Z_P
+					(ParamFloat<px4::params::MPC_Z_P>) MPC_Z_P,
+					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) MPC_MAN_Y_MAX, /**< scaling factor from stick to yaw rate */
+					(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) MPC_MAN_TILT_MAX /**< maximum tilt allowed for manual flight */
 				       )
 private:
-	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
-	float _max_speed_up = 10.0f;
-	float _min_speed_down = 1.0f;
-	bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */
-	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
-
-	/**
-	 * Distance to ground during terrain following.
-	 * If user does not demand velocity change in D-direction and the vehcile
-	 * is in terrain-following mode, then height to ground will be locked to
-	 * _dist_to_ground_lock.
-	 */
-	float _dist_to_ground_lock = NAN;
-
 	/**
 	 * Terrain following.
 	 * During terrain following, the position setpoint is adjusted
@@ -99,4 +99,22 @@ private:
 	void _respectMinAltitude();
 
 	void _respectMaxAltitude();
+
+
+	WeatherVane *_ext_yaw_handler =
+		nullptr;	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+
+	uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
+	float _max_speed_up = 10.0f;
+	float _min_speed_down = 1.0f;
+	bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */
+	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
+
+	/**
+	 * Distance to ground during terrain following.
+	 * If user does not demand velocity change in D-direction and the vehcile
+	 * is in terrain-following mode, then height to ground will be locked to
+	 * _dist_to_ground_lock.
+	 */
+	float _dist_to_ground_lock = NAN;
 };

src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -53,10 +53,11 @@ bool FlightTaskManualPosition::updateInitialize()
 
 bool FlightTaskManualPosition::activate()
 {
-
 	// all requirements from altitude-mode still have to hold
 	bool ret = FlightTaskManualAltitude::activate();
 
+	_constraints.tilt = math::radians(MPC_TILTMAX_AIR.get());
+
 	// set task specific constraint
 	if (_constraints.speed_xy >= MPC_VEL_MANUAL.get()) {
 		_constraints.speed_xy = MPC_VEL_MANUAL.get();

src/lib/FlightTasks/tasks/ManualStabilized/CMakeLists.txt

@@ -1,39 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_library(FlightTaskManualStabilized
-	FlightTaskManualStabilized.cpp
-)
-
-target_link_libraries(FlightTaskManualStabilized PUBLIC FlightTaskManual)
-target_include_directories(FlightTaskManualStabilized PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.cpp

@@ -1,172 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file FlightManualStabilized.cpp
- */
-
-#include "FlightTaskManualStabilized.hpp"
-#include <mathlib/mathlib.h>
-#include <float.h>
-
-using namespace matrix;
-
-bool FlightTaskManualStabilized::activate()
-{
-	bool ret = FlightTaskManual::activate();
-	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_throttle_hover.get());
-	_yaw_setpoint = _yaw;
-	_yawspeed_setpoint = 0.0f;
-	_constraints.tilt = math::radians(_tilt_max_man.get());
-	return ret;
-}
-
-bool FlightTaskManualStabilized::updateInitialize()
-{
-	bool ret = FlightTaskManual::updateInitialize();
-	// need a valid yaw-state
-	return ret && PX4_ISFINITE(_yaw);
-}
-
-void 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(_yaw_rate_scaling.get());
-	_throttle = _throttleCurve();
-}
-
-void FlightTaskManualStabilized::_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 FlightTaskManualStabilized::_updateThrustSetpoints()
-{
-	/* Rotate setpoint into local frame. */
-	Vector2f sp(&_sticks(0));
-	_rotateIntoHeadingFrame(sp);
-
-	/* Ensure that maximum tilt is in [0.001, Pi] */
-	float tilt_max = math::constrain(_constraints.tilt, 0.001f, M_PI_F);
-
-	const float x = sp(0) * tilt_max;
-	const float y = sp(1) * tilt_max;
-
-	/* The norm of the xy stick input provides the pointing
-	 * direction of the horizontal desired thrust setpoint. The magnitude of the
-	 * xy stick inputs represents the desired tilt. Both tilt and magnitude can
-	 * be captured through Axis-Angle.
-	 */
-	/* The Axis-Angle is the perpendicular vector to xy-stick input */
-	Vector2f v = Vector2f(y, -x);
-	float v_norm = v.norm(); // the norm of v defines the tilt angle
-
-	if (v_norm > tilt_max) { // limit to the configured maximum tilt angle
-		v *= tilt_max / v_norm;
-	}
-
-	/* The final thrust setpoint is found by rotating the scaled unit vector pointing
-	 * upward by the Axis-Angle.
-	 * Make sure that the attitude can be controlled even at 0 throttle.
-	 */
-	Quatf q_sp = AxisAnglef(v(0), v(1), 0.0f);
-	_thrust_setpoint = q_sp.conjugate(Vector3f(0.0f, 0.0f, -1.0f)) * math::max(_throttle, 0.0001f);
-}
-
-void FlightTaskManualStabilized::_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 FlightTaskManualStabilized::_updateSetpoints()
-{
-	_updateHeadingSetpoints();
-	_updateThrustSetpoints();
-}
-
-float FlightTaskManualStabilized::_throttleCurve()
-{
-	// Scale stick z from [-1,1] to [min thrust, max thrust]
-	float throttle = -((_sticks(2) - 1.0f) * 0.5f);
-
-	switch (_throttle_curve.get()) {
-	case 1: // no rescaling
-		return throttle;
-
-	default: // 0 or other: rescale to hover throttle at 0.5 stick
-		if (throttle < 0.5f) {
-			return (_throttle_hover.get() - _throttle_min_stabilized.get()) / 0.5f * throttle + _throttle_min_stabilized.get();
-
-		} else {
-			return (_throttle_max.get() - _throttle_hover.get()) / 0.5f * (throttle - 1.0f) + _throttle_max.get();
-		}
-	}
-
-}
-
-bool FlightTaskManualStabilized::update()
-{
-	_scaleSticks();
-	_updateSetpoints();
-
-	return true;
-}

src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp

@@ -1,83 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file FlightManualStabilized.hpp
- *
- * Flight task for manual controlled attitude.
- * It generates thrust and yaw setpoints.
- */
-
-#pragma once
-
-#include "FlightTaskManual.hpp"
-
-class FlightTaskManualStabilized : public FlightTaskManual
-{
-public:
-	FlightTaskManualStabilized() = default;
-
-	virtual ~FlightTaskManualStabilized() = default;
-	bool activate() override;
-	bool updateInitialize() override;
-	bool update() override;
-
-	/**
-	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
-	 */
-	void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;}
-
-protected:
-	virtual void _updateSetpoints(); /**< updates all setpoints */
-	void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
-	virtual void _scaleSticks(); /**< scales sticks to yaw and thrust */
-	void _rotateIntoHeadingFrame(matrix::Vector2f &vec); /**< rotates vector into local frame */
-
-private:
-	void _updateThrustSetpoints(); /**< sets thrust setpoint */
-	float _throttleCurve(); /**< piecewise linear mapping from stick to throttle */
-
-	float _throttle{}; /** mapped from stick z */
-
-	WeatherVane *_ext_yaw_handler =
-		nullptr;	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
-
-	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual,
-					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _yaw_rate_scaling, /**< scaling factor from stick to yaw rate */
-					(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _tilt_max_man, /**< maximum tilt allowed for manual flight */
-					(ParamFloat<px4::params::MPC_MANTHR_MIN>) _throttle_min_stabilized, /**< minimum throttle for stabilized */
-					(ParamFloat<px4::params::MPC_THR_MAX>) _throttle_max, /**< maximum throttle that always has to be satisfied in flight*/
-					(ParamFloat<px4::params::MPC_THR_HOVER>) _throttle_hover, /**< throttle at which vehicle is at hover equilibrium */
-					(ParamInt<px4::params::MPC_THR_CURVE>) _throttle_curve /**< throttle curve behavior */
-				       )
-};

src/modules/mc_att_control/mc_att_control.hpp

@@ -114,6 +114,20 @@ private:
 	void		publish_rates_setpoint();
 	void		publish_rate_controller_status();
 
+	float		throttle_curve(float throttle_stick_input);
+
+	/**
+	 * Generate & publish an attitude setpoint from stick inputs
+	 */
+	void		generate_attitude_setpoint(float dt, bool reset_yaw_sp);
+
+	/**
+	 * Get the landing gear state based on the manual control switch position
+	 * @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN
+	 */
+	float		get_landing_gear_state();
+
+
 	/**
 	 * Attitude controller.
 	 */
@@ -150,6 +164,7 @@ private:
 	orb_advert_t	_v_rates_sp_pub{nullptr};		/**< rate setpoint publication */
 	orb_advert_t	_actuators_0_pub{nullptr};		/**< attitude actuator controls publication */
 	orb_advert_t	_controller_status_pub{nullptr};	/**< controller status publication */
+	orb_advert_t	_vehicle_attitude_setpoint_pub{nullptr};
 
 	orb_id_t _rates_sp_id{nullptr};		/**< pointer to correct rates setpoint uORB metadata structure */
 	orb_id_t _actuators_id{nullptr};	/**< pointer to correct actuator controls0 uORB metadata structure */
@@ -186,6 +201,9 @@ private:
 
 	matrix::Dcmf _board_rotation;			/**< rotation matrix for the orientation that the board is mounted */
 
+	float _man_yaw_sp{0.f};				/**< current yaw setpoint in manual mode */
+	bool _gear_state_initialized{false};		/**< true if the gear state has been initialized */
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MC_ROLL_P>) _roll_p,
 		(ParamFloat<px4::params::MC_ROLLRATE_P>) _roll_rate_p,
@@ -221,6 +239,7 @@ private:
 		(ParamFloat<px4::params::MC_PITCHRATE_MAX>) _pitch_rate_max,
 		(ParamFloat<px4::params::MC_YAWRATE_MAX>) _yaw_rate_max,
 		(ParamFloat<px4::params::MC_YAWRAUTO_MAX>) _yaw_auto_max,
+		(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _yaw_rate_scaling,			/**< scaling factor from stick to yaw rate */
 
 		(ParamFloat<px4::params::MC_ACRO_R_MAX>) _acro_roll_max,
 		(ParamFloat<px4::params::MC_ACRO_P_MAX>) _acro_pitch_max,
@@ -238,7 +257,14 @@ private:
 
 		(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _board_offset_x,
 		(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _board_offset_y,
-		(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _board_offset_z
+		(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _board_offset_z,
+
+		/* Stabilized mode params */
+		(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _man_tilt_max_deg,			/**< maximum tilt allowed for manual flight */
+		(ParamFloat<px4::params::MPC_MANTHR_MIN>) _man_throttle_min,			/**< minimum throttle for stabilized */
+		(ParamFloat<px4::params::MPC_THR_MAX>) _throttle_max,				/**< maximum throttle for stabilized */
+		(ParamFloat<px4::params::MPC_THR_HOVER>) _throttle_hover,			/**< throttle at which vehicle is at hover equilibrium */
+		(ParamInt<px4::params::MPC_THR_CURVE>) _throttle_curve				/**< throttle curve behavior */
 	)
 
 	matrix::Vector3f _attitude_p;		/**< P gain for attitude control */
@@ -251,6 +277,7 @@ private:
 	matrix::Vector3f _mc_rate_max;		/**< attitude rate limits in stabilized modes */
 	matrix::Vector3f _auto_rate_max;	/**< attitude rate limits in auto modes */
 	matrix::Vector3f _acro_rate_max;	/**< max attitude rates in acro mode */
+	float _man_tilt_max;			/**< maximum tilt allowed for manual flight [rad] */
 
 };
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -177,6 +177,8 @@ MulticopterAttitudeControl::parameters_updated()
 	_acro_rate_max(1) = math::radians(_acro_pitch_max.get());
 	_acro_rate_max(2) = math::radians(_acro_yaw_max.get());
 
+	_man_tilt_max = math::radians(_man_tilt_max_deg.get());
+
 	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
 
 	/* get transformation matrix from sensor/board to body frame */
@@ -319,7 +321,15 @@ MulticopterAttitudeControl::vehicle_attitude_poll()
 	orb_check(_v_att_sub, &updated);
 
 	if (updated) {
+		uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter;
+
 		orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
+
+		// Check for a heading reset
+		if (prev_quat_reset_counter != _v_att.quat_reset_counter) {
+			// we only extract the heading change from the delta quaternion
+			_man_yaw_sp += Eulerf(Quatf(_v_att.delta_q_reset)).psi();
+		}
 		return true;
 	}
 	return false;
@@ -368,6 +378,143 @@ MulticopterAttitudeControl::vehicle_land_detected_poll()
 
 }
 
+float
+MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
+{
+	// throttle_stick_input is in range [0, 1]
+	switch (_throttle_curve.get()) {
+	case 1: // no rescaling to hover throttle
+		return _man_throttle_min.get() + throttle_stick_input * (_throttle_max.get() - _man_throttle_min.get());
+
+	default: // 0 or other: rescale to hover throttle at 0.5 stick
+		if (throttle_stick_input < 0.5f) {
+			return (_throttle_hover.get() - _man_throttle_min.get()) / 0.5f * throttle_stick_input + _man_throttle_min.get();
+
+		} else {
+			return (_throttle_max.get() - _throttle_hover.get()) / 0.5f * (throttle_stick_input - 1.0f) + _throttle_max.get();
+		}
+	}
+}
+
+float
+MulticopterAttitudeControl::get_landing_gear_state()
+{
+	// Only switch the landing gear up if we are not landed and if
+	// the user switched from gear down to gear up.
+	// If the user had the switch in the gear up position and took off ignore it
+	// until he toggles the switch to avoid retracting the gear immediately on takeoff.
+	if (_vehicle_land_detected.landed) {
+		_gear_state_initialized = false;
+	}
+	float landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN; // default to down
+	if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) {
+		landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP;
+
+	} else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
+		// Switching the gear off does put it into a safe defined state
+		_gear_state_initialized = true;
+	}
+
+	return landing_gear;
+}
+
+void
+MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_sp)
+{
+	vehicle_attitude_setpoint_s attitude_setpoint{};
+	const float yaw = Eulerf(Quatf(_v_att.q)).psi();
+
+	/* reset yaw setpoint to current position if needed */
+	if (reset_yaw_sp) {
+		_man_yaw_sp = yaw;
+
+	} else if (_manual_control_sp.z > 0.05f) {
+
+		const float yaw_rate = math::radians(_yaw_rate_scaling.get());
+		attitude_setpoint.yaw_sp_move_rate = _manual_control_sp.r * yaw_rate;
+		_man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt);
+	}
+
+	/*
+	 * Input mapping for roll & pitch setpoints
+	 * ----------------------------------------
+	 * We control the following 2 angles:
+	 * - tilt angle, given by sqrt(x*x + y*y)
+	 * - the direction of the maximum tilt in the XY-plane, which also defines the direction of the motion
+	 *
+	 * This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick
+	 * points to, and changes of the stick input are linear.
+	 */
+	const float x = _manual_control_sp.x * _man_tilt_max;
+	const float y = _manual_control_sp.y * _man_tilt_max;
+
+	// we want to fly towards the direction of (x, y), so we use a perpendicular axis angle vector in the XY-plane
+	Vector2f v = Vector2f(y, -x);
+	float v_norm = v.norm(); // the norm of v defines the tilt angle
+
+	if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle
+		v *= _man_tilt_max / v_norm;
+	}
+
+	Quatf q_sp_rpy = AxisAnglef(v(0), v(1), 0.f);
+	Eulerf euler_sp = q_sp_rpy;
+	attitude_setpoint.roll_body = euler_sp(0);
+	attitude_setpoint.pitch_body = euler_sp(1);
+	// The axis angle can change the yaw as well (noticeable at higher tilt angles).
+	// This is the formula by how much the yaw changes:
+	//   let a := tilt angle, b := atan(y/x) (direction of maximum tilt)
+	//   yaw = atan(-2 * sin(b) * cos(b) * sin^2(a/2) / (1 - 2 * cos^2(b) * sin^2(a/2))).
+	attitude_setpoint.yaw_body = _man_yaw_sp + euler_sp(2);
+
+	/* modify roll/pitch only if we're a VTOL */
+	if (_vehicle_status.is_vtol) {
+		// Construct attitude setpoint rotation matrix. Modify the setpoints for roll
+		// and pitch such that they reflect the user's intention even if a large yaw error
+		// (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix
+		// from the pure euler angle setpoints will lead to unexpected attitude behaviour from
+		// the user's view as the euler angle sequence uses the  yaw setpoint and not the current
+		// heading of the vehicle.
+		// However there's also a coupling effect that causes oscillations for fast roll/pitch changes
+		// at higher tilt angles, so we want to avoid using this on multicopters.
+		// The effect of that can be seen with:
+		// - roll/pitch into one direction, keep it fixed (at high angle)
+		// - apply a fast yaw rotation
+		// - look at the roll and pitch angles: they should stay pretty much the same as when not yawing
+
+		// calculate our current yaw error
+		float yaw_error = wrap_pi(attitude_setpoint.yaw_body - yaw);
+
+		// compute the vector obtained by rotating a z unit vector by the rotation
+		// given by the roll and pitch commands of the user
+		Vector3f zB = {0.0f, 0.0f, 1.0f};
+		Dcmf R_sp_roll_pitch = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, 0.0f);
+		Vector3f z_roll_pitch_sp = R_sp_roll_pitch * zB;
+
+		// transform the vector into a new frame which is rotated around the z axis
+		// by the current yaw error. this vector defines the desired tilt when we look
+		// into the direction of the desired heading
+		Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error);
+		z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;
+
+		// use the formula z_roll_pitch_sp = R_tilt * [0;0;1]
+		// R_tilt is computed from_euler; only true if cos(roll) not equal zero
+		// -> valid if roll is not +-pi/2;
+		attitude_setpoint.roll_body = -asinf(z_roll_pitch_sp(1));
+		attitude_setpoint.pitch_body = atan2f(z_roll_pitch_sp(0), z_roll_pitch_sp(2));
+	}
+
+	/* copy quaternion setpoint to attitude setpoint topic */
+	Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body);
+	q_sp.copyTo(attitude_setpoint.q_d);
+	attitude_setpoint.q_d_valid = true;
+
+	attitude_setpoint.thrust = throttle_curve(_manual_control_sp.z);
+
+	attitude_setpoint.landing_gear = get_landing_gear_state();
+	attitude_setpoint.timestamp = hrt_absolute_time();
+	orb_publish_auto(ORB_ID(vehicle_attitude_setpoint), &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
+}
+
 /**
  * Attitude controller.
  * Input: 'vehicle_attitude_setpoint' topics (depending on mode)
@@ -666,6 +813,9 @@ MulticopterAttitudeControl::run()
 	float dt_accumulator = 0.f;
 	int loop_counter = 0;
 
+	bool reset_yaw_sp = true;
+	float attitude_dt = 0.f;
+
 	while (!should_exit()) {
 
 		poll_fds.fd = _sensor_gyro_sub[_selected_gyro];
@@ -723,6 +873,7 @@ MulticopterAttitudeControl::run()
 			vehicle_land_detected_poll();
 			const bool manual_control_updated = vehicle_manual_poll();
 			const bool attitude_updated = vehicle_attitude_poll();
+			attitude_dt += dt;
 
 			/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
 			 * or roll (yaw can rotate 360 in normal att control).  If both are true don't
@@ -734,8 +885,19 @@ MulticopterAttitudeControl::run()
 				}
 			}
 
+			bool attitude_setpoint_generated = false;
+
 			if (_v_control_mode.flag_control_attitude_enabled) {
 				if (attitude_updated) {
+					// Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
+					if (_v_control_mode.flag_control_manual_enabled &&
+							!_v_control_mode.flag_control_altitude_enabled &&
+							!_v_control_mode.flag_control_velocity_enabled &&
+							!_v_control_mode.flag_control_position_enabled) {
+						generate_attitude_setpoint(attitude_dt, reset_yaw_sp);
+						attitude_setpoint_generated = true;
+					}
+
 					control_attitude();
 					publish_rates_setpoint();
 				}
@@ -775,6 +937,13 @@ MulticopterAttitudeControl::run()
 				}
 			}
 
+			if (attitude_updated) {
+				reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) ||
+						_vehicle_land_detected.landed ||
+						(_vehicle_status.is_vtol && !_vehicle_status.is_rotary_wing); // VTOL in FW mode
+				attitude_dt = 0.f;
+			}
+
 			/* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */
 			if (!_v_control_mode.flag_armed || (now - task_start) < 3300000) {
 				dt_accumulator += dt;

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -1003,25 +1003,6 @@ MulticopterPositionControl::start_flight_task()
 		}
 	}
 
-	// manual stabilized control
-	if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL
-	    ||  _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || task_failure) {
-		should_disable_task = false;
-		int error = _flight_tasks.switchTask(FlightTaskIndex::ManualStabilized);
-
-		if (error != 0) {
-			if (prev_failure_count == 0) {
-				PX4_WARN("Stabilized-Ctrl failed with error: %s", _flight_tasks.errorToString(error));
-			}
-			task_failure = true;
-			_task_failure_count++;
-
-		} else {
-			check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_STAB);
-			task_failure = false;
-		}
-	}
-
 	// check task failure
 	if (task_failure) {