MC stabilized mode: move from mc_pos_control to mc_att_control

msg/vehicle_attitude_setpoint.msg

@@ -2,9 +2,9 @@ uint64 timestamp		# time since system start (microseconds)
 int8 LANDING_GEAR_UP = 1	# landing gear up
 int8 LANDING_GEAR_DOWN = -1	# landing gear down
 
-float32 roll_body		# body angle in NED frame
-float32 pitch_body		# body angle in NED frame
-float32 yaw_body		# body angle in NED frame
+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)
 

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/FlightTask/FlightTask.hpp

@@ -150,7 +150,7 @@ class FlightTask : public ModuleParams
 	 * Sets an external yaw handler which can be used by any flight task to implement a different yaw control strategy.
 	 * This method does nothing, each flighttask which wants to use the yaw handler needs to override this method.
 	 */
-	virtual void setYawHandler(WeatherVane *ext_yaw_handler) {};
+	virtual void setYawHandler(WeatherVane *ext_yaw_handler) {}
 
 	void updateVelocityControllerIO(const matrix::Vector3f &vel_sp,
 					const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; }
@@ -166,7 +166,7 @@ class FlightTask : public ModuleParams
 	 */
 	void _resetSetpoints();
 
-	/*
+	/**
 	 * Check and update local position
 	 */
 	void _evaluateVehicleLocalPosition();

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 = NAN;
+	_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,10 +82,9 @@ bool FlightTaskManualAltitude::activate()
 
 void FlightTaskManualAltitude::_scaleSticks()
 {
-	// reuse same scaling as for stabilized
-	FlightTaskManualStabilized::_scaleSticks();
+	// Use sticks input with deadzone and exponential curve for vertical velocity and yawspeed
+	_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;
 	_velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
 }
@@ -249,9 +252,41 @@ 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;
+			}
+		}
+	}
+}
+
 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 +306,11 @@ void FlightTaskManualAltitude::_updateSetpoints()
 
 	_updateAltitudeLock();
 }
+
+bool FlightTaskManualAltitude::update()
+{
+	_scaleSticks();
+	_updateSetpoints();
+
+	return true;
+}

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

@@ -39,19 +39,26 @@
 
 #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;
 
 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 +67,15 @@ class FlightTaskManualAltitude : public FlightTaskManualStabilized
 	 */
 	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, /**< position controller altitude propotional gain */
+					(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 +94,19 @@ class FlightTaskManualAltitude : public FlightTaskManualStabilized
 	void _respectMinAltitude();
 
 	void _respectMaxAltitude();
+
+
+	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();
@@ -146,6 +147,13 @@ void FlightTaskManualPosition::_updateXYlock()
 void FlightTaskManualPosition::_updateSetpoints()
 {
 	FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction
+
+	// check if an external yaw handler is active and if yes, let it update the yaw setpoints
+	if (_weathervane_yaw_handler != nullptr && _weathervane_yaw_handler->is_active()) {
+		_yaw_setpoint = NAN;
+		_yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate();
+	}
+
 	_thrust_setpoint.setAll(NAN); // don't require any thrust setpoints
 	_updateXYlock(); // check for position lock
 }

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

@@ -51,6 +51,11 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude
 	bool activate() override;
 	bool updateInitialize() override;
 
+	/**
+	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
+	 */
+	void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; }
+
 protected:
 	void _updateXYlock(); /**< applies position lock based on stick and velocity */
 	void _updateSetpoints() override;
@@ -65,4 +70,8 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude
 private:
 	float _velocity_scale{0.0f}; //scales the stick input to velocity
 	uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
+
+	WeatherVane *_weathervane_yaw_handler =
+		nullptr;	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+
 };