From 12ee75700abe407d492a4b2f68fd4c842f882401 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 23 Apr 2020 11:53:49 +0200 Subject: [PATCH] mc_att_control: adapt setpoint on estimation reset There was a gap where the attitude controller already used the estimate with a new reference but the last known attitude setpoint was still based on the old reference. This leads to a big glitch on reset because until the attitude setpoint gets updated the error is wrong and as large as the attitude delta of the reset. --- .../mc_att_control/AttitudeControl/AttitudeControl.hpp | 7 +++++++ src/modules/mc_att_control/mc_att_control_main.cpp | 4 +++- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp index 8883708b84..25f809164e 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp @@ -77,6 +77,13 @@ public: */ void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint) { _attitude_setpoint_q = qd; _yawspeed_setpoint = yawspeed_setpoint; } + /** + * Adjust last known attitude setpoint by a delta rotation + * Optional use to avoid glitches when attitude estimate reference e.g. heading changes. + * @param q_delta delta rotation to apply + */ + void adaptAttitudeSetpoint(const matrix::Quatf &q_delta) { _attitude_setpoint_q = q_delta * _attitude_setpoint_q; } + /** * Run one control loop cycle calculation * @param q estimation of the current vehicle attitude unit quaternion 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 6173e1a908..e5a703c13c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -286,7 +286,9 @@ MulticopterAttitudeControl::Run() // Check for a heading reset if (prev_quat_reset_counter != _v_att.quat_reset_counter) { // we only extract the heading change from the delta quaternion - _man_yaw_sp += Eulerf(Quatf(_v_att.delta_q_reset)).psi(); + const Quatf delta_q_reset(_v_att.delta_q_reset); + _man_yaw_sp += Eulerf(delta_q_reset).psi(); + _attitude_control.adaptAttitudeSetpoint(delta_q_reset); } const hrt_abstime now = hrt_absolute_time();