Reduce the orbit jerk by using a slew rate

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

@@ -181,6 +181,7 @@ bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint)
 	_initial_heading = _yaw;
 	_slew_rate_yaw.setForcedValue(_yaw);
 	_slew_rate_yaw.setSlewRate(math::radians(_param_mpc_yawrauto_max.get()));
+	_slew_rate_velocity.setSlewRate(_param_mpc_acc_hor.get());
 
 	// need a valid position and velocity
 	ret = ret && _position.isAllFinite() && _velocity.isAllFinite();
@@ -216,6 +217,7 @@ bool FlightTaskOrbit::update()
 	if (_is_position_on_circle()) {
 		if (_in_circle_approach) {
 			_in_circle_approach = false;
+			_slew_rate_velocity.setForcedValue(0.f); // reset the slew rate when moving between orbits.
 			FlightTaskManualAltitudeSmoothVel::_smoothing.reset(
 				PX4_ISFINITE(_acceleration_setpoint(2)) ? _acceleration_setpoint(2) : 0.f,
 				PX4_ISFINITE(_velocity_setpoint(2)) ? _velocity_setpoint(2) : _velocity(2),
@@ -329,15 +331,20 @@ void FlightTaskOrbit::_generate_circle_setpoints()
 	Vector3f center_to_position = _position - _center;
 	// xy velocity to go around in a circle
 	Vector2f velocity_xy(-center_to_position(1), center_to_position(0));
+
+	// slew rate is used to reduce the jerk when starting an orbit.
+	_slew_rate_velocity.update(_orbit_velocity, _deltatime);
+
 	velocity_xy = velocity_xy.unit_or_zero();
-	velocity_xy *= _orbit_velocity;
+	velocity_xy *= _slew_rate_velocity.getState();
 
 	// xy velocity adjustment to stay on the radius distance
 	velocity_xy += (_orbit_radius - center_to_position.xy().norm()) * Vector2f(center_to_position).unit_or_zero();
 
 	_position_setpoint(0) = _position_setpoint(1) = NAN;
 	_velocity_setpoint.xy() = velocity_xy;
-	_acceleration_setpoint.xy() = -Vector2f(center_to_position.unit_or_zero()) * _orbit_velocity * _orbit_velocity /
+	_acceleration_setpoint.xy() = -Vector2f(center_to_position.unit_or_zero()) * _slew_rate_velocity.getState() *
+				      _slew_rate_velocity.getState() /
 				      _orbit_radius;
 }