fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 05:37:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add const modifier

Browse Source
This commit is contained in:
kamilritz
2020-06-21 13:18:39 +02:00
committed by Mathieu Bresciani
parent 61c139ea95
commit 22274b1d30
2 changed files with 6 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+3 -4
View File
@@ -508,10 +508,9 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
} else {
// pitched more than rolled so use 312 rotation order
Vector3f rotVec312;
rotVec312(0) = 0.0f; // first rotation (yaw)
rotVec312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
rotVec312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
const Vector3f rotVec312(0.0f, // yaw
asinf(_R_to_earth(2, 1)), // roll
atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2))); // pitch
R_to_earth = taitBryan312ToRotMat(rotVec312);
}
EKF/mag_fusion.cpp
+3 -4
View File
@@ -863,10 +863,9 @@ void Ekf::fuseHeading()
// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
// with yaw angle set to to zero
Vector3f rotVec312;
rotVec312(0) = 0.0f; // first rotation (yaw) set to zero for alter use when rotating the mag field into earth frame
rotVec312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
rotVec312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
const Vector3f rotVec312(0.0f, // yaw
asinf(_R_to_earth(2, 1)), // roll
atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2))); // pitch
const Dcmf R_to_earth = taitBryan312ToRotMat(rotVec312);
// rotate the magnetometer measurements into earth frame using a zero yaw angle