diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp index 35cfd1cfdb..607a589880 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp @@ -79,7 +79,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) // Gate sample consumption on these checks bool quality_ok = sample.quality > 0; // TODO: what about unknown? (-1) bool tilt_ok = _range_sensor.isTiltOk(); - bool range_ok = sample.rng <= _range_sensor.getValidMaxVal() && sample.rng >= _range_sensor.getValidMinVal(); + bool range_ok = sample.range <= _range_sensor.getValidMaxVal() && sample.range >= _range_sensor.getValidMinVal(); // - Not stuck value // - Not fog detected @@ -112,7 +112,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) // Correct the range data for position offset relative to the IMU const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - sample.rng = sample.rng + pos_offset_earth(2) / _range_sensor.getCosTilt(); + sample.range = sample.range + pos_offset_earth(2) / _range_sensor.getCosTilt(); // Provide sample from buffer to object _range_sensor.setSample(sample); 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 0e92012f1c..15eaf2c524 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 @@ -51,7 +51,7 @@ namespace sensor struct rangeSample { uint64_t time_us{}; ///< timestamp of the measurement (uSec) - float rng{}; ///< range (distance to ground) measurement (m) + float range{}; ///< 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. }; @@ -97,7 +97,7 @@ public: // void setRange(float rng) { _sample.rng = rng; } // float getRange() const { return _sample.rng; } - float getDistBottom() const { return _sample.rng * _cos_tilt_rng_to_earth; } + float getDistBottom() const { return _sample.range * _cos_tilt_rng_to_earth; } float getValidMinVal() const { return _rng_valid_min_val; } float getValidMaxVal() const { return _rng_valid_max_val; } diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 1991fc5866..b118c367ab 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -106,8 +106,8 @@ void Ekf::initialiseCovariance() #if defined(CONFIG_EKF2_TERRAIN) // use the ground clearance value as our uncertainty - // TODO: ^ why? - P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.ekf2_min_rng)); + // TODO: ^ why? wouldn't the noise value make more sense? + P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.ekf2_rng_noise)); #endif // CONFIG_EKF2_TERRAIN } diff --git a/src/modules/ekf2/EKF/terrain_control.cpp b/src/modules/ekf2/EKF/terrain_control.cpp index 93d807352e..4149a962dd 100644 --- a/src/modules/ekf2/EKF/terrain_control.cpp +++ b/src/modules/ekf2/EKF/terrain_control.cpp @@ -42,12 +42,12 @@ void Ekf::initTerrain() { - // JAKE: review // assume a ground clearance _state.terrain = -_gpos.altitude() + _params.ekf2_min_rng; // use the ground clearance value as our uncertainty - P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.ekf2_min_rng)); + // TODO: JAKE: ^ doesn't make any sense + P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.ekf2_rng_noise)); } void Ekf::controlTerrainFakeFusion() diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 3fcf7e1d96..3f21b86bb8 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2401,7 +2401,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) estimator::sensor::rangeSample range_sample { .time_us = optical_flow.timestamp_sample, - .rng = optical_flow.distance_m, + .range = optical_flow.distance_m, .quality = quality, }; _ekf.setRangeData(range_sample); @@ -2576,7 +2576,7 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) { estimator::sensor::rangeSample range_sample { .time_us = distance_sensor.timestamp, - .rng = distance_sensor.current_distance, + .range = distance_sensor.current_distance, .quality = distance_sensor.signal_quality, }; _ekf.setRangeData(range_sample);