From 1c687099495d6a49cccaf162f7eaf32317114a56 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Wed, 25 Dec 2019 17:50:34 +0100 Subject: [PATCH] Rename mag ambiguous mag abbreviations with magnitude --- EKF/control.cpp | 4 ++-- EKF/covariance.cpp | 8 ++++---- EKF/ekf.cpp | 4 ++-- EKF/ekf.h | 10 +++++----- EKF/mag_control.cpp | 4 ++-- 5 files changed, 15 insertions(+), 15 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 9d4deb0ba5..0953e05e96 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -415,7 +415,7 @@ void Ekf::controlOpticalFlowFusion() const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit || (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit - || (_ang_rate_mag_filt > _flow_max_rate) // angular rate exceeds flow sensor limit + || (_ang_rate_magnitude_filt > _flow_max_rate) // angular rate exceeds flow sensor limit || (_R_to_earth(2,2) < cosf(math::radians(30.0f)))); // tilted excessively if (motion_is_excessive) { @@ -1136,7 +1136,7 @@ void Ekf::controlHeightFusion() _hgt_sensor_offset = 0.0f; } } - // TODO: Add EV normal case here + // determine if we should use the vertical position observation if (_control_status.flags.ev_hgt) { _fuse_height = true; diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 8645991b04..fbb2e65086 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -161,12 +161,12 @@ void Ekf::predictCovariance() // inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f); float beta = 1.0f - alpha; - _ang_rate_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_mag_filt); - _accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_mag_filt); + _ang_rate_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt); + _accel_magnitude_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_magnitude_filt); _accel_vec_filt = alpha * dt_inv * _imu_sample_delayed.delta_vel + beta * _accel_vec_filt; - if (_ang_rate_mag_filt > _params.acc_bias_learn_gyr_lim - || _accel_mag_filt > _params.acc_bias_learn_acc_lim + if (_ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim + || _accel_magnitude_filt > _params.acc_bias_learn_acc_lim || _bad_vert_accel_detected) { // store the bias state variances to be reinstated later diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 8628bd77f5..89b029d8ab 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -95,8 +95,8 @@ void Ekf::reset(uint64_t timestamp) _fault_status.value = 0; _innov_check_fail_status.value = 0; - _accel_mag_filt = 0.0f; - _ang_rate_mag_filt = 0.0f; + _accel_magnitude_filt = 0.0f; + _ang_rate_magnitude_filt = 0.0f; _prev_dvel_bias_var.zero(); } diff --git a/EKF/ekf.h b/EKF/ekf.h index 4505e405be..f4a58356aa 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -336,11 +336,9 @@ private: bool _tas_data_ready{false}; ///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator - uint64_t _time_last_fake_pos{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec) uint64_t _time_ins_deadreckon_start{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) bool _using_synthetic_position{false}; ///< true if we are using a synthetic position to constrain drift - // TODO: Split those into sensor and measurement specifics uint64_t _time_last_hor_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec) uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of vertical position measurements was performed (uSec) uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec) @@ -350,6 +348,9 @@ private: uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec) uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec) uint64_t _time_last_rng_ready{0}; ///< time the last range finder measurement was ready (uSec) + uint64_t _time_last_mag{0}; ///< measurement time of last magnetomter sample (uSec) + uint64_t _time_last_fake_pos{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec) + Vector2f _last_known_posNE; ///< last known local NE position vector (m) float _imu_collection_time_adj{0.0f}; ///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec) @@ -461,7 +462,6 @@ private: bool _is_first_imu_sample{true}; uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation - uint64_t _time_last_mag{0}; ///< measurement time of last magnetomter sample (uSec) AlphaFilterVector3f _mag_lpf; ///< filtered magnetometer measurement for instant reset(Gauss) AlphaFilterVector3f _accel_lpf; ///< filtered accelerometer measurement for instant reset(Gauss) float _hgt_sensor_offset{0.0f}; ///< set as necessary if desired to maintain the same height after a height reset (m) @@ -481,8 +481,8 @@ private: // variables used to inhibit accel bias learning bool _accel_bias_inhibit{false}; ///< true when the accel bias learning is being inhibited Vector3f _accel_vec_filt; ///< acceleration vector after application of a low pass filter (m/sec**2) - float _accel_mag_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec) - float _ang_rate_mag_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec) + float _accel_magnitude_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec) + float _ang_rate_magnitude_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec) Vector3f _prev_dvel_bias_var; ///< saved delta velocity XYZ bias variances (m/sec)**2 // Terrain height state estimation diff --git a/EKF/mag_control.cpp b/EKF/mag_control.cpp index fe4f563edb..5963fcd15f 100644 --- a/EKF/mag_control.cpp +++ b/EKF/mag_control.cpp @@ -165,7 +165,7 @@ void Ekf::runInAirYawReset() } bool Ekf::canRealignYawUsingGps() const -{ +{ return _control_status.flags.fixed_wing; } @@ -256,7 +256,7 @@ void Ekf::checkMagDeclRequired() void Ekf::checkMagInhibition() { _mag_use_inhibit = shouldInhibitMag(); - if (!_mag_use_inhibit) { + if (!_mag_use_inhibit) { _mag_use_not_inhibit_us = _imu_sample_delayed.time_us; }