fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 15:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskOrbit: also use HeadingSmoothing to avoid steps during the approach

Browse Source
This commit is contained in:
Matthias Grob
2025-05-21 13:25:46 +02:00
parent 70ad2e6fe5
commit a8f5b6dc1b
2 changed files with 14 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+11 -4
View File
@@ -179,8 +179,8 @@ bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint)
_orbit_velocity = 1.f;
_center = _position;
_initial_heading = _yaw;
_slew_rate_yaw.setForcedValue(_yaw);
_slew_rate_yaw.setSlewRate(math::radians(_param_mpc_yawrauto_max.get()));
_heading_smoothing.reset(PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw,
PX4_ISFINITE(last_setpoint.yawspeed) ? last_setpoint.yawspeed : 0.f);
_slew_rate_velocity.setSlewRate(_param_mpc_acc_hor.get());
// need a valid position and velocity
@@ -238,7 +238,14 @@ bool FlightTaskOrbit::update()
}
// Apply yaw smoothing
_yaw_setpoint = _slew_rate_yaw.update(_yaw_setpoint, _deltatime);
_heading_smoothing.setMaxHeadingRate(math::radians(_param_mpc_yawrauto_max.get()));
_heading_smoothing.setMaxHeadingAccel(math::radians(_param_mpc_yawrauto_acc.get()));
_heading_smoothing.update(_yaw_setpoint, _deltatime);
_yaw_setpoint = _heading_smoothing.getSmoothedHeading();
if (_in_circle_approach) { // don't override feed-forward which is already calculated for circling
_yawspeed_setpoint = _heading_smoothing.getSmoothedHeadingRate();
}
// publish information to UI
sendTelemetry();
@@ -362,7 +369,7 @@ void FlightTaskOrbit::_generate_circle_yaw_setpoints()
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_UNCONTROLLED:
// no yaw setpoint
_yaw_setpoint = NAN;
_yawspeed_setpoint = NAN;
_yawspeed_setpoint = 0.f; // No yaw setpoint is invalid -> just brake to 0°/s
break;
case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE: