From 463d5512d92cf30b2bad05c5576a0c7c1fb2aff2 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Sun, 10 Mar 2019 23:00:05 +0100
Subject: [PATCH] mc_att_control: only adapt yaw rate limit on control mode
change
Put adaption into a method because it needs to be called when
the control mode or the parameter changes.
---
src/modules/mc_att_control/mc_att_control.hpp | 3 +++
.../mc_att_control/mc_att_control_main.cpp | 19 +++++++++++--------
2 files changed, 14 insertions(+), 8 deletions(-)
diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp
index 4a57da9ea8..41e041dfef 100644
--- a/src/modules/mc_att_control/mc_att_control.hpp
+++ b/src/modules/mc_att_control/mc_att_control.hpp
@@ -147,6 +147,9 @@ private:
*/
matrix::Vector3f pid_attenuations(float tpa_breakpoint, float tpa_rate);
+ /** lower yawspeed limit in auto modes because we expect yaw steps */
+ void adapt_auto_yaw_rate_limit();
+
AttitudeControl _attitude_control; /**< class for attitude control calculations */
int _v_att_sub{-1}; /**< vehicle attitude subscription */
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 43db579d88..fae4d87964 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -149,6 +149,7 @@ MulticopterAttitudeControl::parameters_updated()
// angular rate limits
using math::radians;
_attitude_control.setRateLimit(Vector3f(radians(_roll_rate_max.get()), radians(_pitch_rate_max.get()), radians(_yaw_rate_max.get())));
+ adapt_auto_yaw_rate_limit();
// manual rate control acro mode rate limits
_acro_rate_max = Vector3f(radians(_acro_roll_max.get()), radians(_acro_pitch_max.get()), radians(_acro_yaw_max.get()));
@@ -194,6 +195,16 @@ MulticopterAttitudeControl::vehicle_control_mode_poll()
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
+ adapt_auto_yaw_rate_limit();
+ }
+}
+
+void MulticopterAttitudeControl::adapt_auto_yaw_rate_limit() {
+ if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
+ !_v_control_mode.flag_control_manual_enabled) {
+ _attitude_control.setRateLimitYaw(math::radians(_yaw_auto_max.get()));
+ } else {
+ _attitude_control.setRateLimitYaw(math::radians(_yaw_rate_max.get()));
}
}
@@ -518,14 +529,6 @@ MulticopterAttitudeControl::control_attitude()
// physical thrust axis is the negative of body z axis
_thrust_sp = -_v_att_sp.thrust_body[2];
- // lower yawspeed limit in auto modes because we expect yaw steps
- if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
- !_v_control_mode.flag_control_manual_enabled) {
- _attitude_control.setRateLimitYaw(math::radians(_yaw_auto_max.get()));
- } else {
- _attitude_control.setRateLimitYaw(math::radians(_yaw_rate_max.get()));
- }
-
_rates_sp = _attitude_control.update(Quatf(_v_att.q), Quatf(_v_att_sp.q_d), _v_att_sp.yaw_sp_move_rate);
}
--
GitLab