FlightTaskAutoMapper: move remaining methods into FlightTaskAutoLineSmooth

src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -36,6 +36,7 @@
  */
 
 #include "FlightTaskAutoLineSmoothVel.hpp"
+#include <mathlib/mathlib.h>
 
 using namespace matrix;
 
@@ -313,3 +314,95 @@ void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
 		_position_smoothing.setMaxVelocityZ(_param_mpc_z_vel_max_dn.get());
 	}
 }
+
+void FlightTaskAutoLineSmoothVel::_prepareIdleSetpoints()
+{
+	// Send zero thrust setpoint
+	_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
+	_velocity_setpoint.setNaN();
+	_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
+}
+
+void FlightTaskAutoLineSmoothVel::_prepareLandSetpoints()
+{
+	_velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint
+
+	// Slow down automatic descend close to ground
+	float land_speed = math::gradual(_dist_to_ground,
+					 _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+					 _param_mpc_land_speed.get(), _constraints.speed_down);
+
+	if (_type_previous != WaypointType::land) {
+		// initialize xy-position and yaw to waypoint such that home is reached exactly without user input
+		_land_position = Vector3f(_target(0), _target(1), NAN);
+		_land_heading = _yaw_setpoint;
+		_stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1)));
+	}
+
+	// User input assisted landing
+	if (_param_mpc_land_rc_help.get() && _sticks.checkAndSetStickInputs()) {
+		// Stick full up -1 -> stop, stick full down 1 -> double the speed
+		land_speed *= (1 + _sticks.getPositionExpo()(2));
+
+		_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
+					       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime);
+		_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position,
+				_velocity_setpoint_feedback.xy(), _deltatime);
+		_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
+
+		// Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input
+		if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) {
+			_yaw_sp_aligned = true;
+		}
+
+	} else {
+		// Make sure we have a valid land position even in the case we loose RC while amending it
+		if (!PX4_ISFINITE(_land_position(0))) {
+			_land_position.xy() = Vector2f(_position);
+		}
+	}
+
+	_position_setpoint = _land_position; // The last element of the land position has to stay NAN
+	_yaw_setpoint = _land_heading;
+	_velocity_setpoint(2) = land_speed;
+	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+}
+
+void FlightTaskAutoLineSmoothVel::_prepareTakeoffSetpoints()
+{
+	// Takeoff is completely defined by target position
+	_position_setpoint = _target;
+	_velocity_setpoint = Vector3f(NAN, NAN, NAN);
+
+	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+}
+
+void FlightTaskAutoLineSmoothVel::_prepareVelocitySetpoints()
+{
+	// XY Velocity waypoint
+	// TODO : Rewiew that. What is the expected behavior?
+	_position_setpoint = Vector3f(NAN, NAN, _position(2));
+	Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed;
+	_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
+}
+
+void FlightTaskAutoLineSmoothVel::_preparePositionSetpoints()
+{
+	// Simple waypoint navigation: go to xyz target, with standard limitations
+	_position_setpoint = _target;
+	_velocity_setpoint.setNaN(); // No special velocity limitations
+}
+
+void FlightTaskAutoLineSmoothVel::updateParams()
+{
+	FlightTaskAuto::updateParams();
+
+	// make sure that alt1 is above alt2
+	_param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
+}
+
+bool FlightTaskAutoLineSmoothVel::_highEnoughForLandingGear()
+{
+	// return true if altitude is above two meters
+	return _dist_to_ground > 2.0f;
+}

src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp

@@ -83,6 +83,15 @@ protected:
 	bool _checkTakeoff() override { return _want_takeoff; };
 	bool _want_takeoff{false};
 
+	void _prepareIdleSetpoints();
+	void _prepareLandSetpoints();
+	void _prepareVelocitySetpoints();
+	void _prepareTakeoffSetpoints();
+	void _preparePositionSetpoints();
+	bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
+
+	void updateParams() override; /**< See ModuleParam class */
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper,
 					(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
 					(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight

src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -36,103 +36,8 @@
  */
 
 #include "FlightTaskAutoMapper.hpp"
-#include <mathlib/mathlib.h>
-
-using namespace matrix;
 
 FlightTaskAutoMapper::FlightTaskAutoMapper() :
 	_sticks(this),
 	_stick_acceleration_xy(this)
 {}
-
-void FlightTaskAutoMapper::_prepareIdleSetpoints()
-{
-	// Send zero thrust setpoint
-	_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
-	_velocity_setpoint.setNaN();
-	_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
-}
-
-void FlightTaskAutoMapper::_prepareLandSetpoints()
-{
-	_velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint
-
-	// Slow down automatic descend close to ground
-	float land_speed = math::gradual(_dist_to_ground,
-					 _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
-					 _param_mpc_land_speed.get(), _constraints.speed_down);
-
-	if (_type_previous != WaypointType::land) {
-		// initialize xy-position and yaw to waypoint such that home is reached exactly without user input
-		_land_position = Vector3f(_target(0), _target(1), NAN);
-		_land_heading = _yaw_setpoint;
-		_stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1)));
-	}
-
-	// User input assisted landing
-	if (_param_mpc_land_rc_help.get() && _sticks.checkAndSetStickInputs()) {
-		// Stick full up -1 -> stop, stick full down 1 -> double the speed
-		land_speed *= (1 + _sticks.getPositionExpo()(2));
-
-		_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
-					       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime);
-		_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position,
-				_velocity_setpoint_feedback.xy(), _deltatime);
-		_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
-
-		// Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input
-		if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) {
-			_yaw_sp_aligned = true;
-		}
-
-	} else {
-		// Make sure we have a valid land position even in the case we loose RC while amending it
-		if (!PX4_ISFINITE(_land_position(0))) {
-			_land_position.xy() = Vector2f(_position);
-		}
-	}
-
-	_position_setpoint = _land_position; // The last element of the land position has to stay NAN
-	_yaw_setpoint = _land_heading;
-	_velocity_setpoint(2) = land_speed;
-	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
-}
-
-void FlightTaskAutoMapper::_prepareTakeoffSetpoints()
-{
-	// Takeoff is completely defined by target position
-	_position_setpoint = _target;
-	_velocity_setpoint = Vector3f(NAN, NAN, NAN);
-
-	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
-}
-
-void FlightTaskAutoMapper::_prepareVelocitySetpoints()
-{
-	// XY Velocity waypoint
-	// TODO : Rewiew that. What is the expected behavior?
-	_position_setpoint = Vector3f(NAN, NAN, _position(2));
-	Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed;
-	_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
-}
-
-void FlightTaskAutoMapper::_preparePositionSetpoints()
-{
-	// Simple waypoint navigation: go to xyz target, with standard limitations
-	_position_setpoint = _target;
-	_velocity_setpoint.setNaN(); // No special velocity limitations
-}
-
-void FlightTaskAutoMapper::updateParams()
-{
-	FlightTaskAuto::updateParams();
-
-	// make sure that alt1 is above alt2
-	_param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
-}
-
-bool FlightTaskAutoMapper::_highEnoughForLandingGear()
-{
-	// return true if altitude is above two meters
-	return _dist_to_ground > 2.0f;
-}

src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp

@@ -52,13 +52,6 @@ public:
 	virtual ~FlightTaskAutoMapper() = default;
 
 protected:
-	void _prepareIdleSetpoints();
-	void _prepareLandSetpoints();
-	void _prepareVelocitySetpoints();
-	void _prepareTakeoffSetpoints();
-	void _preparePositionSetpoints();
-
-	void updateParams() override; /**< See ModuleParam class */
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
 					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
@@ -80,5 +73,5 @@ protected:
 	matrix::Vector3f _land_position;
 	float _land_heading;
 	WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
-	bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
+
 };