fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 18:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revert if statement swap in resetMagHeading

Browse Source
This commit is contained in:
kamilritz
2019-12-18 10:06:31 +01:00
committed by Mathieu Bresciani
parent a6bbafc7b0
commit 490c749f80
1 changed files with 14 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+14 -14
View File
@@ -592,18 +592,18 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
Dcmf R_to_earth(euler321); Dcmf R_to_earth(euler321);
// calculate the observed yaw angle // calculate the observed yaw angle
if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { if (_control_status.flags.ev_yaw) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * mag_init;
// the angle of the projection onto the horizontal gives the yaw angle
euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
} else if (_control_status.flags.ev_yaw) {
// convert the observed quaternion to a rotation matrix // convert the observed quaternion to a rotation matrix
Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
// calculate the yaw angle for a 312 sequence // calculate the yaw angle for a 312 sequence
euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0)); euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * mag_init;
// the angle of the projection onto the horizontal gives the yaw angle
euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) { } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) {
// we are operating without knowing the earth frame yaw angle // we are operating without knowing the earth frame yaw angle
return true; return true;
@@ -650,18 +650,18 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
R_to_earth(2, 1) = s1; R_to_earth(2, 1) = s1;
// calculate the observed yaw angle // calculate the observed yaw angle
if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { if (_control_status.flags.ev_yaw) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * mag_init;
// the angle of the projection onto the horizontal gives the yaw angle
euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
} else if (_control_status.flags.ev_yaw) {
// convert the observed quaternion to a rotation matrix // convert the observed quaternion to a rotation matrix
Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
// calculate the yaw angle for a 312 sequence // calculate the yaw angle for a 312 sequence
euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1)); euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * mag_init;
// the angle of the projection onto the horizontal gives the yaw angle
euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) { } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) {
// we are operating without knowing the earth frame yaw angle // we are operating without knowing the earth frame yaw angle
return true; return true;