Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 14:24:21 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

tailsitter.cpp:ensure input quaternions are exactly normalized

Browse Source
This commit is contained in:
xdwgood
2020-07-20 13:32:01 +08:00
committed by Silvan Fuhrer
parent 730af406a6
commit 7ecccf01c2
1 changed files with 6 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/tailsitter.cpp
+6 -3
View File
@@ -180,7 +180,7 @@ void Tailsitter::update_vtol_state()
void Tailsitter::update_transition_state()
{
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
if (!_flag_was_in_trans_mode) {
_flag_was_in_trans_mode = true;
@@ -212,9 +212,12 @@ void Tailsitter::update_transition_state()
_q_trans_sp = _q_trans_start;
}
// ensure input quaternions are exactly normalized because acosf(1.00001) == NaN
_q_trans_sp.normalize();
// tilt angle (zero if vehicle nose points up (hover))
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));
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));
if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {