From 3d82d822aec3ce0a00f5b2ab01fd0a5f88a330d2 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Fri, 3 Jul 2020 16:27:56 +0200 Subject: [PATCH] Add const modifier --- EKF/control.cpp | 4 ++-- EKF/ekf.cpp | 3 +-- EKF/ekf_helper.cpp | 9 ++++----- EKF/gps_checks.cpp | 10 +++++----- EKF/mag_fusion.cpp | 8 ++++---- 5 files changed, 16 insertions(+), 18 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 9b152f72c7..021e1e472b 100644 --- a/EKF/control.cpp +++ b/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()); } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 2398a2de73..e58502f814 100644 --- a/EKF/ekf.cpp +++ b/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)); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 84ffa64b41..6ab5373e66 100644 --- a/EKF/ekf_helper.cpp +++ b/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); } diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 3113dd84f5..4a9c16ec57 100644 --- a/EKF/gps_checks.cpp +++ b/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); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index f1f13fbaf1..32224d7060 100644 --- a/EKF/mag_fusion.cpp +++ b/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));