From 21194239c7f4fb460f3cb8a03a4ce5b3da08cc6a Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Fri, 26 Apr 2019 15:43:09 +0200
Subject: [PATCH] FlightTaskAuto: revisit yaw rate limit
There were multiple comments not addressed in pr #11904.
See commit 4bcb37f9bc9ef20b521ebcf33e7a0ed08fa86ccd
---
.../FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 30 +++++++------------
1 file changed, 10 insertions(+), 20 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
index 13277008c6..8e08a51990 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
@@ -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)) {
- // 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 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
+ 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);
}
}
--
GitLab