FlightTaskOrbit: Fix altitude adjustment by stick

This is done by inheriting from FlightTaskManualAltitudeSmoothVel again.
The altitude change by command is taken care of by switching
to the apporach when the altitude difference is big enough and
switching back once the altitude is close enough.

The altitude of the command is not perfectly reached but this can
only be done smoothly when the Orbit has full control over the
altitude smoothing. The independent altitude smoothing is not kept
because it was lacking stick handling like altitude lock and smooth
transitions when opening and closing the vertical position loop.

src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp

@@ -57,15 +57,14 @@ protected:
 	void _ekfResetHandlerPositionZ(float delta_z) override;
 	void _ekfResetHandlerVelocityZ(float delta_vz) override;
 
+	void _updateTrajConstraints();
+	void _setOutputState();
+
+	ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
 					(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
 					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
 					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
 				       )
-
-private:
-	void _updateTrajConstraints();
-	void _setOutputState();
-
-	ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction
 };

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp

@@ -188,9 +188,7 @@ bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_set
 
 bool FlightTaskOrbit::update()
 {
-	// update altitude
-	bool ret = FlightTaskManualAltitude::update();
-
+	bool ret = true;
 	_updateTrajectoryBoundaries();
 
 	// stick input adjusts parameters within a fixed time frame
@@ -203,7 +201,10 @@ bool FlightTaskOrbit::update()
 	if (_is_position_on_circle()) {
 		if (_in_circle_approach) {
 			_in_circle_approach = false;
-			_altitude_velocity_smoothing.reset(0, _velocity(2), _position(2));
+			FlightTaskManualAltitudeSmoothVel::_smoothing.reset(
+				PX4_ISFINITE(_acceleration_setpoint(2)) ? _acceleration_setpoint(2) : 0.f,
+				PX4_ISFINITE(_velocity_setpoint(2)) ? _velocity_setpoint(2) : _velocity(2),
+				PX4_ISFINITE(_position_setpoint(2)) ? _position_setpoint(2) : _position(2));
 		}
 	}
 
@@ -211,20 +212,12 @@ bool FlightTaskOrbit::update()
 		_generate_circle_approach_setpoints();
 
 	} else {
+		// update altitude
+		ret = ret && FlightTaskManualAltitudeSmoothVel::update();
+
 		// this generates x / y setpoints
 		_generate_circle_setpoints();
 		_generate_circle_yaw_setpoints();
-
-		// in case we have a velocity setpoint in altititude (from altitude parent)
-		// smooth this
-		if (!PX4_ISFINITE(_position_setpoint(2))) {
-			_altitude_velocity_smoothing.updateDurations(_velocity_setpoint(2));
-			_altitude_velocity_smoothing.updateTraj(_deltatime);
-			_velocity_setpoint(2) = _altitude_velocity_smoothing.getCurrentVelocity();
-			_acceleration_setpoint(2) = _altitude_velocity_smoothing.getCurrentAcceleration();
-			// set orbit altitude center to expected new altitude
-			_center(2) = _altitude_velocity_smoothing.getCurrentPosition();
-		}
 	}
 
 	// Apply yaw smoothing
@@ -250,21 +243,14 @@ void FlightTaskOrbit::_updateTrajectoryBoundaries()
 	_position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get());
 	float max_jerk = _param_mpc_jerk_auto.get();
 	_position_smoothing.setMaxJerk({max_jerk, max_jerk, max_jerk}); // TODO : Should be computed using heading
-	_altitude_velocity_smoothing.setMaxJerk(max_jerk);
-
-		const float z_accel_constraint = _param_mpc_acc_up_max.get();
 
 	if (_velocity_setpoint(2) < 0.f) { // up
 		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_up.get());
-		_position_smoothing.setMaxAccelerationZ(z_accel_constraint);
-		_altitude_velocity_smoothing.setMaxVel(_param_mpc_z_vel_max_up.get());
-		_altitude_velocity_smoothing.setMaxAccel(z_accel_constraint);
+		_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_up_max.get());
 
 	} else { // down
 		_position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get());
 		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get());
-		_altitude_velocity_smoothing.setMaxAccel(_param_mpc_acc_down_max.get());
-		_altitude_velocity_smoothing.setMaxVel(_param_mpc_z_vel_max_dn.get());
 	}
 
 }

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp

@@ -49,7 +49,7 @@
 #include <lib/motion_planning/VelocitySmoothing.hpp>
 
 
-class FlightTaskOrbit : public FlightTaskManualAltitude
+class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel
 {
 public:
 
@@ -119,7 +119,6 @@ private:
 
 	bool _in_circle_approach = false;
 	PositionSmoothing _position_smoothing;
-	VelocitySmoothing _altitude_velocity_smoothing;
 
 	/** yaw behaviour during the orbit flight according to MAVLink's ORBIT_YAW_BEHAVIOUR enum */
 	int _yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER;