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 239c415d69..4c6c585cb3 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 @@ -47,9 +47,6 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } // TODO: move setting params to init function - _range_sensor.setPitchOffset(_params.ekf2_rng_pitch); - float cosine_max_tilt = 0.9659; // 10 degrees - _range_sensor.setCosMaxTilt(cosine_max_tilt); _rng_consistency_check.setGate(_params.ekf2_rng_k_gate); _range_sensor.updateSensorToEarthRotation(_R_to_earth); @@ -57,10 +54,11 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _aid_src_rng_hgt.fused = false; // Pop rangefinder measurement from buffer of samples into active sample - sensor::rangeSample sample = {}; + rangeSample sample = {}; if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) { - if (_range_sensor.timedOut(imu_sample.time_us)) { + // Check if sample is timed out (200ms) + if (imu_sample.time_us > sample.time_us + 200'000) { stopRangeAltitudeFusion("sensor timed out"); stopRangeTerrainFusion("sensor timed out"); } 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 dc072b19ce..9bfe7bab0d 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 @@ -44,33 +44,12 @@ namespace estimator { -namespace sensor -{ void SensorRangeFinder::setSample(const rangeSample &sample) { _sample = sample; } -bool SensorRangeFinder::timedOut(uint64_t time_now) const -{ - if (_sample.time_us > time_now) { - return false; - } - - // TODO: 200ms? - return time_now > _sample.time_us + 200'000; -} - -void SensorRangeFinder::setPitchOffset(float new_pitch_offset) -{ - if (fabsf(_pitch_offset_rad - new_pitch_offset) > FLT_EPSILON) { - _sin_pitch_offset = sinf(new_pitch_offset); - _cos_pitch_offset = cosf(new_pitch_offset); - _pitch_offset_rad = new_pitch_offset; - } -} - void SensorRangeFinder::setLimits(float min_distance, float max_distance) { _min_distance = min_distance; @@ -81,8 +60,7 @@ void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_ear { // 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, 0) * _sin_pitch_offset + R_to_earth(2, 2) * _cos_pitch_offset; + _cos_tilt_rng_to_earth = R_to_earth(2, 2) * -1; // sensor points straight down } -} // namespace sensor } // 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 abae5760dc..5c2c9a8da6 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 @@ -46,8 +46,6 @@ namespace estimator { -namespace sensor -{ struct rangeSample { uint64_t time_us{}; // timestamp of the measurement (uSec) @@ -61,27 +59,11 @@ public: SensorRangeFinder() = default; ~SensorRangeFinder() = default; - struct Parameters { - float ekf2_imu_pos_x{}; - float ekf2_imu_pos_y{}; - float ekf2_imu_pos_z{}; - - float ekf2_rng_pos_x{}; - float ekf2_rng_pos_y{}; - float ekf2_rng_pos_z{}; - - float ekf2_rng_pitch{}; - - float range_cos_max_tilt{0.7071f}; // 45 degrees max tilt - }; - // This is required because of the ring buffer // TODO: move the ring buffer here rangeSample *sample() { return &_sample; } void setSample(const rangeSample &sample); - void setPitchOffset(float new_pitch_offset); - void setCosMaxTilt(float cos_max_tilt) { _range_cos_max_tilt = cos_max_tilt; } void setLimits(float min_distance, float max_distance); float getCosTilt() const { return _cos_tilt_rng_to_earth; } @@ -89,22 +71,19 @@ public: float getValidMinVal() const { return _min_distance; } float getValidMaxVal() const { return _max_distance; } - void updateParameters(Parameters ¶meters) { _parameters = parameters; }; void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth); bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; } - bool timedOut(uint64_t time_now) const; private: - rangeSample _sample{}; + float range_cos_max_tilt{0.7071f}; // 45 degrees max tilt - Parameters _parameters{}; + rangeSample _sample{}; // Tilt check float _cos_tilt_rng_to_earth{1.f}; // 2,2 element of the rotation matrix from sensor frame to earth frame - float _range_cos_max_tilt{0.7071f}; // cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data - float _pitch_offset_rad{3.14f}; // range finder tilt rotation about the Y body axis + float _range_cos_max_tilt{0.9659}; // 10 degrees. Cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data float _sin_pitch_offset{0.0f}; // sine of the range finder tilt rotation about the Y body axis float _cos_pitch_offset{-1.0f}; // cosine of the range finder tilt rotation about the Y body axis @@ -113,6 +92,5 @@ private: float _max_distance{}; // maximum distance that the rangefinder can measure (m) }; -} // namespace sensor } // namespace estimator #endif // !EKF_SENSOR_RANGE_FINDER_HPP diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 998495981a..a205b87db6 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -419,7 +419,6 @@ struct parameters { float ekf2_rng_delay{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) float ekf2_rng_noise{0.1f}; ///< observation noise for range finder measurements (m) float ekf2_rng_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD) - float ekf2_rng_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis. float ekf2_rng_sfe{0.0f}; ///< scaling from range measurement to noise (m/m) float ekf2_rng_a_hmax{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1) float ekf2_rng_a_vmax{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 11b7666cdd..27c111a7b5 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -402,17 +402,8 @@ void Ekf::updateParameters() _aux_global_position.updateParameters(); #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION #if defined (CONFIG_EKF2_RANGE_FINDER) + // TODO: set the tilt/rotation shit - sensor::SensorRangeFinder::Parameters params = {}; - // TODO: - // params.ekf2_imu_pos_x = _param_ekf2_imu_pos_x.get(); - // params.ekf2_imu_pos_y = _param_ekf2_imu_pos_y.get(); - // params.ekf2_imu_pos_z = _param_ekf2_imu_pos_z.get(); - // params.ekf2_rng_pos_x = _param_ekf2_rng_pos_x.get(); - // params.ekf2_rng_pos_y = _param_ekf2_rng_pos_y.get(); - // params.ekf2_rng_pos_z = _param_ekf2_rng_pos_z.get(); - params.ekf2_rng_pitch = _params.ekf2_rng_pitch; - _range_sensor.updateParameters(params); #endif } diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 5c30c72afc..7c18f9def0 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -283,7 +283,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_RANGE_FINDER) -void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample) +void EstimatorInterface::setRangeData(const rangeSample &range_sample) { if (!_initialised) { return; @@ -291,7 +291,7 @@ void EstimatorInterface::setRangeData(const sensor::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; @@ -308,7 +308,7 @@ void EstimatorInterface::setRangeData(const sensor::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)) { - sensor::rangeSample range_sample_new{range_sample}; + 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 780bdb2378..834584121c 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -113,7 +113,7 @@ public: #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_RANGE_FINDER) - void setRangeData(const estimator::sensor::rangeSample &range_sample); + void setRangeData(const estimator::rangeSample &range_sample); // set sensor limitations reported by the rangefinder void set_rangefinder_limits(float min_distance, float max_distance) @@ -372,7 +372,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}; // Range aiding @@ -380,7 +380,7 @@ protected: uint64_t _time_rng_aid_conditions_valid{}; uint64_t _range_time_last_good_sample{}; - sensor::SensorRangeFinder _range_sensor{}; + SensorRangeFinder _range_sensor{}; RangeFinderConsistencyCheck _rng_consistency_check; #endif // CONFIG_EKF2_RANGE_FINDER diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index a40127f616..b1f5811af2 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -159,7 +159,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_noise(_params->ekf2_rng_noise), _param_ekf2_rng_sfe(_params->ekf2_rng_sfe), _param_ekf2_rng_gate(_params->ekf2_rng_gate), - _param_ekf2_rng_pitch(_params->ekf2_rng_pitch), _param_ekf2_rng_a_vmax(_params->ekf2_rng_a_vmax), _param_ekf2_rng_a_hmax(_params->ekf2_rng_a_hmax), _param_ekf2_rng_k_gate(_params->ekf2_rng_k_gate), @@ -2399,7 +2398,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) int8_t quality = static_cast(optical_flow.quality) / static_cast(UINT8_MAX) * 100.f; - estimator::sensor::rangeSample range_sample { + estimator::rangeSample range_sample { .time_us = optical_flow.timestamp_sample, .range = optical_flow.distance_m, .quality = quality, @@ -2574,7 +2573,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) { - estimator::sensor::rangeSample range_sample { + estimator::rangeSample range_sample { .time_us = distance_sensor.timestamp, .range = distance_sensor.current_distance, .quality = distance_sensor.signal_quality, diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 53e818ac2c..4e04e22886 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -618,7 +618,6 @@ private: (ParamExtFloat) _param_ekf2_rng_noise, (ParamExtFloat) _param_ekf2_rng_sfe, (ParamExtFloat) _param_ekf2_rng_gate, - (ParamExtFloat) _param_ekf2_rng_pitch, (ParamExtFloat) _param_ekf2_rng_a_vmax, (ParamExtFloat) _param_ekf2_rng_a_hmax, (ParamExtFloat) _param_ekf2_rng_k_gate, diff --git a/src/modules/ekf2/params_range_finder.yaml b/src/modules/ekf2/params_range_finder.yaml index 2697de8e5e..a619ae99c7 100644 --- a/src/modules/ekf2/params_range_finder.yaml +++ b/src/modules/ekf2/params_range_finder.yaml @@ -52,15 +52,6 @@ parameters: min: 0.01 unit: m decimal: 2 - EKF2_RNG_PITCH: - description: - short: Range sensor pitch offset - type: float - default: 0.0 - min: -0.75 - max: 0.75 - unit: rad - decimal: 3 EKF2_RNG_A_VMAX: description: short: Maximum horizontal velocity allowed for conditional range aid mode diff --git a/src/modules/ekf2/test/sensor_simulator/range_finder.h b/src/modules/ekf2/test/sensor_simulator/range_finder.h index c4910ed57c..838ee04599 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: - estimator::sensor::rangeSample _range_sample{}; + estimator::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 ed67c0c2dc..5b3fa5bce3 100644 --- a/src/modules/ekf2/test/test_SensorRangeFinder.cpp +++ b/src/modules/ekf2/test/test_SensorRangeFinder.cpp @@ -37,7 +37,7 @@ #include "EKF/aid_sources/range_finder/sensor_range_finder.hpp" #include -using estimator::sensor::rangeSample; +using estimator::rangeSample; using matrix::Dcmf; using matrix::Eulerf; using namespace estimator::sensor; @@ -48,8 +48,6 @@ public: // Setup the Ekf with synthetic measurements void SetUp() override { - _range_finder.setPitchOffset(0.f); - _range_finder.setCosMaxTilt(0.707f); _range_finder.setLimits(_min_range, _max_range); }