FlightTaskAltitude: initial draft of a cruise mode

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -72,6 +72,13 @@ bool FlightTaskManualAltitude::activate(const trajectory_setpoint_s &last_setpoi
 	return ret;
 }
 
+void FlightTaskManualAltitude::reActivate()
+{
+	FlightTask::reActivate();
+
+	_stick_tilt_xy.reset();
+}
+
 void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
 {
 	if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_min)) {
@@ -275,8 +282,8 @@ void FlightTaskManualAltitude::_ekfResetHandlerHagl(float delta_hagl)
 void FlightTaskManualAltitude::_updateSetpoints()
 {
 	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _deltatime, _unaided_yaw);
-	_acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw,
+	_acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpointsForCruise(_sticks.getPitchRoll(), _deltatime,
-				      _yaw_setpoint);
+				      _yaw, _yaw_setpoint);
 	_updateAltitudeLock();
 	_respectGroundSlowdown();
 }

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -51,6 +51,7 @@ public:
 	FlightTaskManualAltitude() = default;
 	virtual ~FlightTaskManualAltitude() = default;
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+	void reActivate() override;
 	bool updateInitialize() override;
 	bool update() override;
 	void setMaxDistanceToGround(float max_distance) { _max_distance_to_ground = max_distance; }

src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.cpp

@@ -44,6 +44,11 @@ StickTiltXY::StickTiltXY(ModuleParams *parent) :
 	updateParams();
 }
 
+void StickTiltXY::reset()
+{
+	_cruise_acceleration.setZero();
+}
+
 void StickTiltXY::updateParams()
 {
 	ModuleParams::updateParams();
@@ -53,12 +58,31 @@ void StickTiltXY::updateParams()
 	_maximum_acceleration = math::constrain(tanf(maximum_tilt), .02f, 3.f) * CONSTANTS_ONE_G;
 }
 
-Vector2f StickTiltXY::generateAccelerationSetpoints(Vector2f stick_xy, const float dt, const float yaw,
+Vector2f StickTiltXY::processSticks(Vector2f stick_xy, const float dt, const float yaw,
-		const float yaw_setpoint)
+				    const float yaw_setpoint)
 {
 	Sticks::limitStickUnitLengthXY(stick_xy);
 	_man_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
 	stick_xy = _man_input_filter.update(stick_xy);
 	Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_setpoint);
-	return stick_xy * _maximum_acceleration;
+	return stick_xy;
+}
+
+Vector2f StickTiltXY::generateAccelerationSetpoints(Vector2f stick_xy, const float dt, const float yaw,
+		const float yaw_setpoint)
+{
+	return processSticks(stick_xy, dt, yaw, yaw_setpoint) * _maximum_acceleration;
+}
+
+Vector2f StickTiltXY::generateAccelerationSetpointsForCruise(Vector2f stick_xy, const float dt, const float yaw,
+		const float yaw_setpoint)
+{
+	Vector2f increment = processSticks(stick_xy, dt, yaw, yaw_setpoint);
+	_cruise_acceleration += increment * _maximum_acceleration * dt;
+
+	if (_cruise_acceleration.longerThan(_maximum_acceleration)) {
+		_cruise_acceleration = _cruise_acceleration.unit_or_zero() * _maximum_acceleration;
+	}
+
+	return _cruise_acceleration;
 }

src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.hpp

@@ -62,11 +62,19 @@ public:
 	 */
 	matrix::Vector2f generateAccelerationSetpoints(matrix::Vector2f stick_xy, const float dt, const float yaw,
 			const float yaw_setpoint);
+
+	matrix::Vector2f generateAccelerationSetpointsForCruise(matrix::Vector2f stick_xy, const float dt, const float yaw,
+			const float yaw_setpoint);
+
+	void reset();
 private:
 	void updateParams() override;
 
+	matrix::Vector2f processSticks(matrix::Vector2f stick_xy, const float dt, const float yaw, const float yaw_setpoint);
+
 	float _maximum_acceleration{0.f};
 	AlphaFilter<matrix::Vector2f> _man_input_filter;
+	matrix::Vector2f _cruise_acceleration{};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, ///< maximum tilt allowed for manual flight