From 82b6797547e1c8cd4685c0f59d81eb4891fe40a9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 10 Jul 2024 11:09:05 -0400 Subject: [PATCH] ekf2: manual style cleanup after astyle enforcement --- .../aid_sources/airspeed/airspeed_fusion.cpp | 4 +- .../barometer/baro_height_control.cpp | 3 + .../external_vision/ev_control.cpp | 9 +- .../ekf2/EKF/aid_sources/fake_pos_control.cpp | 2 +- .../aid_sources/gnss/gnss_height_control.cpp | 3 + .../ekf2/EKF/aid_sources/gnss/gps_checks.cpp | 4 +- .../ekf2/EKF/aid_sources/gnss/gps_control.cpp | 10 +- .../aid_sources/gravity/gravity_fusion.cpp | 8 +- .../aid_sources/magnetometer/mag_control.cpp | 38 +-- .../aid_sources/magnetometer/mag_fusion.cpp | 12 +- .../optical_flow/optical_flow_control.cpp | 3 +- .../range_finder/range_height_control.cpp | 12 +- .../range_finder/sensor_range_finder.hpp | 4 +- .../zero_innovation_heading_update.cpp | 6 +- src/modules/ekf2/EKF/common.h | 267 ++++++++---------- src/modules/ekf2/EKF/covariance.cpp | 9 +- src/modules/ekf2/EKF/ekf.h | 2 - src/modules/ekf2/EKF/ekf_helper.cpp | 54 ++-- src/modules/ekf2/EKF/estimator_interface.cpp | 36 +-- src/modules/ekf2/EKF/height_control.cpp | 2 + .../EKF/output_predictor/output_predictor.cpp | 4 +- .../ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp | 4 +- 22 files changed, 238 insertions(+), 258 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp index c9822d4829..e2cf37e41e 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -89,8 +89,8 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) updateAirspeed(airspeed_sample, _aid_src_airspeed); - _innov_check_fail_status.flags.reject_airspeed = - _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag + // TODO: remove this redundant flag + _innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected; const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index 80c51e0979..2ad273427d 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -38,6 +38,9 @@ #include "ekf.h" +// Maximum allowable time interval between pressure altitude measurements (uSec) +static constexpr uint64_t BARO_MAX_INTERVAL{200'000}; + void Ekf::controlBaroHeightFusion() { static constexpr const char *HGT_SRC_NAME = "baro"; diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp index 78cc2bd30a..64591addc9 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp @@ -38,6 +38,9 @@ #include "ekf.h" +// Maximum allowable time interval between external vision system measurements (uSec) +static constexpr uint64_t EV_MAX_INTERVAL{200'000}; + void Ekf::controlExternalVisionFusion() { _ev_pos_b_est.predict(_dt_ekf_avg); @@ -55,10 +58,8 @@ void Ekf::controlExternalVisionFusion() const bool starting_conditions_passing = quality_sufficient && ((ev_sample.time_us - _ev_sample_prev.time_us) < EV_MAX_INTERVAL) - && ((_params.ev_quality_minimum <= 0) - || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient - && ((_params.ev_quality_minimum <= 0) - || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient + && ((_params.ev_quality_minimum <= 0) || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) + && ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) && isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL); updateEvAttitudeErrorFilter(ev_sample, ev_reset); diff --git a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp index 3704434d5f..b6e3416002 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp @@ -108,7 +108,7 @@ void Ekf::controlFakePosFusion() resetFakePosFusion(); if (_control_status.flags.tilt_align) { - // The fake position fusion is not started for initial alignement + // The fake position fusion is not started for initial alignment ECL_WARN("stopping navigation"); } } diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index ff04cc3b39..49bdf960e3 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -38,6 +38,9 @@ #include "ekf.h" +// Maximum allowable time interval between GNSS measurements (uSec) +static constexpr uint64_t GNSS_MAX_INTERVAL{500'000}; + void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) { static constexpr const char *HGT_SRC_NAME = "GNSS"; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index 652069caf5..dbf4e70f9c 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -158,8 +158,8 @@ bool Ekf::runGnssChecks(const gnssSample &gps) // Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient constexpr float filt_time_const = 10.0f; - const float dt = math::constrain(float(int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) - * 1e-6f, 0.001f, filt_time_const); + const int64_t dt_us = int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp()); + const float dt = math::constrain(fabsf(dt_us) * 1e-6f, 0.001f, filt_time_const); const float filter_coef = dt / filt_time_const; // The following checks are only valid when the vehicle is at rest diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index d5bca4f46a..2abfade116 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -55,8 +55,6 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) imu_delayed.delta_vel, imu_delayed.delta_vel_dt, (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest)); - _gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL); - // check for arrival of new sensor data at the fusion time horizon _gps_data_ready = _gps_buffer->pop_first_older_than(imu_delayed.time_us, &_gps_sample_delayed); @@ -365,13 +363,13 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample) const bool continuing_conditions_passing = _control_status.flags.tilt_align; - const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, - 2 * GNSS_YAW_MAX_INTERVAL); + // Maximum allowable time interval between GNSS yaw measurements (uSec) + static constexpr uint64_t GNSS_YAW_MAX_INTERVAL{1'500'000}; + const bool data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL); const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed - && !is_gps_yaw_data_intermittent - && !_gps_intermittent; + && !data_intermittent; if (_control_status.flags.gps_yaw) { if (continuing_conditions_passing) { diff --git a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp index e6961ed17c..380df18973 100644 --- a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp @@ -83,6 +83,8 @@ void Ekf::controlGravityFusion(const imuSample &imu) // update the states and covariance using sequential fusion for (uint8_t index = 0; index <= 2; index++) { // Calculate Kalman gains and observation jacobians + const Vector3f gravity_vector_pred = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f)); + if (index == 0) { // everything was already computed above @@ -91,16 +93,14 @@ void Ekf::controlGravityFusion(const imuSample &imu) sym::ComputeGravityYInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H); // recalculate innovation using the updated state - _aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, - -1.f))(index) - measurement(index); + _aid_src_gravity.innovation[index] = gravity_vector_pred(index) - measurement(index); } else if (index == 2) { // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) sym::ComputeGravityZInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H); // recalculate innovation using the updated state - _aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, - -1.f))(index) - measurement(index); + _aid_src_gravity.innovation[index] = gravity_vector_pred(index) - measurement(index); } VectorState K = P * H / _aid_src_gravity.innovation_variance[index]; diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 979807f116..29ea8ef242 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -41,6 +41,9 @@ #include +// Maximum allowable time interval between magnetic field measurements (uSec) +static constexpr uint64_t MAG_MAX_INTERVAL{500'000}; + void Ekf::controlMagFusion() { static constexpr const char *AID_SRC_NAME = "mag"; @@ -134,8 +137,7 @@ void Ekf::controlMagFusion() && checkMagField(mag_sample.mag) && (_mag_counter > 3) // wait until we have more than a few samples through the filter && (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame - && (_state_reset_status.reset_count.quat == - _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset + && (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) // no yaw reset this frame && isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL); checkMagHeadingConsistency(mag_sample); @@ -145,26 +147,24 @@ void Ekf::controlMagFusion() const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos; - { - const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding; - const bool common_conditions_passing = _control_status.flags.mag - && ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding) - || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) - && !_control_status.flags.mag_fault - && !_control_status.flags.mag_field_disturbed - && !_control_status.flags.ev_yaw - && !_control_status.flags.gps_yaw; + const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding; - _control_status.flags.mag_3D = common_conditions_passing - && (_params.mag_fusion_type == MagFuseType::AUTO) - && _control_status.flags.mag_aligned_in_flight; + const bool common_conditions_passing = _control_status.flags.mag + && ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding) + || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) + && !_control_status.flags.mag_fault + && !_control_status.flags.mag_field_disturbed + && !_control_status.flags.ev_yaw + && !_control_status.flags.gps_yaw; - _control_status.flags.mag_hdg = common_conditions_passing - && ((_params.mag_fusion_type == MagFuseType::HEADING) - || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D)); - } + _control_status.flags.mag_3D = common_conditions_passing + && (_params.mag_fusion_type == MagFuseType::AUTO) + && _control_status.flags.mag_aligned_in_flight; + + _control_status.flags.mag_hdg = common_conditions_passing + && ((_params.mag_fusion_type == MagFuseType::HEADING) + || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D)); - // TODO: allow clearing mag_fault if mag_3d is good? if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) { ECL_INFO("starting mag 3D fusion"); diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp index acd16578c5..9d25b83c65 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp @@ -65,6 +65,8 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima // update the states and covariance using sequential fusion of the magnetometer components for (uint8_t index = 0; index <= 2; index++) { // Calculate Kalman gains and observation jacobians + const Vector3f mag_predicted = _state.quat_nominal.rotateVectorInverse(_state.mag_I) + _state.mag_B; + if (index == 0) { // everything was already computed @@ -73,8 +75,7 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H); // recalculate innovation using the updated state - aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag( - index); + aid_src.innovation[index] = mag_predicted(index) - mag(index); } else if (index == 2) { // we do not fuse synthesized magnetomter measurements when doing 3D fusion @@ -87,8 +88,7 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H); // recalculate innovation using the updated state - aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag( - index); + aid_src.innovation[index] = mag_predicted(index) - mag(index); } if (aid_src.innovation_variance[index] < R_MAG) { @@ -174,8 +174,8 @@ bool Ekf::fuseDeclination(float decl_sigma) float decl_pred; float innovation_variance; - sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, - &H); + sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, + &decl_pred, &innovation_variance, &H); const float innovation = wrap_pi(decl_pred - decl_measurement); diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index 0e4b8b668e..2d6be39fec 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -47,8 +47,6 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) return; } - VectorState H; - // New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon if (_flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed)) { @@ -92,6 +90,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) const float R_LOS = calcOptFlowMeasVar(flow_sample); Vector2f innov_var; + VectorState H; sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, R_LOS, FLT_EPSILON, &innov_var, &H); // run the innovation consistency check and record result diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 1b7db97e30..469ac7847c 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -246,7 +246,7 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src) observation_variance, // observation variance getHagl() - aid_src.observation, // innovation innovation_variance, // innovation variance - math::max(_params.range_innov_gate, 1.f)); // innovation gate + math::max(_params.range_innov_gate, 1.f)); // innovation gate // z special case if there is bad vertical acceleration data, then don't reject measurement, // but limit innovation to prevent spikes that could destabilise the filter @@ -259,11 +259,11 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src) float Ekf::getRngVar() const { - return fmaxf( - P(State::pos.idx + 2, State::pos.idx + 2) - + sq(_params.range_noise) - + sq(_params.range_noise_scaler * _range_sensor.getRange()), - 0.f); + float rng_var = P(State::pos.idx + 2, State::pos.idx + 2) + + sq(_params.range_noise) + + sq(_params.range_noise_scaler * _range_sensor.getRange()); + + return math::max(rng_var, 0.f); } void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp index f3c59be541..c8ca588626 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp @@ -56,8 +56,8 @@ struct rangeSample { int8_t quality{}; ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality. }; -static constexpr uint64_t RNG_MAX_INTERVAL = - 200e3; ///< Maximum allowable time interval between range finder measurements (uSec) +// Maximum allowable time interval between range finder measurements (uSec) +static constexpr uint64_t RNG_MAX_INTERVAL{200'000}; class SensorRangeFinder : public Sensor { diff --git a/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp index 3c0a2c1d6c..162ce2f1a3 100644 --- a/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp +++ b/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp @@ -44,8 +44,7 @@ void Ekf::controlZeroInnovationHeadingUpdate() || _control_status.flags.ev_yaw || _control_status.flags.gps_yaw; // fuse zero innovation at a limited rate if the yaw variance is too large - if (!yaw_aiding - && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) { + if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, 200'000)) { // Use an observation variance larger than usual but small enough // to constrain the yaw variance just below the threshold @@ -61,7 +60,8 @@ void Ekf::controlZeroInnovationHeadingUpdate() computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW); if (!_control_status.flags.tilt_align - || (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) { + || ((aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) + ) { // The yaw variance is too large, fuse fake measurement fuseYaw(aid_src_status, H_YAW); } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 05610bca58..ce3bbb6e5a 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -65,28 +65,6 @@ using math::Utilities::shouldUse321RotationSequence; using math::Utilities::sq; using math::Utilities::updateYawInRotMat; -// maximum sensor intervals in usec -static constexpr uint64_t BARO_MAX_INTERVAL = - 200e3; ///< Maximum allowable time interval between pressure altitude measurements (uSec) -static constexpr uint64_t EV_MAX_INTERVAL = - 200e3; ///< Maximum allowable time interval between external vision system measurements (uSec) -static constexpr uint64_t GNSS_MAX_INTERVAL = - 500e3; ///< Maximum allowable time interval between GNSS measurements (uSec) -static constexpr uint64_t GNSS_YAW_MAX_INTERVAL = - 1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec) -static constexpr uint64_t MAG_MAX_INTERVAL = - 500e3; ///< Maximum allowable time interval between magnetic field measurements (uSec) - -// bad accelerometer detection and mitigation -static constexpr uint64_t BADACC_PROBATION = - 10e6; ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec) -static constexpr float BADACC_BIAS_PNOISE = - 4.9f; ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2) - -// ground effect compensation -static constexpr uint64_t GNDEFFECT_TIMEOUT = - 10e6; ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec) - enum class PositionFrame : uint8_t { LOCAL_FRAME_NED = 0, LOCAL_FRAME_FRD = 1, @@ -496,19 +474,18 @@ struct parameters { union fault_status_u { struct { - bool bad_mag_x : 1; ///< 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error - bool bad_mag_y : 1; ///< 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error - bool bad_mag_z : 1; ///< 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error - bool bad_hdg : 1; ///< 3 - true if the fusion of the heading angle has encountered a numerical error - bool bad_mag_decl : 1; ///< 4 - true if the fusion of the magnetic declination has encountered a numerical error - bool bad_airspeed : 1; ///< 5 - true if fusion of the airspeed has encountered a numerical error -bool bad_sideslip : - 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error - bool bad_optflow_X : 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error - bool bad_optflow_Y : 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error - bool bad_acc_bias : 1; ///< 9 - true if bad delta velocity bias estimates have been detected - bool bad_acc_vertical : 1; ///< 10 - true if bad vertical accelerometer data has been detected - bool bad_acc_clipping : 1; ///< 11 - true if delta velocity data contains clipping (asymmetric railing) + bool bad_mag_x : 1; ///< 0 - fusion of the magnetometer X-axis has encountered a numerical error + bool bad_mag_y : 1; ///< 1 - fusion of the magnetometer Y-axis has encountered a numerical error + bool bad_mag_z : 1; ///< 2 - fusion of the magnetometer Z-axis has encountered a numerical error + bool bad_hdg : 1; ///< 3 - fusion of the heading angle has encountered a numerical error + bool bad_mag_decl : 1; ///< 4 - fusion of the magnetic declination has encountered a numerical error + bool bad_airspeed : 1; ///< 5 - fusion of the airspeed has encountered a numerical error + bool bad_sideslip : 1; ///< 6 - fusion of the synthetic sideslip constraint encountered a numerical error + bool bad_optflow_X : 1; ///< 7 - fusion of the optical flow X axis has encountered a numerical error + bool bad_optflow_Y : 1; ///< 8 - fusion of the optical flow Y axis has encountered a numerical error + bool bad_acc_bias : 1; ///< 9 - bad delta velocity bias estimates have been detected + bool bad_acc_vertical : 1; ///< 10 - bad vertical accelerometer data has been detected + bool bad_acc_clipping : 1; ///< 11 - delta velocity data contains clipping (asymmetric railing) } flags; uint32_t value; }; @@ -516,19 +493,19 @@ bool bad_sideslip : // define structure used to communicate innovation test failures union innovation_fault_status_u { struct { - bool reject_hor_vel : 1; ///< 0 - true if horizontal velocity observations have been rejected - bool reject_ver_vel : 1; ///< 1 - true if vertical velocity observations have been rejected - bool reject_hor_pos : 1; ///< 2 - true if horizontal position observations have been rejected - bool reject_ver_pos : 1; ///< 3 - true if true if vertical position observations have been rejected - bool reject_mag_x : 1; ///< 4 - true if the X magnetometer observation has been rejected - bool reject_mag_y : 1; ///< 5 - true if the Y magnetometer observation has been rejected - bool reject_mag_z : 1; ///< 6 - true if the Z magnetometer observation has been rejected - bool reject_yaw : 1; ///< 7 - true if the yaw observation has been rejected - bool reject_airspeed : 1; ///< 8 - true if the airspeed observation has been rejected - bool reject_sideslip : 1; ///< 9 - true if the synthetic sideslip observation has been rejected + bool reject_hor_vel : 1; ///< 0 - horizontal velocity observations have been rejected + bool reject_ver_vel : 1; ///< 1 - vertical velocity observations have been rejected + bool reject_hor_pos : 1; ///< 2 - horizontal position observations have been rejected + bool reject_ver_pos : 1; ///< 3 - vertical position observations have been rejected + bool reject_mag_x : 1; ///< 4 - X magnetometer observation has been rejected + bool reject_mag_y : 1; ///< 5 - Y magnetometer observation has been rejected + bool reject_mag_z : 1; ///< 6 - Z magnetometer observation has been rejected + bool reject_yaw : 1; ///< 7 - yaw observation has been rejected + bool reject_airspeed : 1; ///< 8 - airspeed observation has been rejected + bool reject_sideslip : 1; ///< 9 - synthetic sideslip observation has been rejected bool reject_hagl : 1; ///< 10 - unused - bool reject_optflow_X : 1; ///< 11 - true if the X optical flow observation has been rejected - bool reject_optflow_Y : 1; ///< 12 - true if the Y optical flow observation has been rejected + bool reject_optflow_X : 1; ///< 11 - X optical flow observation has been rejected + bool reject_optflow_Y : 1; ///< 12 - Y optical flow observation has been rejected } flags; uint16_t value; }; @@ -536,17 +513,17 @@ union innovation_fault_status_u { // publish the status of various GPS quality checks union gps_check_fail_status_u { struct { - uint16_t fix : 1; ///< 0 - true if the fix type is insufficient (no 3D solution) - uint16_t nsats : 1; ///< 1 - true if number of satellites used is insufficient - uint16_t pdop : 1; ///< 2 - true if position dilution of precision is insufficient - uint16_t hacc : 1; ///< 3 - true if reported horizontal accuracy is insufficient - uint16_t vacc : 1; ///< 4 - true if reported vertical accuracy is insufficient - uint16_t sacc : 1; ///< 5 - true if reported speed accuracy is insufficient - uint16_t hdrift : 1; ///< 6 - true if horizontal drift is excessive (can only be used when stationary on ground) - uint16_t vdrift : 1; ///< 7 - true if vertical drift is excessive (can only be used when stationary on ground) - uint16_t hspeed : 1; ///< 8 - true if horizontal speed is excessive (can only be used when stationary on ground) - uint16_t vspeed : 1; ///< 9 - true if vertical speed error is excessive - uint16_t spoofed: 1; ///< 10 - true if the GNSS data is spoofed + uint16_t fix : 1; ///< 0 - fix type is insufficient (no 3D solution) + uint16_t nsats : 1; ///< 1 - number of satellites used is insufficient + uint16_t pdop : 1; ///< 2 - position dilution of precision is insufficient + uint16_t hacc : 1; ///< 3 - reported horizontal accuracy is insufficient + uint16_t vacc : 1; ///< 4 - reported vertical accuracy is insufficient + uint16_t sacc : 1; ///< 5 - reported speed accuracy is insufficient + uint16_t hdrift : 1; ///< 6 - horizontal drift is excessive (can only be used when stationary on ground) + uint16_t vdrift : 1; ///< 7 - vertical drift is excessive (can only be used when stationary on ground) + uint16_t hspeed : 1; ///< 8 - horizontal speed is excessive (can only be used when stationary on ground) + uint16_t vspeed : 1; ///< 9 - vertical speed error is excessive + uint16_t spoofed: 1; ///< 10 - GNSS data reports spoofing } flags; uint16_t value; }; @@ -554,60 +531,47 @@ union gps_check_fail_status_u { // bitmask containing filter control status union filter_control_status_u { struct { - uint64_t tilt_align : 1; ///< 0 - true if the filter tilt alignment is complete - uint64_t yaw_align : 1; ///< 1 - true if the filter yaw alignment is complete - uint64_t gps : 1; ///< 2 - true if GPS measurement fusion is intended - uint64_t opt_flow : 1; ///< 3 - true if optical flow measurements fusion is intended - uint64_t mag_hdg : 1; ///< 4 - true if a simple magnetic yaw heading fusion is intended - uint64_t mag_3D : 1; ///< 5 - true if 3-axis magnetometer measurement fusion is intended - uint64_t mag_dec : 1; ///< 6 - true if synthetic magnetic declination measurements fusion is intended - uint64_t in_air : 1; ///< 7 - true when the vehicle is airborne - uint64_t wind : 1; ///< 8 - true when wind velocity is being estimated - uint64_t baro_hgt : 1; ///< 9 - true when baro height is being fused as a primary height reference -uint64_t rng_hgt : - 1; ///< 10 - true when range finder height is being fused as a primary height reference - uint64_t gps_hgt : 1; ///< 11 - true when GPS height is being fused as a primary height reference - uint64_t ev_pos : 1; ///< 12 - true when local position data fusion from external vision is intended - uint64_t ev_yaw : 1; ///< 13 - true when yaw data from external vision measurements fusion is intended - uint64_t ev_hgt : 1; ///< 14 - true when height data from external vision measurements is being fused - uint64_t fuse_beta : 1; ///< 15 - true when synthetic sideslip measurements are being fused - uint64_t mag_field_disturbed : 1; ///< 16 - true when the mag field does not match the expected strength - uint64_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle -uint64_t mag_fault : - 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used - uint64_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused -uint64_t gnd_effect : - 1; ///< 20 - true when protection from ground effect induced static pressure rise is active -uint64_t rng_stuck : - 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough -uint64_t gps_yaw : - 1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended - uint64_t mag_aligned_in_flight : 1; ///< 23 - true when the in-flight mag field alignment has been completed -uint64_t ev_vel : - 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended -uint64_t synthetic_mag_z : - 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component - uint64_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest -uint64_t gps_yaw_fault : - 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used -uint64_t rng_fault : - 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used -uint64_t inertial_dead_reckoning : - 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift - uint64_t wind_dead_reckoning : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements - uint64_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing - uint64_t fake_pos : 1; ///< 32 - true when fake position measurements are being fused - uint64_t fake_hgt : 1; ///< 33 - true when fake height measurements are being fused - uint64_t gravity_vector : 1; ///< 34 - true when gravity vector measurements are being fused -uint64_t mag : - 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended -uint64_t ev_yaw_fault : - 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used -uint64_t mag_heading_consistent : - 1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter - uint64_t aux_gpos : 1; ///< 38 - true if auxiliary global position measurement fusion is intended - uint64_t rng_terrain : 1; ///< 39 - true if we are fusing range finder data for terrain - uint64_t opt_flow_terrain : 1; ///< 40 - true if we are fusing flow data for terrain + uint64_t tilt_align : 1; ///< 0 - filter tilt alignment is complete + uint64_t yaw_align : 1; ///< 1 - filter yaw alignment is complete + uint64_t gps : 1; ///< 2 - GPS measurement fusion is intended + uint64_t opt_flow : 1; ///< 3 - optical flow measurements fusion is intended + uint64_t mag_hdg : 1; ///< 4 - a simple magnetic yaw heading fusion is intended + uint64_t mag_3D : 1; ///< 5 - 3-axis magnetometer measurement fusion is intended + uint64_t mag_dec : 1; ///< 6 - synthetic magnetic declination measurements fusion is intended + uint64_t in_air : 1; ///< 7 - the vehicle is airborne + uint64_t wind : 1; ///< 8 - wind velocity is being estimated + uint64_t baro_hgt : 1; ///< 9 - baro height is being fused as a primary height reference + uint64_t rng_hgt : 1; ///< 10 - range finder height is being fused as a primary height ref + uint64_t gps_hgt : 1; ///< 11 - GPS height is being fused as a primary height reference + uint64_t ev_pos : 1; ///< 12 - local position data fusion from external vision is intended + uint64_t ev_yaw : 1; ///< 13 - yaw data from external vision measurements fusion is intended + uint64_t ev_hgt : 1; ///< 14 - height data from external vision measurements is being fused + uint64_t fuse_beta : 1; ///< 15 - synthetic sideslip measurements are being fused + uint64_t mag_field_disturbed : 1; ///< 16 - the mag field does not match the expected strength + uint64_t fixed_wing : 1; ///< 17 - the vehicle is operating as a fixed wing vehicle + uint64_t mag_fault : 1; ///< 18 - the mag has been declared faulty and is no longer being used + uint64_t fuse_aspd : 1; ///< 19 - airspeed measurements are being fused + uint64_t gnd_effect : 1; ///< 20 - protection from ground effect induced static pressure rise is active + uint64_t rng_stuck : 1; ///< 21 - rng data wasn't ready and new values haven't changed enough + uint64_t gps_yaw : 1; ///< 22 - yaw (not ground course) data fusion from a GPS receiver is intended + uint64_t mag_aligned_in_flight : 1; ///< 23 - the in-flight mag field alignment has been completed + uint64_t ev_vel : 1; ///< 24 - local frame velocity data fusion from vision measurements is intended + uint64_t synthetic_mag_z : 1; ///< 25 - using a synthesized measurement for the magnetometer Z component + uint64_t vehicle_at_rest : 1; ///< 26 - the vehicle is at rest + uint64_t gps_yaw_fault : 1; ///< 27 - the GNSS heading has been declared faulty and is no longer being used + uint64_t rng_fault : 1; ///< 28 - the range finder has been declared faulty and is no longer being used + uint64_t inertial_dead_reckoning : 1; ///< 29 - longer fusing measurements that constrain horizontal velocity drift + uint64_t wind_dead_reckoning : 1; ///< 30 - navigation reliant on wind relative measurements + uint64_t rng_kin_consistent : 1; ///< 31 - the range finder kinematic consistency check is passing + uint64_t fake_pos : 1; ///< 32 - fake position measurements are being fused + uint64_t fake_hgt : 1; ///< 33 - fake height measurements are being fused + uint64_t gravity_vector : 1; ///< 34 - gravity vector measurements are being fused + uint64_t mag : 1; ///< 35 - 3-axis magnetometer measurement fusion (mag states only) is intended + uint64_t ev_yaw_fault : 1; ///< 36 - the EV heading has been declared faulty and is no longer being used + uint64_t mag_heading_consistent : 1; ///< 37 - the heading obtained mag is declared consistent with the filter + uint64_t aux_gpos : 1; ///< 38 - auxiliary global position measurement fusion is intended + uint64_t rng_terrain : 1; ///< 39 - fusing range finder data for terrain + uint64_t opt_flow_terrain : 1; ///< 40 - fusing flow data for terrain } flags; uint64_t value; @@ -616,52 +580,53 @@ uint64_t mag_heading_consistent : // Mavlink bitmask containing state of estimator solution union ekf_solution_status_u { struct { - uint16_t attitude : 1; ///< 0 - True if the attitude estimate is good - uint16_t velocity_horiz : 1; ///< 1 - True if the horizontal velocity estimate is good - uint16_t velocity_vert : 1; ///< 2 - True if the vertical velocity estimate is good - uint16_t pos_horiz_rel : 1; ///< 3 - True if the horizontal position (relative) estimate is good - uint16_t pos_horiz_abs : 1; ///< 4 - True if the horizontal position (absolute) estimate is good - uint16_t pos_vert_abs : 1; ///< 5 - True if the vertical position (absolute) estimate is good - uint16_t pos_vert_agl : 1; ///< 6 - True if the vertical position (above ground) estimate is good -uint16_t const_pos_mode : - 1; ///< 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) -uint16_t pred_pos_horiz_rel : - 1; ///< 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate -uint16_t pred_pos_horiz_abs : - 1; ///< 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate - uint16_t gps_glitch : 1; ///< 10 - True if the EKF has detected a GPS glitch - uint16_t accel_error : 1; ///< 11 - True if the EKF has detected bad accelerometer data + uint16_t attitude : 1; ///< 0 - attitude estimate is good + uint16_t velocity_horiz : 1; ///< 1 - horizontal velocity estimate is good + uint16_t velocity_vert : 1; ///< 2 - vertical velocity estimate is good + uint16_t pos_horiz_rel : 1; ///< 3 - horizontal position (relative) estimate is good + uint16_t pos_horiz_abs : 1; ///< 4 - horizontal position (absolute) estimate is good + uint16_t pos_vert_abs : 1; ///< 5 - vertical position (absolute) estimate is good + uint16_t pos_vert_agl : 1; ///< 6 - vertical position (above ground) estimate is good + uint16_t const_pos_mode : 1; ///< 7 - in a constant position mode and is not using external measurements + uint16_t pred_pos_horiz_rel : 1; ///< 8 - sufficient data for a (relative) position estimate + uint16_t pred_pos_horiz_abs : 1; ///< 9 - sufficient data for a (absolute) position estimate + uint16_t gps_glitch : 1; ///< 10 - detected a GPS glitch + uint16_t accel_error : 1; ///< 11 - EKF has detected bad accelerometer data } flags; uint16_t value; }; +#if defined(CONFIG_EKF2_TERRAIN) +union terrain_fusion_status_u { + struct { + bool range_finder : 1; ///< 0 - fusing range finder data for terrain + bool flow : 1; ///< 1 - fusing flow data for terrain + } flags; + uint8_t value; +}; +#endif // CONFIG_EKF2_TERRAIN + // define structure used to communicate information events union information_event_status_u { struct { - bool gps_checks_passed : 1; ///< 0 - true when gps quality checks are passing passed - bool reset_vel_to_gps : 1; ///< 1 - true when the velocity states are reset to the gps measurement - bool reset_vel_to_flow : 1; ///< 2 - true when the velocity states are reset using the optical flow measurement - bool reset_vel_to_vision : 1; ///< 3 - true when the velocity states are reset to the vision system measurement - bool reset_vel_to_zero : 1; ///< 4 - true when the velocity states are reset to zero - bool reset_pos_to_last_known : 1; ///< 5 - true when the position states are reset to the last known position - bool reset_pos_to_gps : 1; ///< 6 - true when the position states are reset to the gps measurement - bool reset_pos_to_vision : 1; ///< 7 - true when the position states are reset to the vision system measurement -bool starting_gps_fusion : - 1; ///< 8 - true when the filter starts using gps measurements to correct the state estimates -bool starting_vision_pos_fusion : - 1; ///< 9 - true when the filter starts using vision system position measurements to correct the state estimates -bool starting_vision_vel_fusion : - 1; ///< 10 - true when the filter starts using vision system velocity measurements to correct the state estimates -bool starting_vision_yaw_fusion : - 1; ///< 11 - true when the filter starts using vision system yaw measurements to correct the state estimates -bool yaw_aligned_to_imu_gps : - 1; ///< 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data - bool reset_hgt_to_baro : 1; ///< 13 - true when the vertical position state is reset to the baro measurement - bool reset_hgt_to_gps : 1; ///< 14 - true when the vertical position state is reset to the gps measurement - bool reset_hgt_to_rng : 1; ///< 15 - true when the vertical position state is reset to the rng measurement - bool reset_hgt_to_ev : 1; ///< 16 - true when the vertical position state is reset to the ev measurement -bool reset_pos_to_ext_obs : - 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning + bool gps_checks_passed : 1; ///< 0 - gps quality checks are passing passed + bool reset_vel_to_gps : 1; ///< 1 - the velocity states are reset to the gps measurement + bool reset_vel_to_flow : 1; ///< 2 - the velocity states are reset using the optical flow measurement + bool reset_vel_to_vision : 1; ///< 3 - the velocity states are reset to the vision system measurement + bool reset_vel_to_zero : 1; ///< 4 - the velocity states are reset to zero + bool reset_pos_to_last_known : 1; ///< 5 - the position states are reset to the last known position + bool reset_pos_to_gps : 1; ///< 6 - the position states are reset to the gps measurement + bool reset_pos_to_vision : 1; ///< 7 - the position states are reset to the vision system measurement + bool starting_gps_fusion : 1; ///< 8 - started using gps measurements to correct the state estimates + bool starting_vision_pos_fusion : 1; ///< 9 - started using vision position measurements to correct the state estimates + bool starting_vision_vel_fusion : 1; ///< 10 - started using vision velocity measurements to correct the state estimates + bool starting_vision_yaw_fusion : 1; ///< 11 - started using vision yaw measurements to correct the state estimates + bool yaw_aligned_to_imu_gps : 1; ///< 12 - the filter resets the yaw to an estimate derived from IMU and GPS data + bool reset_hgt_to_baro : 1; ///< 13 - the vertical position state is reset to the baro measurement + bool reset_hgt_to_gps : 1; ///< 14 - the vertical position state is reset to the gps measurement + bool reset_hgt_to_rng : 1; ///< 15 - the vertical position state is reset to the rng measurement + bool reset_hgt_to_ev : 1; ///< 16 - the vertical position state is reset to the ev measurement + bool reset_pos_to_ext_obs : 1; ///< 17 - position was reset to an external observation while deadreckoning } flags; uint32_t value; }; diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 8108d26c13..8918c872de 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -125,6 +125,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) for (unsigned i = 0; i < 3; i++) { if (_fault_status.flags.bad_acc_vertical || imu_delayed.delta_vel_clipping[i]) { // Increase accelerometer process noise if bad accel data is detected + static constexpr float BADACC_BIAS_PNOISE{4.9f}; ///< IMU acceleration noise when accel data is declared bad (m/sec**2) accel_var(i) = sq(BADACC_BIAS_PNOISE); } else { @@ -202,8 +203,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) if (_control_status.flags.wind) { // wind vel: add process noise - float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, - 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); + float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) + * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; for (unsigned index = 0; index < State::wind_vel.dof; index++) { @@ -222,8 +223,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) float terrain_process_noise = sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise); // process noise due to terrain gradient - terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel( - 1))); + terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) + * (sq(_state.vel(0)) + sq(_state.vel(1))); P(State::terrain.idx, State::terrain.idx) += terrain_process_noise; } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 1b20c433e2..5cb8cfcd43 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -657,8 +657,6 @@ private: bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed gps_check_fail_status_u _gps_check_fail_status{}; - // height sensor status - bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref}; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index e44d6c0bf6..98e72f6e2c 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -72,8 +72,8 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo return _NED_origin_initialised; } -bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph, - const float epv) +bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, + const float eph, const float epv) { // sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90) @@ -536,14 +536,21 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const ekf_solution_status_u soln_status{}; // TODO: Is this accurate enough? soln_status.flags.attitude = attitude_valid(); - soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta - && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); + + soln_status.flags.velocity_horiz = (isHorizontalAidingActive() + || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); + soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt - || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0); + || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) + && (_fault_status.value == 0); + soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos - || _control_status.flags.opt_flow) && (_fault_status.value == 0); + || _control_status.flags.opt_flow) + && (_fault_status.value == 0); + soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0); + soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert; #if defined(CONFIG_EKF2_TERRAIN) soln_status.flags.pos_vert_agl = isTerrainEstimateValid(); @@ -580,36 +587,37 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const void Ekf::fuse(const VectorState &K, float innovation) { // quat_nominal - Quatf delta_quat(matrix::AxisAnglef(K.slice(State::quat_nominal.idx, - 0) * (-1.f * innovation))); + Quatf delta_quat(AxisAnglef(K.slice(State::quat_nominal.idx, 0) + * (-1.f * innovation))); _state.quat_nominal = delta_quat * _state.quat_nominal; _state.quat_nominal.normalize(); _R_to_earth = Dcmf(_state.quat_nominal); // vel - _state.vel = matrix::constrain(_state.vel - K.slice(State::vel.idx, 0) * innovation, -1.e3f, 1.e3f); + _state.vel -= K.slice(State::vel.idx, 0) * innovation; + _state.vel = matrix::constrain(_state.vel, -1.e3f, 1.e3f); // pos - _state.pos = matrix::constrain(_state.pos - K.slice(State::pos.idx, 0) * innovation, -1.e6f, 1.e6f); + _state.pos -= K.slice(State::pos.idx, 0) * innovation; + _state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f); // gyro_bias - _state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice(State::gyro_bias.idx, - 0) * innovation, - -getGyroBiasLimit(), getGyroBiasLimit()); + _state.gyro_bias -= K.slice(State::gyro_bias.idx, 0) * innovation; + _state.gyro_bias = matrix::constrain(_state.gyro_bias, -getGyroBiasLimit(), getGyroBiasLimit()); // accel_bias - _state.accel_bias = matrix::constrain(_state.accel_bias - K.slice(State::accel_bias.idx, - 0) * innovation, - -getAccelBiasLimit(), getAccelBiasLimit()); + _state.accel_bias -= K.slice(State::accel_bias.idx, 0) * innovation; + _state.accel_bias = matrix::constrain(_state.accel_bias, -getAccelBiasLimit(), getAccelBiasLimit()); #if defined(CONFIG_EKF2_MAGNETOMETER) // mag_I, mag_B if (_control_status.flags.mag) { - _state.mag_I = matrix::constrain(_state.mag_I - K.slice(State::mag_I.idx, 0) * innovation, -1.f, - 1.f); - _state.mag_B = matrix::constrain(_state.mag_B - K.slice(State::mag_B.idx, 0) * innovation, - -getMagBiasLimit(), getMagBiasLimit()); + _state.mag_I -= K.slice(State::mag_I.idx, 0) * innovation; + _state.mag_I = matrix::constrain(_state.mag_I, -1.f, 1.f); + + _state.mag_B -= K.slice(State::mag_B.idx, 0) * innovation; + _state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit()); } #endif // CONFIG_EKF2_MAGNETOMETER @@ -618,8 +626,8 @@ void Ekf::fuse(const VectorState &K, float innovation) // wind_vel if (_control_status.flags.wind) { - _state.wind_vel = matrix::constrain(_state.wind_vel - K.slice(State::wind_vel.idx, - 0) * innovation, -1.e2f, 1.e2f); + _state.wind_vel -= K.slice(State::wind_vel.idx, 0) * innovation; + _state.wind_vel = matrix::constrain(_state.wind_vel, -1.e2f, 1.e2f); } #endif // CONFIG_EKF2_WIND @@ -787,6 +795,8 @@ void Ekf::updateGroundEffect() #endif // CONFIG_EKF2_TERRAIN if (_control_status.flags.gnd_effect) { // Turn off ground effect compensation if it times out + static constexpr uint64_t GNDEFFECT_TIMEOUT = 10e6; + if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { _control_status.flags.gnd_effect = false; } diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index bce6ec8863..0ea4a66600 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -142,8 +142,8 @@ void EstimatorInterface::setMagData(const magSample &mag_sample) _time_last_mag_buffer_push = _time_latest_us; } else { - ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, - _min_obs_interval_us); + ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", + time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us); } } #endif // CONFIG_EKF2_MAGNETOMETER @@ -189,8 +189,8 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample) #endif // CONFIG_EKF2_GNSS_YAW } else { - ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, - _min_obs_interval_us); + ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", + time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us); } } #endif // CONFIG_EKF2_GNSS @@ -228,8 +228,8 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample) _time_last_baro_buffer_push = _time_latest_us; } else { - ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, - _min_obs_interval_us); + ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", + time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us); } } #endif // CONFIG_EKF2_BAROMETER @@ -266,8 +266,8 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) _airspeed_buffer->push(airspeed_sample_new); } else { - ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, - _min_obs_interval_us); + ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", + time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us); } } #endif // CONFIG_EKF2_AIRSPEED @@ -305,8 +305,8 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample) _time_last_range_buffer_push = _time_latest_us; } else { - ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, - _min_obs_interval_us); + ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", + time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us); } } #endif // CONFIG_EKF2_RANGE_FINDER @@ -343,8 +343,8 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow) _flow_buffer->push(optflow_sample_new); } else { - ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, - _min_obs_interval_us); + ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", + time_us, _flow_buffer->get_newest().time_us, _min_obs_interval_us); } } #endif // CONFIG_EKF2_OPTICAL_FLOW @@ -383,8 +383,8 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata) _time_last_ext_vision_buffer_push = _time_latest_us; } else { - ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, - _min_obs_interval_us); + ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", + time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us); } } #endif // CONFIG_EKF2_EXTERNAL_VISION @@ -421,8 +421,8 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample) _auxvel_buffer->push(auxvel_sample_new); } else { - ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, - _min_obs_interval_us); + ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", + time_us, _auxvel_buffer->get_newest().time_us, _min_obs_interval_us); } } #endif // CONFIG_EKF2_AUXVEL @@ -457,8 +457,8 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags) _system_flag_buffer->push(system_flags_new); } else { - ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, - _system_flag_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", + time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us); } } diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 5ad5d7db34..854bf7058f 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -269,6 +269,8 @@ void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_delayed) // declare a bad vertical acceleration measurement and make the declaration persist // for a minimum of BADACC_PROBATION seconds + static constexpr uint64_t BADACC_PROBATION = 10e6; + if (_fault_status.flags.bad_acc_vertical) { _fault_status.flags.bad_acc_vertical = isRecent(_time_bad_vert_accel, BADACC_PROBATION); diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp index d16e20a790..ffa69663d8 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp @@ -261,8 +261,8 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector } void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, - const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, const matrix::Vector3f &gyro_bias, - const matrix::Vector3f &accel_bias) + const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, + const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias) { // calculate an average filter update time if (_time_last_correct_states_us != 0) { diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp index 7aad6620ce..e989f68b90 100644 --- a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp @@ -301,8 +301,8 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang // Use fixed values for delta angle process noise variances const float d_ang_var = sq(_gyro_noise * delta_ang_dt); - _ekf_gsf[model_index].P = sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P, Vector2f(dvx, - dvy), d_vel_var, daz, d_ang_var); + _ekf_gsf[model_index].P = sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P, + Vector2f(dvx, dvy), d_vel_var, daz, d_ang_var); // covariance matrix is symmetrical, so copy upper half to lower half _ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1);