From adf1fea554cf7c5f2231c0cd0400b920220ab286 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 5 Nov 2018 10:43:32 +0100
Subject: [PATCH] mc_att_control: fix for Rattitude mode
When switching back from rate to attitude control, the code depended on a
vehicle_control_mode topics update, but the publication frequency of that
is low. So the switch was noticeably delayed.
---
src/modules/mc_att_control/mc_att_control_main.cpp | 9 ++++-----
1 file changed, 4 insertions(+), 5 deletions(-)
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 dd8a54555c..c4858a5db3 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -876,13 +876,12 @@ MulticopterAttitudeControl::run()
attitude_dt += dt;
/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
- * or roll (yaw can rotate 360 in normal att control). If both are true don't
+ * or roll (yaw can rotate 360 in normal att control). If both are true don't
* even bother running the attitude controllers */
if (_v_control_mode.flag_control_rattitude_enabled) {
- if (fabsf(_manual_control_sp.y) > _rattitude_thres.get() ||
- fabsf(_manual_control_sp.x) > _rattitude_thres.get()) {
- _v_control_mode.flag_control_attitude_enabled = false;
- }
+ _v_control_mode.flag_control_attitude_enabled =
+ fabsf(_manual_control_sp.y) <= _rattitude_thres.get() &&
+ fabsf(_manual_control_sp.x) <= _rattitude_thres.get();
}
bool attitude_setpoint_generated = false;
--
GitLab