From 1f637af7de316a07ace0c0ecdd4a43b946f842a6 Mon Sep 17 00:00:00 2001 From: Kamil Ritz Date: Sat, 15 Aug 2020 08:53:21 +0200 Subject: [PATCH] Add const modifier --- EKF/airspeed_fusion.cpp | 2 +- EKF/covariance.cpp | 4 ++-- EKF/mag_fusion.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 2f678c93ad..f1ed00c983 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -173,7 +173,7 @@ Vector2f Ekf::getWindVelocityVariance() const void Ekf::get_true_airspeed(float *tas) { - float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2))); + const float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2))); memcpy(tas, &tempvar, sizeof(float)); } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index bccddfeadc..70867b8595 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -1103,9 +1103,9 @@ void Ekf::resetWindCovariance() Eulerf euler321(_state.quat_nominal); const float euler_yaw = euler321(2); const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); - const float initial_sideslip_uncertainty = math::radians(15.0f); + constexpr float initial_sideslip_uncertainty = math::radians(15.0f); const float initial_wind_var_body_y = sq(_airspeed_sample_delayed.true_airspeed * sinf(initial_sideslip_uncertainty)); - const float R_yaw = sq(math::radians(10.0f)); + constexpr float R_yaw = sq(math::radians(10.0f)); // rotate wind velocity into earth frame aligned with vehicle yaw const float Wx = _state.wind_vel(0) * cosf(euler_yaw) + _state.wind_vel(1) * sinf(euler_yaw); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index bd61c3b57a..9cb34580be 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -812,8 +812,8 @@ void Ekf::fuseHeading() } else if (_control_status.flags.ev_yaw) { // calculate the yaw angle for a 312 sequence // Values from yaw_input_312.c file produced by https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m - float Tbn_0_1_neg = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)-_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2)); - float Tbn_1_1 = sq(_ev_sample_delayed.quat(0))-sq(_ev_sample_delayed.quat(1))+sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3)); + const float Tbn_0_1_neg = 2.0f*(_ev_sample_delayed.quat(0)*_ev_sample_delayed.quat(3)-_ev_sample_delayed.quat(1)*_ev_sample_delayed.quat(2)); + const float Tbn_1_1 = sq(_ev_sample_delayed.quat(0))-sq(_ev_sample_delayed.quat(1))+sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3)); measured_hdg = atan2f(Tbn_0_1_neg,Tbn_1_1); } else {