yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

vtol_att_control: occasional tailsitter forward transition failure issue solved

Browse Source 
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.
This commit is contained in:
Nidhish Raj
2021-04-02 16:50:52 +02:00
committed by Silvan Fuhrer
parent b30f3917d3
commit 01d8c3da3d
3 changed files with 8 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/vtol_att_control/tailsitter.cpp
View File

@@ -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) {

2
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@@ -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);
}

2
src/modules/vtol_att_control/vtol_type.h
View File

@@ -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: