diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 144c6c7ba1..3136c98051 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/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; } diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index 2c55bbc15b..13e53bf83f 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -47,6 +47,7 @@ #include #include #include +#include class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel @@ -127,6 +128,7 @@ private: bool _currently_orbiting{false}; float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */ SlewRateYaw _slew_rate_yaw; + SlewRate _slew_rate_velocity; orb_advert_t _mavlink_log_pub{nullptr}; uORB::PublicationMulti _orbit_status_pub{ORB_ID(orbit_status)};