From 32c1364c277d54239577497dd10e36c5534083d3 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 22 Sep 2025 19:52:32 -0800 Subject: [PATCH] simplify rotation logic, fix timeout check --- .../aid_sources/range_finder/range_height_control.cpp | 11 +++++------ .../aid_sources/range_finder/sensor_range_finder.cpp | 7 ------- .../aid_sources/range_finder/sensor_range_finder.hpp | 3 --- 3 files changed, 5 insertions(+), 16 deletions(-) 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 1040bc5fa6..03a250e259 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 @@ -46,9 +46,6 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) return; } - // Apply rotation to earth frame - _range_sensor.updateSensorToEarthRotation(_R_to_earth); - // TODO: why isn't this being done anywhere? _aid_src_rng_hgt.fused = false; @@ -56,8 +53,10 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) rangeSample sample = {}; if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) { - // Check if sample is timed out (200ms) - if (imu_sample.time_us > sample.time_us + 200'000) { + // no new sample available, check if we've timed out + bool had_sample = _range_time_last_good_sample > 0; + bool timed_out = imu_sample.time_us > _range_time_last_good_sample + 200'000; + if (had_sample && timed_out) { stopRangeAltitudeFusion("sensor timed out"); stopRangeTerrainFusion("sensor timed out"); } @@ -107,7 +106,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.range = sample.range + pos_offset_earth(2) / _range_sensor.getCosTilt(); + sample.range = sample.range + pos_offset_earth(2) / _R_to_earth(2, 2); // rotate into earth frame // Provide sample from buffer to object _range_sensor.setSample(sample); diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp index 9bfe7bab0d..9a79ba0e38 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp @@ -56,11 +56,4 @@ void SensorRangeFinder::setLimits(float min_distance, float max_distance) _max_distance = max_distance; } -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 - _cos_tilt_rng_to_earth = R_to_earth(2, 2) * -1; // sensor points straight down -} - } // namespace estimator 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 5c2c9a8da6..7ab9d614ec 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 @@ -66,13 +66,10 @@ public: void setSample(const rangeSample &sample); void setLimits(float min_distance, float max_distance); - float getCosTilt() const { return _cos_tilt_rng_to_earth; } float getDistBottom() const { return _sample.range * _cos_tilt_rng_to_earth; } float getValidMinVal() const { return _min_distance; } float getValidMaxVal() const { return _max_distance; } - void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth); - bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; } private: