Add const modifier

EKF/control.cpp

@@ -132,8 +132,8 @@ void Ekf::controlFusionModes()
 
 	if (_range_sensor.isDataHealthy()) {
 		// correct the range data for position offset relative to the IMU
-		Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
-		Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
+		const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
+		const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 		_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
 	}
 

EKF/ekf.cpp

@@ -236,8 +236,7 @@ bool Ekf::initialiseTilt()
 	}
 
 	// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
-	Vector3f gravity_in_body = _accel_lpf.getState();
-	gravity_in_body.normalize();
+	const Vector3f gravity_in_body = _accel_lpf.getState().normalized();
 	const float pitch = asinf(gravity_in_body(0));
 	const float roll = atan2f(-gravity_in_body(1), -gravity_in_body(2));
 

EKF/ekf_helper.cpp

@@ -411,7 +411,7 @@ bool Ekf::realignYawGPS()
 		if (!_control_status.flags.mag_aligned_in_flight) {
 			// This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a
 			// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
-			Eulerf euler321(_state.quat_nominal);
+			const Eulerf euler321(_state.quat_nominal);
 			yaw_new = euler321(2) + courseYawError;
 			_control_status.flags.mag_aligned_in_flight = true;
 
@@ -1639,10 +1639,9 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
 		// Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame
 		// We use a 312 sequence as an alternate when there is more pitch tilt than roll tilt
 		// to avoid gimbal lock
-		Vector3f rot312;
-		rot312(0) = yaw;
-		rot312(1) = asinf(_R_to_earth(2, 1));
-		rot312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2));
+		const Vector3f rot312(yaw,
+				      asinf(_R_to_earth(2, 1)),
+				      atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)));
 		_R_to_earth = taitBryan312ToRotMat(rot312);
 
 	}

EKF/gps_checks.cpp

@@ -138,7 +138,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
 	_gps_error_norm = fmaxf(_gps_error_norm , (gps.sacc / _params.req_sacc));
 
 	// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
-	const float filt_time_const = 10.0f;
+	constexpr float filt_time_const = 10.0f;
 	const float dt = fminf(fmaxf(float(int64_t(_time_last_imu) - int64_t(_gps_pos_prev.timestamp)) * 1e-6f, 0.001f), filt_time_const);
 	const float filter_coef = dt / filt_time_const;
 
@@ -177,9 +177,9 @@ bool Ekf::gps_is_good(const gps_message &gps)
 		_gps_check_fail_status.flags.vdrift = (_gps_drift_metrics[1] > _params.req_vdrift);
 
 		// Check the magnitude of the filtered horizontal GPS velocity
-		Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel_ned.xy()),
-							-10.0f * _params.req_hdrift,
-							 10.0f * _params.req_hdrift);
+		const Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel_ned.xy()),
+							     -10.0f * _params.req_hdrift,
+							      10.0f * _params.req_hdrift);
 		_gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef);
 		_gps_drift_metrics[2] = _gps_velNE_filt.norm();
 		_gps_check_fail_status.flags.hspeed = (_gps_drift_metrics[2] > _params.req_hdrift);
@@ -208,7 +208,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
 	_gps_alt_prev = 1e-3f * (float)gps.alt;
 
 	// Check  the filtered difference between GPS and EKF vertical velocity
-	float vz_diff_limit = 10.0f * _params.req_vdrift;
+	const float vz_diff_limit = 10.0f * _params.req_vdrift;
 	float vertVel = fminf(fmaxf((gps.vel_ned(2) - _state.vel(2)), -vz_diff_limit), vz_diff_limit);
 	_gps_velD_diff_filt = vertVel * filter_coef + _gps_velD_diff_filt * (1.0f - filter_coef);
 	_gps_check_fail_status.flags.vspeed = (fabsf(_gps_velD_diff_filt) > _params.req_vdrift);

EKF/mag_fusion.cpp

@@ -792,7 +792,7 @@ void Ekf::fuseHeading()
 				// unconstrained quaternion variance growth and record the predicted heading
 				// to use as an observation when movement ceases.
 				// TODO a better way of determining when this is necessary
-				float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3);
+				const float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3);
 				if (sumQuatVar > _params.quat_max_variance) {
 					fuse_zero_innov = true;
 					R_YAW = 0.25f;
@@ -859,7 +859,7 @@ void Ekf::fuseHeading()
 				// unconstrained quaterniion variance growth and record the predicted heading
 				// to use as an observation when movement ceases.
 				// TODO a better way of determining when this is necessary
-				float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3);
+				const float sumQuatVar = P(0,0) + P(1,1) + P(2,2) + P(3,3);
 				if (sumQuatVar > _params.quat_max_variance) {
 					fuse_zero_innov = true;
 					R_YAW = 0.25f;
@@ -891,7 +891,7 @@ void Ekf::fuseDeclination(float decl_sigma)
 	const float magE = _state.mag_I(1);
 
 	// minimum horizontal field strength before calculation becomes badly conditioned (T)
-	const float h_field_min = 0.001f;
+	constexpr float h_field_min = 0.001f;
 
 	// observation variance (rad**2)
 	const float R_DECL = sq(decl_sigma);
@@ -1050,7 +1050,7 @@ void Ekf::limitDeclination()
 	}
 
 	// do not allow the declination estimate to vary too much relative to the reference value
-	const float decl_tolerance = 0.5f;
+	constexpr float decl_tolerance = 0.5f;
 	const float decl_max = decl_reference + decl_tolerance;
 	const float decl_min = decl_reference - decl_tolerance;
 	const float decl_estimate = atan2f(_state.mag_I(1) , _state.mag_I(0));