From 01d8c3da3d4021ce9b6d9661dfa8910f17fcecf1 Mon Sep 17 00:00:00 2001 From: Nidhish Raj Date: Fri, 2 Apr 2021 16:50:52 +0200 Subject: [PATCH] vtol_att_control: occasional tailsitter forward transition failure issue solved Tailsitter VTOLs very occasionally gets stuck with zero roll and pitch angle in multicopter mode after a forward transition command is issued. This very rare behavior is triggered by the following events: 1> a forward transition is triggered either in auto or manual mode. 2> in the vtol_att_control main loop, if multicopter and fixed wing attitude setpoints are not updated, transition state is not updated 3> the commander changes the vehicle status to transition mode. 4> multicopter pos controller initiated Transition flight task. This results in zero roll and pitch setpoint due to zero acceleration setpoint 5> now vtol_att_control executes and updates the transition state. Specifically, _q_trans_start and _q_trans_sp are set with zero roll and pitch sp 6> tilt is evaluated to be NaN, despite _q_trans_sp being normalized. This happens for 25% of all yaw angles when using float datatype. This can be verified using the matrix library 7> once tilt is evaluated to be NaN, _q_trans_sp is never updated again and is stuck in this state for ever. This has been fixed by constraining the cos(tilt) within +1 to -1 range Further, _q_trans_start and _q_trans_sp are immedietly initialized after a transition event is triggered. --- src/modules/vtol_att_control/tailsitter.cpp | 7 +++++-- src/modules/vtol_att_control/vtol_att_control_main.cpp | 2 +- src/modules/vtol_att_control/vtol_type.h | 2 ++ 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index ba4b373182..2a7cda3f57 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -225,8 +225,11 @@ void Tailsitter::update_transition_state() _q_trans_sp.normalize(); // tilt angle (zero if vehicle nose points up (hover)) - const float tilt = acosf(_q_trans_sp(0) * _q_trans_sp(0) - _q_trans_sp(1) * _q_trans_sp(1) - _q_trans_sp(2) * - _q_trans_sp(2) + _q_trans_sp(3) * _q_trans_sp(3)); + float cos_tilt = _q_trans_sp(0) * _q_trans_sp(0) - _q_trans_sp(1) * _q_trans_sp(1) - _q_trans_sp(2) * + _q_trans_sp(2) + _q_trans_sp(3) * _q_trans_sp(3); + cos_tilt = cos_tilt > 1.0f ? 1.0f : cos_tilt; + cos_tilt = cos_tilt < -1.0f ? -1.0f : cos_tilt; + const float tilt = acosf(cos_tilt); if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index dd4d8cb6e2..0cf205742f 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -424,7 +424,7 @@ VtolAttitudeControl::Run() _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); - if (mc_att_sp_updated || fw_att_sp_updated) { + if (!_vtol_type->was_in_trans_mode() || mc_att_sp_updated || fw_att_sp_updated) { _vtol_type->update_transition_state(); _v_att_sp_pub.publish(_v_att_sp); } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 68c5a89cb6..0230efc5c1 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -190,6 +190,8 @@ public: mode get_mode() {return _vtol_mode;} + bool was_in_trans_mode() {return _flag_was_in_trans_mode;} + virtual void parameters_update() = 0; protected: