dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

shouldUse321RotationSequence(const Dcmf& R)

This commit is contained in:
Kamil Ritz 2020-08-15 13:43:55 +02:00 committed by Paul Riseborough
parent 4872f2a545
commit 15afa8ae17
5 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/EKFGSF_yaw.cpp
View File

@ -224,7 +224,7 @@ void EKFGSF_yaw::ahrsAlignYaw()
{
// Align yaw angle for each model
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
if (fabsf(_ahrs_ekf_gsf[model_index].R(2, 0)) < fabsf(_ahrs_ekf_gsf[model_index].R(2, 1))) {
if (shouldUse321RotationSequence(_ahrs_ekf_gsf[model_index].R)) {
// get the roll, pitch, yaw estimates from the rotation matrix using a 321 Tait-Bryan rotation sequence
Eulerf euler_init(_ahrs_ekf_gsf[model_index].R);
@ -260,7 +260,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
}
// Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock
if (fabsf(_ahrs_ekf_gsf[model_index].R(2, 0)) < fabsf(_ahrs_ekf_gsf[model_index].R(2, 1))) {
if (shouldUse321RotationSequence(_ahrs_ekf_gsf[model_index].R)) {
// use 321 Tait-Bryan rotation to define yaw state
_ekf_gsf[model_index].X(2) = atan2f(_ahrs_ekf_gsf[model_index].R(1, 0), _ahrs_ekf_gsf[model_index].R(0, 0));
} else {

4
EKF/ekf_helper.cpp
View File

@ -486,7 +486,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Dcmf R_to_earth;
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
if (shouldUse321RotationSequence(_R_to_earth)) {
// rolled more than pitched so use 321 rotation order
Eulerf euler321(_state.quat_nominal);
euler321(2) = 0.0f;
@ -1658,7 +1658,7 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
// update the rotation matrix using the new yaw value
// determine if a 321 or 312 Euler sequence is best
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
if (shouldUse321RotationSequence(_R_to_earth)) {
// use a 321 sequence
Eulerf euler321(_R_to_earth);
euler321(2) = yaw;

2
EKF/mag_fusion.cpp
View File

@ -723,7 +723,7 @@ void Ekf::fuseHeading()
_R_to_earth = Dcmf(_state.quat_nominal);
// determine if a 321 or 312 Euler sequence is best
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
if (shouldUse321RotationSequence(_R_to_earth)) {
// rolled more than pitched so use 321 rotation order to calculate the observed yaw angle
Eulerf euler321(_state.quat_nominal);
predicted_hdg = euler321(2);

4
EKF/utils.cpp
View File

@ -58,3 +58,7 @@ matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
return dcm;
}
bool shouldUse321RotationSequence(const matrix::Dcmf& R) {
return fabsf(R(2, 0)) < fabsf(R(2, 1));
}

1
EKF/utils.hpp
View File

@ -20,6 +20,7 @@ float kahanSummation(float sum_previous, float input, float &accumulator);
// this produces the inverse rotation to that produced by the math library quaternion to Dcmf operator
matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat);
bool shouldUse321RotationSequence(const matrix::Dcmf& R);
namespace ecl{
inline float powf(float x, int exp)
{