Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

FlightTaskAuto: add missing wrap_pi and do not override yaw_sp_aligned

Browse Source
This commit is contained in:
bresch
2020-06-12 13:55:54 +02:00
committed by Matthias Grob
parent 7a401e5ca3
commit 12449c23c5
1 changed files with 3 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
+3 -4
View File
@@ -102,11 +102,10 @@ void FlightTaskAuto::_limitYawRate()
const float dyaw_desired = matrix::wrap_pi(_yaw_setpoint - _yaw_sp_prev);
const float dyaw_max = yawrate_max * _deltatime;
const float dyaw = math::constrain(dyaw_desired, -dyaw_max, dyaw_max);
float yaw_setpoint_sat = _yaw_sp_prev + dyaw;
yaw_setpoint_sat = matrix::wrap_pi(yaw_setpoint_sat);
const float yaw_setpoint_sat = matrix::wrap_pi(_yaw_sp_prev + dyaw);
// The yaw setpoint is aligned when it is within tolerance
_yaw_sp_aligned = fabsf(_yaw_setpoint - yaw_setpoint_sat) < math::radians(_param_mis_yaw_err.get());
_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;
@@ -121,7 +120,7 @@ void FlightTaskAuto::_limitYawRate()
_yawspeed_setpoint = math::constrain(_yawspeed_setpoint, -yawrate_max, yawrate_max);
// The yaw setpoint is aligned when its rate is not saturated
_yaw_sp_aligned = fabsf(_yawspeed_setpoint) < yawrate_max;
_yaw_sp_aligned = _yaw_sp_aligned && (fabsf(_yawspeed_setpoint) < yawrate_max);
}
}