fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-13 02:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

change quality check to time based, refactor and clean up

Browse Source
This commit is contained in:
Jacob Dahl
2025-09-22 18:18:42 -08:00
parent 75f3768be6
commit 613a784ce2
5 changed files with 82 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+57 -40
View File
@@ -46,22 +46,6 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
return;
}
// TODO: why isn't this being done anywhere?
_aid_src_rng_hgt.fused = false;
// Pop rangefinder measurement from buffer of samples into active sample
sensor::rangeSample sample = {};
if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) {
if (_range_sensor.timedOut(imu_sample.time_us)) {
stopRangeAltitudeFusion("sensor timed out");
stopRangeTerrainFusion("sensor timed out");
}
return;
}
// Set the raw sample
_range_sensor.setSample(sample);
// TODO: move setting params to init function
_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
float cosine_max_tilt = 0.9659; // 10 degrees
@@ -69,20 +53,54 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
_rng_consistency_check.setGate(_params.ekf2_rng_k_gate);
_range_sensor.updateSensorToEarthRotation(_R_to_earth);
bool quality_ok = sample.quality > 0;
// TODO: why isn't this being done anywhere?
_aid_src_rng_hgt.fused = false;
// While on ground fake a measurement at ground level if the signal quality is bad
if (!quality_ok && !_control_status.flags.in_air) {
sample.quality = 100;
quality_ok = true;
sample.range = _range_sensor.getValidMinVal();
// Pop rangefinder measurement from buffer of samples into active sample
sensor::rangeSample sample = {};
if (!_range_buffer->pop_first_older_than(imu_sample.time_us, &sample)) {
if (_range_sensor.timedOut(imu_sample.time_us)) {
stopRangeAltitudeFusion("sensor timed out");
stopRangeTerrainFusion("sensor timed out");
}
return;
}
// Set the raw sample
_range_sensor.setSample(sample);
// Quality checks
if (sample.quality == 0) {
if (_control_status.flags.in_air) {
// Disable fusion after 1s of bad quality
uint64_t elapsed = sample.time_us - _range_time_last_good_sample;
if (elapsed > 1'000'000) {
stopRangeAltitudeFusion("sensor quality bad");
stopRangeTerrainFusion("sensor quality bad");
}
return;
} else {
// While on ground fake a measurement at ground level if the signal quality is bad
sample.quality = 100;
sample.range = _range_sensor.getValidMinVal();
_range_time_last_good_sample = sample.time_us;
}
} else {
_range_time_last_good_sample = sample.time_us;
}
// Tilt and range checks
bool tilt_ok = _range_sensor.isTiltOk();
bool range_ok = sample.range <= _range_sensor.getValidMaxVal() && sample.range >= _range_sensor.getValidMinVal();
// If quality, tilt, or value are outside of bounds -- throw away measurement
if (!quality_ok || !tilt_ok || !range_ok) {
// If tilt or value are outside of bounds -- throw away measurement
if (!tilt_ok || !range_ok) {
stopRangeAltitudeFusion("pre checks failed");
stopRangeTerrainFusion("pre checks failed");
return;
@@ -125,6 +143,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
// NOTE: terrain is not estimated in this mode
if (_params.ekf2_hgt_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
fuseRangeAsHeightSource();
} else {
// Fuse Range data as aiding height source (Primary GPS or Baro)
fuseRangeAsHeightAiding();
@@ -133,13 +152,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
void Ekf::fuseRangeAsHeightAiding()
{
// ISSUES: 6/25/2025
// - optical flow terrain fucks everything up, I've hardcoded disabled it
// - when rng fusion starts again, EKF Z state is corrupted (oscillation)
// -
const char* kNotKinematicallyConsistentText = "not kinematically consistent";
const char* kConditionsFailingText = "conditions failing";
const char *kNotKinematicallyConsistentText = "not kinematically consistent";
const char *kConditionsFailingText = "conditions failing";
// Stop fusion if rangefinder kinematic consistency fails
if (!_control_status.flags.rng_kin_consistent) {
@@ -148,7 +162,6 @@ void Ekf::fuseRangeAsHeightAiding()
return;
}
//// TERRAIN FUSION ////
// Fuse Range into Terrain if:
@@ -208,6 +221,7 @@ void Ekf::fuseRangeAsHeightAiding()
fuseHaglRng(_aid_src_rng_hgt, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
}
// TODO: remove this mode entirely
void Ekf::fuseRangeAsHeightSource()
{
// When primary height source is RANGE, we always fuse it
@@ -280,6 +294,7 @@ bool Ekf::rangeAidConditionsPassed()
// Conditions have been valid for at least 1s
conditions_passing = true;
}
} else {
if (_rng_aid_conditions_valid) {
@@ -292,6 +307,7 @@ bool Ekf::rangeAidConditionsPassed()
if (!is_below_max_speed) {
ECL_INFO("!is_below_max_speed");
}
if (!is_in_range) {
ECL_INFO("!is_in_range");
}
@@ -345,12 +361,12 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f);
updateAidSourceStatus(aid_src,
_range_sensor.sample()->time_us, // sample timestamp
measurement, // observation
measurement_variance, // observation variance
getHagl() - measurement, // innovation
innovation_variance, // innovation variance
innov_gate); // innovation gate
_range_sensor.sample()->time_us, // sample timestamp
measurement, // observation
measurement_variance, // observation variance
getHagl() - measurement, // innovation
innovation_variance, // innovation variance
innov_gate); // 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
@@ -368,18 +384,19 @@ float Ekf::getRngVar() const
return dist_var;
}
void Ekf::stopRangeAltitudeFusion(const char* reason)
void Ekf::stopRangeAltitudeFusion(const char *reason)
{
if (_control_status.flags.rng_hgt) {
ECL_INFO("STOP RNG Altitude fusion: %s", reason);
_control_status.flags.rng_hgt = false;
if (_height_sensor_ref == HeightSensor::RANGE) {
_height_sensor_ref = HeightSensor::UNKNOWN;
}
}
}
void Ekf::stopRangeTerrainFusion(const char* reason)
void Ekf::stopRangeTerrainFusion(const char *reason)
{
if (_control_status.flags.rng_terrain) {
ECL_INFO("STOP RNG Terrain fusion: %s", reason);
src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp
+2 -2
View File
@@ -73,8 +73,8 @@ void SensorRangeFinder::setPitchOffset(float new_pitch_offset)
void SensorRangeFinder::setLimits(float min_distance, float max_distance)
{
_rng_valid_min_val = min_distance;
_rng_valid_max_val = max_distance;
_min_distance = min_distance;
_max_distance = max_distance;
}
void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth)
src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp
+18 -34
View File
@@ -50,12 +50,11 @@ namespace sensor
{
struct rangeSample {
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
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.
uint64_t time_us{}; // timestamp of the measurement (uSec)
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.
};
class SensorRangeFinder
{
public:
@@ -76,35 +75,25 @@ public:
float range_cos_max_tilt{0.7071f}; // 45 degrees max tilt
};
void updateParameters(Parameters& parameters) { _parameters = parameters; };
bool timedOut(uint64_t time_now) const;
void setSample(const rangeSample &sample);
// 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; }
float getCosTilt() const { return _cos_tilt_rng_to_earth; }
void setLimits(float min_distance, float max_distance);
// void setRange(float rng) { _sample.rng = rng; }
// float getRange() const { return _sample.rng; }
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; }
float getValidMinVal() const { return _rng_valid_min_val; }
float getValidMaxVal() const { return _rng_valid_max_val; }
void updateParameters(Parameters &parameters) { _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:
@@ -112,21 +101,16 @@ private:
Parameters _parameters{};
/*
* 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 _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
/*
* Range check
*/
float _rng_valid_min_val{}; ///< minimum distance that the rangefinder can measure (m)
float _rng_valid_max_val{}; ///< maximum distance that the rangefinder can measure (m)
// 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 _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
// Range check
float _min_distance{}; // minimum distance that the rangefinder can measure (m)
float _max_distance{}; // maximum distance that the rangefinder can measure (m)
};
} // namespace sensor
src/modules/ekf2/EKF/ekf.h
+2 -2
View File
@@ -783,8 +783,8 @@ private:
void fuseRangeAsHeightAiding();
bool isConditionalRangeAidSuitable();
bool rangeAidConditionsPassed();
void stopRangeAltitudeFusion(const char* reason);
void stopRangeTerrainFusion(const char* reason);
void stopRangeAltitudeFusion(const char *reason);
void stopRangeTerrainFusion(const char *reason);
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
src/modules/ekf2/EKF/estimator_interface.h
+3 -2
View File
@@ -376,8 +376,9 @@ protected:
uint64_t _time_last_range_buffer_push{0};
// Range aiding
bool _rng_aid_conditions_valid{false};
uint64_t _time_rng_aid_conditions_valid{0};
bool _rng_aid_conditions_valid{};
uint64_t _time_rng_aid_conditions_valid{};
uint64_t _range_time_last_good_sample{};
sensor::SensorRangeFinder _range_sensor{};
RangeFinderConsistencyCheck _rng_consistency_check;