From b5e1397c0f825017ece9e870e906eede75f3b6e1 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Sat, 11 Jul 2020 09:11:05 +0200 Subject: [PATCH] Add const modifier const modifier Add missing const modifier --- EKF/EKFGSF_yaw.cpp | 27 +++++++++++---------------- EKF/control.cpp | 10 +++++----- EKF/ekf.cpp | 2 +- EKF/ekf.h | 4 ++-- EKF/ekf_helper.cpp | 35 +++++++++++++++++------------------ EKF/estimator_interface.cpp | 19 ++++++++++--------- EKF/gps_yaw_fusion.cpp | 2 +- EKF/mag_fusion.cpp | 8 ++++---- EKF/sideslip_fusion.cpp | 2 +- EKF/terrain_estimator.cpp | 2 +- EKF/utils.cpp | 24 ++++++++++++------------ 11 files changed, 65 insertions(+), 70 deletions(-) diff --git a/EKF/EKFGSF_yaw.cpp b/EKF/EKFGSF_yaw.cpp index 3161fbca14..e4681b30ad 100644 --- a/EKF/EKFGSF_yaw.cpp +++ b/EKF/EKFGSF_yaw.cpp @@ -236,10 +236,10 @@ void EKFGSF_yaw::ahrsAlignYaw() } else { // Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame - Vector3f rot312; - rot312(0) = wrap_pi(_ekf_gsf[model_index].X(2)); // first rotation (yaw) taken from EKF model state - rot312(1) = asinf(_ahrs_ekf_gsf[model_index].R(2, 1)); // second rotation (roll) - rot312(2) = atan2f(-_ahrs_ekf_gsf[model_index].R(2, 0), _ahrs_ekf_gsf[model_index].R(2, 2)); // third rotation (pitch) + const Vector3f rot312(wrap_pi(_ekf_gsf[model_index].X(2)), // yaw + asinf(_ahrs_ekf_gsf[model_index].R(2, 1)), // roll + atan2f(-_ahrs_ekf_gsf[model_index].R(2, 0), + _ahrs_ekf_gsf[model_index].R(2, 2))); // pitch // Calculate the body to earth frame rotation matrix _ahrs_ekf_gsf[model_index].R = taitBryan312ToRotMat(rot312); @@ -312,7 +312,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index) const float t15 = P22*t10; const float t16 = P12+t15; - const float min_var = 1e-6f; + constexpr float min_var = 1e-6f; _ekf_gsf[model_index].P(0,0) = fmaxf(P00-P20*t6+dvxVar*t14+dvyVar*t13-t6*t7 , min_var); _ekf_gsf[model_index].P(0,1) = P01+t12-P21*t6+t7*t10-dvyVar*t2*t3; _ekf_gsf[model_index].P(0,2) = t7; @@ -356,17 +356,14 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index) // Update the inverse of the innovation covariance matrix S_inverse updateInnovCovMatInv(model_index, S); - // Perform a chi-square innovation consistency test and calculate a compression scale factor that limits the magnitude of innovations to 5-sigma - float innov_comp_scale_factor = 1.0f; - // test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1] const float test_ratio = _ekf_gsf[model_index].innov * (_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov); + // Perform a chi-square innovation consistency test and calculate a compression scale factor + // that limits the magnitude of innovations to 5-sigma // If the test ratio is greater than 25 (5 Sigma) then reduce the length of the innovation vector to clip it at 5-Sigma // This protects from large measurement spikes - if (test_ratio > 25.0f) { - innov_comp_scale_factor = sqrtf(25.0f / test_ratio); - } + const float innov_comp_scale_factor = test_ratio > 25.f ? sqrtf(25.0f / test_ratio) : 1.f; // calculate Kalman gain K nd covariance matrix P // autocode from https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcK.txt @@ -377,13 +374,11 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index) const float t5 = P00*P11; const float t9 = P01*P10; const float t6 = t2+t3+t4+t5-t9; - float t7; - if (fabsf(t6) > 1e-6f) { - t7 = 1.0f/t6; - } else { - // skip this fusion step + if (fabsf(t6) < 1e-6f) { + // skip the fusion step return false; } + const float t7 = 1.f / t6; const float t8 = P11+velObsVar; const float t10 = P00+velObsVar; diff --git a/EKF/control.cpp b/EKF/control.cpp index e1a637b7f9..04a0f1d597 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -771,10 +771,10 @@ void Ekf::controlHeightSensorTimeouts() checkVerticalAccelerationHealth(); // check if height is continuously failing because of accel errors - bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us); + const bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us); // check if height has been inertial deadreckoning for too long - bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6); + const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6); if (hgt_fusion_timeout || continuous_bad_accel_hgt) { @@ -1079,8 +1079,8 @@ void Ekf::controlHeightFusion() // Compensate for positive static pressure transients (negative vertical position innovations) // caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations. - float deadzone_start = 0.0f; - float deadzone_end = deadzone_start + _params.gnd_effect_deadzone; + const float deadzone_start = 0.0f; + const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone; if (_control_status.flags.gnd_effect) { if (_baro_hgt_innov(2) < -deadzone_start) { @@ -1225,7 +1225,7 @@ void Ekf::controlBetaFusion() // Perform synthetic sideslip fusion when in-air and sideslip fuson had been enabled externally in addition to the following criteria: // Sufficient time has lapsed sice the last fusion - bool beta_fusion_time_triggered = isTimedOut(_time_last_beta_fuse, _params.beta_avg_ft_us); + const bool beta_fusion_time_triggered = isTimedOut(_time_last_beta_fuse, _params.beta_avg_ft_us); if (beta_fusion_time_triggered && _control_status.flags.fuse_beta && _control_status.flags.in_air) { // If starting wind state estimation, reset the wind states and covariances before fusing any data diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 427428c448..f454011561 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -268,7 +268,7 @@ void Ekf::predictState() // calculate a filtered horizontal acceleration with a 1 sec time constant // this are used for manoeuvre detection elsewhere - float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt; + const float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt; _accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy(); // save the previous value of velocity so we can use trapzoidal integration diff --git a/EKF/ekf.h b/EKF/ekf.h index 7b67ca00ff..2d31eb3099 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -658,9 +658,9 @@ private: // update the rotation matrix which transforms EV navigation frame measurements into NED void calcExtVisRotMat(); - Vector3f getVisionVelocityInEkfFrame(); + Vector3f getVisionVelocityInEkfFrame() const; - Vector3f getVisionVelocityVarianceInEkfFrame(); + Vector3f getVisionVelocityVarianceInEkfFrame() const; // limit the diagonal of the covariance matrix // force symmetry when the argument is true diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index d93588f357..b4c2f65905 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -219,7 +219,7 @@ void Ekf::resetHeight() const gpsSample &gps_newest = _gps_buffer.get_newest(); // store the current vertical position and velocity for reference so we can calculate and publish the reset amount - float old_vert_pos = _state.pos(2); + const float old_vert_pos = _state.pos(2); bool vert_pos_reset = false; // reset the vertical position @@ -277,8 +277,8 @@ void Ekf::resetHeight() const extVisionSample &ev_newest = _ext_vision_buffer.get_newest(); // use the most recent data if it's time offset from the fusion time horizon is smaller - int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us; - int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us; + const int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us; + const int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us; vert_pos_reset = true; @@ -1090,9 +1090,9 @@ void Ekf::get_ekf_soln_status(uint16_t *status) soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz; soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel; soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs; - bool gps_vel_innov_bad = (_gps_vel_test_ratio(0) > 1.0f) || (_gps_vel_test_ratio(1) > 1.0f); - bool gps_pos_innov_bad = (_gps_pos_test_ratio(0) > 1.0f); - bool mag_innov_good = (_mag_test_ratio.max() < 1.0f) && (_yaw_test_ratio < 1.0f); + const bool gps_vel_innov_bad = (_gps_vel_test_ratio(0) > 1.0f) || (_gps_vel_test_ratio(1) > 1.0f); + const bool gps_pos_innov_bad = (_gps_pos_test_ratio(0) > 1.0f); + const bool mag_innov_good = (_mag_test_ratio.max() < 1.0f) && (_yaw_test_ratio < 1.0f); soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good; soln_status.flags.accel_error = _bad_vert_accel_detected; *status = soln_status.value; @@ -1127,14 +1127,14 @@ bool Ekf::global_position_is_valid() // return true if we are totally reliant on inertial dead-reckoning for position void Ekf::update_deadreckoning_status() { - bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel) - && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) + const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel) + && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max) || isRecent(_time_last_delpos_fuse, _params.no_aid_timeout_max)); - bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max); - bool airDataAiding = _control_status.flags.wind && - isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) && - isRecent(_time_last_beta_fuse, _params.no_aid_timeout_max); + const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max); + const bool airDataAiding = _control_status.flags.wind && + isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) && + isRecent(_time_last_beta_fuse, _params.no_aid_timeout_max); _is_wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding; _is_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding; @@ -1398,17 +1398,16 @@ void Ekf::updateBaroHgtOffset() // calculate a filtered offset between the baro origin and local NED origin if we are not // using the baro as a height reference if (!_control_status.flags.baro_hgt && _baro_data_ready) { - float local_time_step = 1e-6f * _delta_time_baro_us; - local_time_step = math::constrain(local_time_step, 0.0f, 1.0f); + const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f); // apply a 10 second first order low pass filter to baro offset - float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt + _state.pos( - 2) - _baro_hgt_offset); + const float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt + _state.pos(2) - + _baro_hgt_offset); _baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f); } } -Vector3f Ekf::getVisionVelocityInEkfFrame() +Vector3f Ekf::getVisionVelocityInEkfFrame() const { Vector3f vel; // correct velocity for offset relative to IMU @@ -1433,7 +1432,7 @@ Vector3f Ekf::getVisionVelocityInEkfFrame() return vel; } -Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() +Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const { Matrix3f ev_vel_cov = _ev_sample_delayed.velCov; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index cedb0a2402..5203b7f7ba 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -186,7 +186,7 @@ void EstimatorInterface::setGpsData(const gps_message &gps) } // limit data rate to prevent data being lost - bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS); + const bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS); // TODO: remove checks that are not timing related if (((gps.time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps.fix_type > 2) { @@ -357,7 +357,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample& flow) // of min arrival interval because too much data is being lost float delta_time = flow.dt; // in seconds const float delta_time_min = 0.5e-6f * (float)_min_obs_interval_us; - bool delta_time_good = delta_time >= delta_time_min; + const bool delta_time_good = delta_time >= delta_time_min; bool flow_magnitude_good = true; @@ -510,14 +510,15 @@ void EstimatorInterface::setDragData() bool EstimatorInterface::initialise_interface(uint64_t timestamp) { // find the maximum time delay the buffers are required to handle - uint16_t max_time_delay_ms = math::max(_params.mag_delay_ms, - math::max(_params.range_delay_ms, - math::max(_params.gps_delay_ms, + const uint16_t max_time_delay_ms = math::max(_params.mag_delay_ms, + math::max(_params.range_delay_ms, + math::max(_params.gps_delay_ms, math::max(_params.flow_delay_ms, - math::max(_params.ev_delay_ms, - math::max(_params.auxvel_delay_ms, - math::max(_params.min_delay_ms, - math::max(_params.airspeed_delay_ms, _params.baro_delay_ms)))))))); + math::max(_params.ev_delay_ms, + math::max(_params.auxvel_delay_ms, + math::max(_params.min_delay_ms, + math::max(_params.airspeed_delay_ms, + _params.baro_delay_ms)))))))); // calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter _imu_buffer_length = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1; diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 186b93f728..42abd0d40c 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -202,7 +202,7 @@ void Ekf::fuseGpsYaw() } else { // constrain the innovation to the maximum set by the gate - float gate_limit = sqrtf((sq(innov_gate) * _heading_innov_var)); + const float gate_limit = sqrtf((sq(innov_gate) * _heading_innov_var)); _heading_innov = math::constrain(_heading_innov, -gate_limit, gate_limit); } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 9ca549ca49..69e4c90ba2 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -414,8 +414,8 @@ void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation) const float q2 = _state.quat_nominal(2); const float q3 = _state.quat_nominal(3); - float R_YAW = fmaxf(yaw_variance, 1.0e-4f); - float measurement = wrap_pi(yaw); + const float R_YAW = fmaxf(yaw_variance, 1.0e-4f); + const float measurement = wrap_pi(yaw); float H_YAW[4]; // calculate observation jacobian when we are observing the first rotation in a 321 sequence @@ -509,8 +509,8 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation) const float q2 = _state.quat_nominal(2); const float q3 = _state.quat_nominal(3); - float R_YAW = fmaxf(yaw_variance, 1.0e-4f); - float measurement = wrap_pi(yaw); + const float R_YAW = fmaxf(yaw_variance, 1.0e-4f); + const float measurement = wrap_pi(yaw); float H_YAW[4]; // calculate observation jacobian when we are observing a rotation in a 312 sequence diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index 1d133f1d3e..649e0060fd 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -46,7 +46,7 @@ void Ekf::fuseSideslip() { - float R_BETA = _params.beta_noise; + const float R_BETA = _params.beta_noise; // get latest estimated orientation const float q0 = _state.quat_nominal(0); diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 3a54fa6a04..41ef99cd2b 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -165,7 +165,7 @@ void Ekf::fuseHagl() if (_hagl_test_ratio <= 1.0f) { // calculate the Kalman gain - float gain = _terrain_var / _hagl_innov_var; + const float gain = _terrain_var / _hagl_innov_var; // correct the state _terrain_vpos -= gain * _hagl_innov; // correct the variance diff --git a/EKF/utils.cpp b/EKF/utils.cpp index 206f24f430..4f1791dc9c 100644 --- a/EKF/utils.cpp +++ b/EKF/utils.cpp @@ -26,24 +26,24 @@ matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312) float kahanSummation(float sum_previous, float input, float &accumulator) { - float y = input - accumulator; - float t = sum_previous + y; + const float y = input - accumulator; + const float t = sum_previous + y; accumulator = (t - sum_previous) - y; return t; } matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat) { - float q00 = quat(0) * quat(0); - float q11 = quat(1) * quat(1); - float q22 = quat(2) * quat(2); - float q33 = quat(3) * quat(3); - float q01 = quat(0) * quat(1); - float q02 = quat(0) * quat(2); - float q03 = quat(0) * quat(3); - float q12 = quat(1) * quat(2); - float q13 = quat(1) * quat(3); - float q23 = quat(2) * quat(3); + const float q00 = quat(0) * quat(0); + const float q11 = quat(1) * quat(1); + const float q22 = quat(2) * quat(2); + const float q33 = quat(3) * quat(3); + const float q01 = quat(0) * quat(1); + const float q02 = quat(0) * quat(2); + const float q03 = quat(0) * quat(3); + const float q12 = quat(1) * quat(2); + const float q13 = quat(1) * quat(3); + const float q23 = quat(2) * quat(3); matrix::Dcmf dcm; dcm(0, 0) = q00 + q11 - q22 - q33;