Orbit approach transition improvements

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -66,7 +66,7 @@ bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setp
 	Vector3f accel_prev{last_setpoint.acceleration};
 
 	for (int i = 0; i < 3; i++) {
-		// If the position setpoint is unknown, set to the current postion
+		// If the position setpoint is unknown, set to the current position
 		if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); }
 
 		// If the velocity setpoint is unknown, set to the current velocity

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

@@ -57,15 +57,14 @@ class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude
 	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

@@ -51,25 +51,24 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 {
 	bool ret = true;
 	// save previous velocity and roatation direction
-	bool is_clockwise = _orbit_velocity > 0;
-
+	bool new_is_clockwise = _orbit_velocity > 0;
 	float new_radius = _orbit_radius;
-	float new_abs_velocity = fabsf(_orbit_velocity);
+	float new_absolute_velocity = fabsf(_orbit_velocity);
 
 	// commanded radius
 	if (PX4_ISFINITE(command.param1)) {
 		// Note: Radius sign is defined as orbit direction in MAVLINK
-		float radius = command.param1;
-		is_clockwise = radius > 0;
-		new_radius = fabsf(radius);
+		const float radius_parameter = command.param1;
+		new_is_clockwise = radius_parameter > 0;
+		new_radius = fabsf(radius_parameter);
 	}
 
 	// commanded velocity, take sign of radius as rotation direction
 	if (PX4_ISFINITE(command.param2)) {
-		new_abs_velocity = command.param2;
+		new_absolute_velocity = command.param2;
 	}
 
-	float new_velocity = (is_clockwise ? 1.f : -1.f) * new_abs_velocity;
+	float new_velocity = (new_is_clockwise ? 1.f : -1.f) * new_absolute_velocity;
 	_sanitizeParams(new_radius, new_velocity);
 	_orbit_radius = new_radius;
 	_orbit_velocity = new_velocity;
@@ -103,10 +102,9 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 	}
 
 	// perpendicularly approach the orbit circle again when new parameters get commanded
-	if (!_is_position_on_circle()) {
+	if (!_is_position_on_circle() && !_in_circle_approach) {
 		_in_circle_approach = true;
-		_position_smoothing.reset({0.f, 0.f, 0.f}, _velocity, _position);
-		_circle_approach_start_position = _position;
+		_position_smoothing.reset(_acceleration_setpoint, _velocity_setpoint, _position);
 	}
 
 	return ret;
@@ -153,8 +151,7 @@ bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_set
 {
 	bool ret = FlightTaskManualAltitude::activate(last_setpoint);
 	_orbit_radius = _radius_min;
-	_orbit_velocity =  1.f;
-	_sanitizeParams(_orbit_radius, _orbit_velocity);
+	_orbit_velocity = 1.f;
 	_center = _position;
 	_initial_heading = _yaw;
 	_slew_rate_yaw.setForcedValue(_yaw);
@@ -168,58 +165,59 @@ bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_set
 	      && PX4_ISFINITE(_velocity(1))
 	      && PX4_ISFINITE(_velocity(2));
 
-	_position_smoothing.reset({0.f, 0.f, 0.f}, _velocity, _position);
-	_circle_approach_start_position = _position;
+	Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
+	Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
+	Vector3f accel_prev{last_setpoint.acceleration};
+
+	for (int i = 0; i < 3; i++) {
+		// If the position setpoint is unknown, set to the current position
+		if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); }
+
+		// If the velocity setpoint is unknown, set to the current velocity
+		if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); }
+
+		// No acceleration estimate available, set to zero if the setpoint is NAN
+		if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; }
+	}
+
+	_position_smoothing.reset(accel_prev, vel_prev, pos_prev);
+	_in_circle_approach = true;
 
 	return ret;
 }
 
 bool FlightTaskOrbit::update()
 {
-	// update altitude
-	bool ret = FlightTaskManualAltitude::update();
-
+	bool ret = true;
 	_updateTrajectoryBoundaries();
 
 	// stick input adjusts parameters within a fixed time frame
-	float radius = _orbit_radius - _sticks.getPositionExpo()(0) * _deltatime * (_radius_max / 8.f);
-	float velocity = _orbit_velocity - _sticks.getPositionExpo()(1) * _deltatime * (_velocity_max / 4.f);
+	float radius = _orbit_radius - _sticks.getPositionExpo()(0) * _deltatime * _param_mpc_xy_cruise.get();
+	float velocity = _orbit_velocity - _sticks.getPositionExpo()(1) * _deltatime * _param_mpc_acc_hor.get();
 	_sanitizeParams(radius, velocity);
 	_orbit_radius = radius;
 	_orbit_velocity = velocity;
 
 	if (_is_position_on_circle()) {
 		if (_in_circle_approach) {
 			_in_circle_approach = false;
-			_altitude_velocity_smoothing.reset(0, _velocity(2), _position(2));
-		}
-
-	} else {
-		if (!_in_circle_approach) {
-			_in_circle_approach = true;
-			_position_smoothing.reset({0.f, 0.f, 0.f}, _velocity, _position);
-			_circle_approach_start_position = _position;
+			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));
 		}
 	}
 
 	if (_in_circle_approach) {
 		_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
@@ -245,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);
-
-	if (_unsmoothed_velocity_setpoint(2) < 0.f) { // up
-		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());
 	}
 
 }
@@ -288,6 +279,8 @@ void FlightTaskOrbit::_generate_circle_approach_setpoints()
 
 	_position_setpoint = out_setpoints.position;
 	_velocity_setpoint = out_setpoints.velocity;
+	_acceleration_setpoint = out_setpoints.acceleration;
+	_jerk_setpoint = out_setpoints.jerk;
 }
 
 void FlightTaskOrbit::_generate_circle_setpoints()

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:
 
@@ -118,10 +118,7 @@ class FlightTaskOrbit : public FlightTaskManualAltitude
 	matrix::Vector3f _center; /**< local frame coordinates of the center point */
 
 	bool _in_circle_approach = false;
-	Vector3f _circle_approach_start_position;
 	PositionSmoothing _position_smoothing;
-	VelocitySmoothing _altitude_velocity_smoothing;
-	Vector3f _unsmoothed_velocity_setpoint;
 
 	/** 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;