diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 4d59bcffd7..c5f9c05fcf 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -203,9 +203,9 @@ endif() if(CONFIG_EKF2_RANGE_FINDER) list(APPEND EKF_SRCS - EKF/range_finder_consistency_check.cpp - EKF/range_height_control.cpp - EKF/sensor_range_finder.cpp + EKF/aid_sources/range_finder/range_finder_consistency_check.cpp + EKF/aid_sources/range_finder/range_height_control.cpp + EKF/aid_sources/range_finder/sensor_range_finder.cpp ) endif() diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 8dc8c0fad4..a22e3816de 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -122,9 +122,9 @@ endif() if(CONFIG_EKF2_RANGE_FINDER) list(APPEND EKF_SRCS - range_finder_consistency_check.cpp - range_height_control.cpp - sensor_range_finder.cpp + aid_sources/range_finder/range_finder_consistency_check.cpp + aid_sources/range_finder/range_height_control.cpp + aid_sources/range_finder/sensor_range_finder.cpp ) endif() diff --git a/src/modules/ekf2/EKF/Sensor.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp similarity index 99% rename from src/modules/ekf2/EKF/Sensor.hpp rename to src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp index e1b83e708b..8134418fab 100644 --- a/src/modules/ekf2/EKF/Sensor.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp @@ -42,7 +42,7 @@ #ifndef EKF_SENSOR_HPP #define EKF_SENSOR_HPP -#include "common.h" +#include namespace estimator { diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp similarity index 88% rename from src/modules/ekf2/EKF/range_finder_consistency_check.cpp rename to src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp index 453da560b4..918ab523c0 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp @@ -35,9 +35,10 @@ * @file range_finder_consistency_check.cpp */ -#include "range_finder_consistency_check.hpp" +#include -void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us) +void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, + bool horizontal_motion, uint64_t time_us) { if (horizontal_motion) { _time_last_horizontal_motion = time_us; @@ -55,7 +56,8 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt; _innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down - const float var = 2.f * dist_bottom_var / (dt * dt); // Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2 + // Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2 + const float var = 2.f * dist_bottom_var / (dt * dt); _innov_var = var + vz_var; const float normalized_innov_sq = (_innov * _innov) / _innov_var; @@ -84,8 +86,9 @@ void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) } } else { - if (_test_ratio < 1.f - && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) { + if ((_test_ratio < 1.f) + && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us) + ) { _is_kinematically_consistent = true; } } diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp similarity index 100% rename from src/modules/ekf2/EKF/range_finder_consistency_check.hpp rename to src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp similarity index 93% rename from src/modules/ekf2/EKF/range_height_control.cpp rename to src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 8fede499e5..8b4b96712d 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -64,13 +64,14 @@ void Ekf::controlRangeHeightFusion() if (_control_status.flags.in_air) { const bool horizontal_motion = _control_status.flags.fixed_wing - || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f)); + || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f)); const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); const float var = sq(_params.range_noise) + dist_dependant_var; _rng_consistency_check.setGate(_params.range_kin_consistency_gate); - _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us); + _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), + P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us); } } else { @@ -121,13 +122,15 @@ void Ekf::controlRangeHeightFusion() } // determine if we should use height aiding - const bool do_conditional_range_aid = (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL)) && isConditionalRangeAidSuitable(); + const bool do_conditional_range_aid = (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL)) + && isConditionalRangeAidSuitable(); + const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast(RngCtrl::ENABLED)) || do_conditional_range_aid) && measurement_valid && _range_sensor.isDataHealthy(); const bool starting_conditions_passing = continuing_conditions_passing - && isNewestSampleRecent(_time_last_range_buffer_push, 2 * RNG_MAX_INTERVAL) + && isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL) && _range_sensor.isRegularlySendingData(); if (_control_status.flags.rng_hgt) { @@ -165,13 +168,17 @@ void Ekf::controlRangeHeightFusion() } else { if (starting_conditions_passing) { - if ((_params.height_sensor_ref == static_cast(HeightSensor::RANGE)) && (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL))) { + if ((_params.height_sensor_ref == static_cast(HeightSensor::RANGE)) + && (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL)) + ) { // Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference. ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME); _height_sensor_ref = HeightSensor::RANGE; bias_est.setBias(_state.pos(2) + measurement); - } else if ((_params.height_sensor_ref == static_cast(HeightSensor::RANGE)) && (_params.rng_ctrl != static_cast(RngCtrl::CONDITIONAL))) { + } else if ((_params.height_sensor_ref == static_cast(HeightSensor::RANGE)) + && (_params.rng_ctrl != static_cast(RngCtrl::CONDITIONAL)) + ) { // Range finder is the primary height source, the ground is now the datum used // to compute the local vertical position ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME); @@ -193,7 +200,7 @@ void Ekf::controlRangeHeightFusion() } } else if (_control_status.flags.rng_hgt - && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * RNG_MAX_INTERVAL)) { + && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) { // No data anymore. Stop until it comes back. ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME); stopRngHgtFusion(); @@ -203,6 +210,7 @@ void Ekf::controlRangeHeightFusion() bool Ekf::isConditionalRangeAidSuitable() { #if defined(CONFIG_EKF2_TERRAIN) + if (_control_status.flags.in_air && _range_sensor.isHealthy() && isTerrainEstimateValid()) { @@ -236,6 +244,7 @@ bool Ekf::isConditionalRangeAidSuitable() return is_in_range && is_hagl_stable && is_below_max_speed; } + #endif // CONFIG_EKF2_TERRAIN return false; diff --git a/src/modules/ekf2/EKF/sensor_range_finder.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp similarity index 94% rename from src/modules/ekf2/EKF/sensor_range_finder.cpp rename to src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp index c47cf0f74d..359d10ca52 100644 --- a/src/modules/ekf2/EKF/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp @@ -38,20 +38,22 @@ * */ -#include "sensor_range_finder.hpp" +#include + +#include namespace estimator { namespace sensor { -void SensorRangeFinder::runChecks(const uint64_t current_time_us, const Dcmf &R_to_earth) +void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth) { updateSensorToEarthRotation(R_to_earth); updateValidity(current_time_us); } -void SensorRangeFinder::updateSensorToEarthRotation(const Dcmf &R_to_earth) +void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth) { // calculate 2,2 element of rotation matrix from sensor frame to earth frame // this is required for use of range finder and flow data @@ -114,7 +116,7 @@ inline bool SensorRangeFinder::isDataInRange() const void SensorRangeFinder::updateStuckCheck() { - if(!isStuckDetectorEnabled()){ + if (!isStuckDetectorEnabled()) { // Stuck detector disabled _is_stuck = false; return; diff --git a/src/modules/ekf2/EKF/sensor_range_finder.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp similarity index 93% rename from src/modules/ekf2/EKF/sensor_range_finder.hpp rename to src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp index ec642f8e98..dbfbd96b10 100644 --- a/src/modules/ekf2/EKF/sensor_range_finder.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp @@ -42,6 +42,7 @@ #define EKF_SENSOR_RANGE_FINDER_HPP #include "Sensor.hpp" + #include namespace estimator @@ -49,6 +50,14 @@ namespace estimator namespace sensor { +struct rangeSample { + uint64_t time_us{}; ///< timestamp of the measurement (uSec) + float rng{}; ///< range (distance to ground) measurement (m) + 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) + class SensorRangeFinder : public Sensor { public: diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index e760bfbd9d..8624501cb2 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -70,7 +70,6 @@ static constexpr uint64_t BARO_MAX_INTERVAL = 200e3; ///< Maximum allowable 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 RNG_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between range finder measurements (uSec) static constexpr uint64_t MAG_MAX_INTERVAL = 500e3; ///< Maximum allowable time interval between magnetic field measurements (uSec) // bad accelerometer detection and mitigation @@ -197,12 +196,6 @@ struct baroSample { bool reset{false}; }; -struct rangeSample { - uint64_t time_us{}; ///< timestamp of the measurement (uSec) - float rng{}; ///< range (distance to ground) measurement (m) - int8_t quality{}; ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality. -}; - struct airspeedSample { uint64_t time_us{}; ///< timestamp of the measurement (uSec) float true_airspeed{}; ///< true airspeed measurement (m/sec) diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 980328f820..8bc021feed 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -266,7 +266,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_RANGE_FINDER) -void EstimatorInterface::setRangeData(const rangeSample &range_sample) +void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample) { if (!_initialised) { return; @@ -274,7 +274,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample) // Allocate the required buffer size if not previously done if (_range_buffer == nullptr) { - _range_buffer = new RingBuffer(_obs_buffer_length); + _range_buffer = new RingBuffer(_obs_buffer_length); if (_range_buffer == nullptr || !_range_buffer->valid()) { delete _range_buffer; @@ -291,7 +291,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample) // limit data rate to prevent data being lost if (time_us >= static_cast(_range_buffer->get_newest().time_us + _min_obs_interval_us)) { - rangeSample range_sample_new{range_sample}; + sensor::rangeSample range_sample_new{range_sample}; range_sample_new.time_us = time_us; _range_buffer->push(range_sample_new); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index e94d14b5e6..5c757f7214 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -67,8 +67,8 @@ #include "output_predictor.h" #if defined(CONFIG_EKF2_RANGE_FINDER) -# include "range_finder_consistency_check.hpp" -# include "sensor_range_finder.hpp" +# include "aid_sources/range_finder/range_finder_consistency_check.hpp" +# include "aid_sources/range_finder/sensor_range_finder.hpp" #endif // CONFIG_EKF2_RANGE_FINDER #include @@ -107,7 +107,7 @@ public: #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_RANGE_FINDER) - void setRangeData(const rangeSample &range_sample); + void setRangeData(const estimator::sensor::rangeSample &range_sample); // set sensor limitations reported by the rangefinder void set_rangefinder_limits(float min_distance, float max_distance) @@ -115,7 +115,7 @@ public: _range_sensor.setLimits(min_distance, max_distance); } - const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); } + const estimator::sensor::rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); } #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -356,7 +356,7 @@ protected: #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_RANGE_FINDER) - RingBuffer *_range_buffer{nullptr}; + RingBuffer *_range_buffer{nullptr}; uint64_t _time_last_range_buffer_push{0}; sensor::SensorRangeFinder _range_sensor{}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index f531d99ce7..7cfefd9d2c 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2368,7 +2368,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) int8_t quality = static_cast(optical_flow.quality) / static_cast(UINT8_MAX) * 100.f; - rangeSample range_sample { + estimator::sensor::rangeSample range_sample { .time_us = optical_flow.timestamp_sample, .rng = optical_flow.distance_m, .quality = quality, @@ -2507,7 +2507,7 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) { // EKF range sample if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) { - rangeSample range_sample { + estimator::sensor::rangeSample range_sample { .time_us = distance_sensor.timestamp, .rng = distance_sensor.current_distance, .quality = distance_sensor.signal_quality, diff --git a/src/modules/ekf2/test/sensor_simulator/range_finder.h b/src/modules/ekf2/test/sensor_simulator/range_finder.h index 0dcd068232..c4910ed57c 100644 --- a/src/modules/ekf2/test/sensor_simulator/range_finder.h +++ b/src/modules/ekf2/test/sensor_simulator/range_finder.h @@ -55,7 +55,7 @@ public: void setLimits(float min_distance_m, float max_distance_m); private: - rangeSample _range_sample{}; + estimator::sensor::rangeSample _range_sample{}; float _min_distance{0.2f}; float _max_distance{20.0f}; diff --git a/src/modules/ekf2/test/test_SensorRangeFinder.cpp b/src/modules/ekf2/test/test_SensorRangeFinder.cpp index 4a3b3c2b52..fc515a8757 100644 --- a/src/modules/ekf2/test/test_SensorRangeFinder.cpp +++ b/src/modules/ekf2/test/test_SensorRangeFinder.cpp @@ -34,10 +34,10 @@ #include #include #include "EKF/common.h" -#include "EKF/sensor_range_finder.hpp" +#include "EKF/aid_sources/range_finder/sensor_range_finder.hpp" #include -using estimator::rangeSample; +using estimator::sensor::rangeSample; using matrix::Dcmf; using matrix::Eulerf; using namespace estimator::sensor;