From 22274b1d30dd86679dd9ac5999337276145069a8 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Sun, 21 Jun 2020 13:18:39 +0200 Subject: [PATCH] Add const modifier --- EKF/ekf_helper.cpp | 7 +++---- EKF/mag_fusion.cpp | 7 +++---- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index ad49c9f2b1..d3599547c4 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -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); } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 8f1e4b0f66..2cb3210c78 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -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