Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

Reduce the orbit jerk by using a slew rate

Browse Source
This commit is contained in:
Claudio Chies
2024-09-10 11:03:18 +02:00
committed by Mathieu Bresciani
parent 22c2878cf8
commit 4ba4b340cc
2 changed files with 11 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+9 -2
View File
@@ -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;
}
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
+2
View File
@@ -47,6 +47,7 @@
#include <lib/slew_rate/SlewRateYaw.hpp>
#include <lib/motion_planning/PositionSmoothing.hpp>
#include <lib/motion_planning/VelocitySmoothing.hpp>
#include <lib/slew_rate/SlewRate.hpp>
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<float> _slew_rate_yaw;
SlewRate<float> _slew_rate_velocity;
orb_advert_t _mavlink_log_pub{nullptr};
uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};