Add const modifier

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;
+			const Vector3f rotVec312(0.0f,  // yaw
-			rotVec312(0) = 0.0f;  // first rotation (yaw)
+						 asinf(_R_to_earth(2, 1)),  // roll
-			rotVec312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
+						 atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)));  // pitch
-			rotVec312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2));  // third rotation (pitch)
 			R_to_earth = taitBryan312ToRotMat(rotVec312);
 
 		}

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;
+			const Vector3f rotVec312(0.0f,  // yaw
-			rotVec312(0) = 0.0f; // first rotation (yaw) set to zero for alter use when rotating the mag field into earth frame
+						 asinf(_R_to_earth(2, 1)),  // roll
-			rotVec312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
+						 atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)));  // pitch
-			rotVec312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
 			const Dcmf R_to_earth = taitBryan312ToRotMat(rotVec312);
 
 			// rotate the magnetometer measurements into earth frame using a zero yaw angle