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

FlightTaskAuto: revisit yaw rate limit

Browse Source 
There were multiple comments not addressed in pr #11904.
See commit 4bcb37f9bc9ef20b521ebcf33e7a0ed08fa86ccd
This commit is contained in:
Matthias Grob
2019-04-26 18:58:50 +02:00
committed by Lorenz Meier
parent c3ac1497c0
commit 21194239c7
1 changed files with 10 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+10 -20
View File
@@ -104,37 +104,27 @@ bool FlightTaskAuto::updateFinalize()
{ {
// All the auto FlightTasks have to comply with defined maximum yaw rate // All the auto FlightTasks have to comply with defined maximum yaw rate
// If the FlightTask generates a yaw or a yawrate setpoint that exceeds this value // If the FlightTask generates a yaw or a yawrate setpoint that exceeds this value
// if will see its setpoint constrained here // it will see its setpoint constrained here
_limitYawRate(); _limitYawRate();
return true; return true;
} }
void FlightTaskAuto::_limitYawRate() void FlightTaskAuto::_limitYawRate()
{ {
if (PX4_ISFINITE(_yaw_setpoint) || PX4_ISFINITE(_yaw_sp_prev)) { const float yawrate_max = math::radians(_param_mpc_yawrauto_max.get());
if (PX4_ISFINITE(_yaw_setpoint) && PX4_ISFINITE(_yaw_sp_prev)) {
// Limit the rate of change of the yaw setpoint // Limit the rate of change of the yaw setpoint
float dy_max = math::radians(_param_mpc_yawrauto_max.get()) * _deltatime; const float dyaw_desired = matrix::wrap_pi(_yaw_setpoint - _yaw_sp_prev);
const float dyaw_max = yawrate_max * _deltatime;
if (fabsf(_yaw_setpoint - _yaw_sp_prev) < M_PI_F) { const float dyaw = math::constrain(dyaw_desired, -dyaw_max, dyaw_max);
// Wrap around 0 _yaw_setpoint = _yaw_sp_prev + dyaw;
_yaw_setpoint = math::constrain(_yaw_setpoint, _yaw_setpoint = matrix::wrap_pi(_yaw_setpoint);
_yaw_sp_prev - dy_max,
_yaw_sp_prev + dy_max);
} else {
// Wrap around PI/-PI
_yaw_setpoint = matrix::wrap_pi(
math::constrain(matrix::wrap_2pi(_yaw_setpoint),
matrix::wrap_2pi(_yaw_sp_prev) - dy_max,
matrix::wrap_2pi(_yaw_sp_prev) + dy_max)
);
}
_yaw_sp_prev = _yaw_setpoint; _yaw_sp_prev = _yaw_setpoint;
} }
if (PX4_ISFINITE(_yawspeed_setpoint)) { if (PX4_ISFINITE(_yawspeed_setpoint)) {
_yawspeed_setpoint = math::constrain(_yawspeed_setpoint, -_param_mpc_yawrauto_max.get(), _param_mpc_yawrauto_max.get()); _yawspeed_setpoint = math::constrain(_yawspeed_setpoint, -yawrate_max, yawrate_max);
} }
} }