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

FlightTaskAutoMapper: add horizontal and yaw nudging during final descent

Browse Source
This commit is contained in:
Matthias Grob
2021-10-05 18:05:54 -04:00
committed by Daniel Agar
parent bb9f167d79
commit 0cc13f0799
3 changed files with 53 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+3 -2
View File
@@ -53,7 +53,7 @@ bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setp
bool ret = FlightTask::activate(last_setpoint);
_position_setpoint = _position;
_velocity_setpoint = _velocity;
_yaw_setpoint = _yaw_sp_prev = _yaw;
_yaw_setpoint = _yaw;
_yawspeed_setpoint = 0.0f;
_setDefaultConstraints();
return ret;
@@ -107,7 +107,6 @@ void FlightTaskAuto::_limitYawRate()
_yaw_sp_aligned = fabsf(matrix::wrap_pi(_yaw_setpoint - yaw_setpoint_sat)) < math::radians(_param_mis_yaw_err.get());
_yaw_setpoint = yaw_setpoint_sat;
_yaw_sp_prev = _yaw_setpoint;
if (!PX4_ISFINITE(_yawspeed_setpoint) && (_deltatime > FLT_EPSILON)) {
// Create a feedforward
@@ -115,6 +114,8 @@ void FlightTaskAuto::_limitYawRate()
}
}
_yaw_sp_prev = _yaw_setpoint;
if (PX4_ISFINITE(_yawspeed_setpoint)) {
// The yaw setpoint is aligned when its rate is not saturated
_yaw_sp_aligned = _yaw_sp_aligned && (fabsf(_yawspeed_setpoint) < yawrate_max);