dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FlightTaskAuto: revisit yaw rate limit 

There were multiple comments not addressed in pr #11904.
See commit 4bcb37f9bc9ef20b521ebcf33e7a0ed08fa86ccd
This commit is contained in:
Matthias Grob 2019-04-26 15:43:09 +02:00 committed by Lorenz Meier
parent c3ac1497c0
commit 21194239c7
1 changed files with 10 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
View File

@ -104,37 +104,27 @@ bool FlightTaskAuto::updateFinalize()
{
// 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 will see its setpoint constrained here
// it will see its setpoint constrained here
_limitYawRate();
return true;
}
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
float dy_max = math::radians(_param_mpc_yawrauto_max.get()) * _deltatime;
if (fabsf(_yaw_setpoint - _yaw_sp_prev) < M_PI_F) {
// Wrap around 0
_yaw_setpoint = math::constrain(_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)
);
}
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);
_yaw_setpoint = _yaw_sp_prev + dyaw;
_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint);
_yaw_sp_prev = _yaw_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);
}
}