Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 22:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: add Internal_Error for quaternion of 0 length

Browse Source
This commit is contained in:
Josh Henderson 2021-07-08 23:59:55 -04:00 committed by Peter Barker
parent 69997626d3
commit 9b74452270
1 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_Math/quaternion.cpp
View File

@ -639,6 +639,9 @@ void QuaternionT<T>::normalize(void)
q2 *= quatMagInv; q2 *= quatMagInv;
q3 *= quatMagInv; q3 *= quatMagInv;
q4 *= quatMagInv; q4 *= quatMagInv;
} else {
// The code goes here if the quaternion is [0,0,0,0]. This shouldn't happen.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
} }
} }
@ -722,6 +725,12 @@ QuaternionT<T> QuaternionT<T>::operator/(const QuaternionT<T> &v) const
const T &quat2 = q3; const T &quat2 = q3;
const T &quat3 = q4; const T &quat3 = q4;
const T quatMag = length();
if (is_zero(quatMag)) {
// The code goes here if the quaternion is [0,0,0,0]. This shouldn't happen.
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
const T rquat0 = v.q1; const T rquat0 = v.q1;
const T rquat1 = v.q2; const T rquat1 = v.q2;
const T rquat2 = v.q3; const T rquat2 = v.q3;