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

massive refactoring

Browse Source
This commit is contained in:
Jacob Dahl
2025-06-17 19:48:10 -08:00
parent 1b3ff1a7ec
commit ba8760a3d8
14 changed files with 142 additions and 623 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/CMakeLists.txt
+1 -1
View File
@@ -211,7 +211,7 @@ endif()
if(CONFIG_EKF2_RANGE_FINDER)
list(APPEND EKF_SRCS
EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
EKF/aid_sources/range_finder/range_height_control.cpp
EKF/aid_sources/range_finder/jake_range_height_control.cpp
EKF/aid_sources/range_finder/range_height_fusion.cpp
EKF/aid_sources/range_finder/sensor_range_finder.cpp
)
src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp
-84
View File
@@ -1,84 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Sensor.hpp
* Abstract class for sensors
*
* @author Mathieu Bresciani <brescianimathieu@gmail.com>
*
*/
#ifndef EKF_SENSOR_HPP
#define EKF_SENSOR_HPP
#include <cstdint>
namespace estimator
{
namespace sensor
{
class Sensor
{
public:
virtual ~Sensor() {};
/*
* run sanity checks on the current data
* this has to be called immediately after
* setting new data
*/
virtual void runChecks() {};
/*
* return true if the sensor is healthy
*/
virtual bool isHealthy() const = 0;
/*
* return true if the delayed sample is healthy
* and can be fused in the estimator
*/
virtual bool isDataHealthy() const = 0;
/*
* return true if the sensor data rate is
* stable and high enough
*/
virtual bool isRegularlySendingData() const = 0;
};
} // namespace sensor
} // namespace estimator
#endif // !EKF_SENSOR_HPP
src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp
+49 -291
View File
@@ -42,316 +42,74 @@
void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
{
static constexpr const char *HGT_SRC_NAME = "RNG";
// Check if rangefinder is available/enabled
if (!_range_buffer) {
return;
}
// Get range data from buffer and check validity
bool rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress());
_range_sensor.setDataReadiness(rng_data_ready);
bool sample_valid = 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.isDataReady()) {
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
// Set all of the parameters
_range_sensor.setPitchOffset(_params.ekf2_rng_pitch);
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
_range_sensor.setMaxFogDistance(_params.rng_fog);
_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
// Update sensor to earth rotation
_range_sensor.updateSensorToEarthRotation(_R_to_earth);
if (_range_sensor.isDataHealthy()) {
// correct the range data for position offset relative to the IMU
const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
// Gate sample consumption on these checks
// - Quality is OK
bool quality_ok = sample.quality > 0;
// - Tilt is OK
bool tilt_ok = _range_sensor.isTiltOk();
// - Value is IN range
bool range_ok = sample.rng <= _range_sensor.getValidMaxVal() && sample.rng >= _range_sensor.getValidMinVal();
// - Not stuck value
// - Not fog detected
if (quality_ok && tilt_ok && range_ok) {
// Correct the range data for position offset relative to the IMU
const Vector3f rng_pos_body = { _params.ekf2_rng_pos_x, _params.ekf2_rng_pos_y, _params.ekf2_rng_pos_z };
// const Vector3f imu_pos_body = { _params.ekf2_imu_pos_x, _params.ekf2_imu_pos_y, _params.ekf2_imu_pos_z };
const Vector3f imu_pos_body = _params.imu_pos_body;
const Vector3f pos_offset_body = rng_pos_body - imu_pos_body;
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
sample.rng = sample.rng + pos_offset_earth(2) / _range_sensor.getCosTilt();
// TODO: this is a constant
const float dist_var = getRngVar();
_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
// Provide sample from buffer to object
_range_sensor.setSample(sample);
const float z_var = P(State::pos.idx + 2, State::pos.idx + 2);
const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2);
// Check kinematic consistency of rangefinder measurement w.r.t Altitude Estimate
// TODO: review -- variance
_rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(),
dist_var, imu_sample.time_us);
} else if (_range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) {
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setValidity(true);
} else {
_rng_consistency_check.reset();
// Passes kinematic consistency check?
sample_valid = true;
}
}
_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
_control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent()
&& _rng_consistency_check.isNotKinematicallyInconsistent();
auto &aid_src = _aid_src_rng_hgt;
if (_range_sensor.isDataReady() && _range_sensor.getSampleAddress()) {
updateRangeHagl(aid_src);
const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
|| (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
&& _control_status.flags.tilt_align
&& measurement_valid
&& _range_sensor.isDataHealthy()
&& _rng_consistency_check.isNotKinematicallyInconsistent();
// SUS: _rng_consistency_check.isNotKinematicallyInconsistent()
const bool starting_conditions_passing = continuing_conditions_passing
&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
&& _range_sensor.isRegularlySendingData();
const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
&& (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
&& isConditionalRangeAidSuitable();
// SUS: isConditionalRangeAidSuitable()
const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt)
&& (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED));
if (_control_status.flags.rng_hgt) {
if (!(do_conditional_range_aid || do_range_aid)) {
PX4_INFO("stopping %s fusion", HGT_SRC_NAME);
stopRngHgtFusion();
}
} else {
if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
if (do_conditional_range_aid) {
// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
PX4_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
_height_sensor_ref = HeightSensor::RANGE;
_control_status.flags.rng_hgt = true;
stopRngTerrFusion();
if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected
&& _rng_consistency_check.isKinematicallyConsistent()) {
resetTerrainToRng(aid_src);
resetAidSourceStatusZeroInnovation(aid_src);
}
} else if (do_range_aid) {
// Range finder is the primary height source, the ground is now the datum used
// to compute the local vertical position
PX4_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
_height_sensor_ref = HeightSensor::RANGE;
_information_events.flags.reset_hgt_to_rng = true;
resetAltitudeTo(aid_src.observation, aid_src.observation_variance);
_state.terrain = 0.f;
resetAidSourceStatusZeroInnovation(aid_src);
_control_status.flags.rng_hgt = true;
stopRngTerrFusion();
aid_src.time_last_fuse = imu_sample.time_us;
}
} else {
if (do_conditional_range_aid || do_range_aid) {
PX4_INFO("starting %s height fusion", HGT_SRC_NAME);
_control_status.flags.rng_hgt = true;
if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected
&& _rng_consistency_check.isKinematicallyConsistent()) {
resetTerrainToRng(aid_src);
resetAidSourceStatusZeroInnovation(aid_src);
}
}
}
}
if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) {
if (continuing_conditions_passing) {
fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) {
// All height sources are failing
PX4_INFO("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
_information_events.flags.reset_hgt_to_rng = true;
resetAltitudeTo(aid_src.observation - _state.terrain);
resetAidSourceStatusZeroInnovation(aid_src);
// reset vertical velocity if no valid sources available
if (!isVerticalVelocityAidingActive()) {
resetVerticalVelocityToZero();
}
aid_src.time_last_fuse = imu_sample.time_us;
} else if (is_fusion_failing) {
// Some other height source is still working
if (_control_status.flags.opt_flow_terrain && isTerrainEstimateValid()) {
PX4_INFO("stopping %s fusion, fusion failing", HGT_SRC_NAME);
stopRngHgtFusion();
stopRngTerrFusion();
} else if (_rng_consistency_check.isKinematicallyConsistent()) {
resetTerrainToRng(aid_src);
resetAidSourceStatusZeroInnovation(aid_src);
}
}
} else {
PX4_INFO("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
stopRngHgtFusion();
stopRngTerrFusion();
}
} else {
if (starting_conditions_passing) {
if (_control_status.flags.opt_flow_terrain) {
if (!aid_src.innovation_rejected) {
_control_status.flags.rng_terrain = true;
fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain);
}
} else {
if (aid_src.innovation_rejected && _rng_consistency_check.isKinematicallyConsistent()) {
resetTerrainToRng(aid_src);
resetAidSourceStatusZeroInnovation(aid_src);
}
_control_status.flags.rng_terrain = true;
}
}
}
} else if ((_control_status.flags.rng_hgt || _control_status.flags.rng_terrain)
&& !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) {
// No data anymore. Stop until it comes back.
PX4_INFO("stopping %s fusion, no data", HGT_SRC_NAME);
stopRngHgtFusion();
stopRngTerrFusion();
}
}
void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
{
const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
const float measurement_variance = getRngVar();
float innovation_variance;
sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
const float innov_gate = math::max(_params.range_innov_gate, 1.f);
updateAidSourceStatus(aid_src,
_range_sensor.getSampleAddress()->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
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
aid_src.innovation_rejected = false;
}
}
float Ekf::getRngVar() const
{
const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
const float dist_var = sq(_params.range_noise) + dist_dependant_var;
return dist_var;
}
void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
{
// Since the distance is not a direct observation of the terrain state but is based
// on the height state, a reset should consider the height uncertainty. This can be
// done by manipulating the Kalman gain to inject all the innovation in the terrain state
// and create the correct correlation with the terrain state with a covariance update.
P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 0.f);
const float old_terrain = _state.terrain;
VectorState H;
sym::ComputeHaglH(&H);
VectorState K;
K(State::terrain.idx) = 1.f; // innovation is forced into the terrain state to create a "reset"
measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
// record the state change
const float delta_terrain = _state.terrain - old_terrain;
if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
_state_reset_status.hagl_change = delta_terrain;
} else {
// there's already a reset this update, accumulate total delta
_state_reset_status.hagl_change += delta_terrain;
// Check if rangefinder has timed out
if (_range_sensor.timedOut(imu_sample.time_us)) {
// TODO: timeout value?
// TODO: handle timeout
}
_state_reset_status.reset_count.hagl++;
aid_src.time_last_fuse = _time_delayed_us;
}
bool Ekf::isConditionalRangeAidSuitable()
{
// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
float range_hagl_max = _params.max_hagl_for_range_aid;
float max_vel_xy = _params.max_vel_for_range_aid;
const float hagl_test_ratio = _aid_src_rng_hgt.test_ratio;
bool is_hagl_stable = (hagl_test_ratio < 1.f);
if (!_control_status.flags.rng_hgt) {
range_hagl_max = 0.7f * _params.max_hagl_for_range_aid;
max_vel_xy = 0.7f * _params.max_vel_for_range_aid;
is_hagl_stable = (hagl_test_ratio < 0.01f);
if (!sample_valid) {
// Sample is invalid
// - Timeout
// - Quality
// - Tilt
// - Range
// - KinematicConsistency
// - RangeAidChecks...
// - Horizontal Velocity
// - aid_src_rng_hgt.test_ratio
return;
}
const bool is_in_range = (getHagl() < range_hagl_max);
// Publish EstimatorAidSource1d (observation, variance, rejected, fused)
bool is_below_max_speed = true;
if (isHorizontalAidingActive()) {
is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy);
}
return is_in_range && is_hagl_stable && is_below_max_speed;
}
void Ekf::stopRngHgtFusion()
{
if (_control_status.flags.rng_hgt) {
if (_height_sensor_ref == HeightSensor::RANGE) {
_height_sensor_ref = HeightSensor::UNKNOWN;
}
_control_status.flags.rng_hgt = false;
}
}
void Ekf::stopRngTerrFusion()
{
_control_status.flags.rng_terrain = false;
// Conditionally enable/disable rangefinder fusion for Altitude Estimate and Terrain Estimate
}
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
+2 -5
View File
@@ -39,15 +39,12 @@
#include "ekf_derivation/generated/range_validation_filter_h.h"
#include "ekf_derivation/generated/range_validation_filter_P_init.h"
#include <px4_platform_common/log.h>
using namespace matrix;
void RangeFinderConsistencyCheck::init(const float z, const float z_var, const float dist_bottom,
const float dist_bottom_var)
{
PX4_INFO("RangeFinderConsistencyCheck::init");
printf("RangeFinderConsistencyCheck::init\n");
_P = sym::RangeValidationFilterPInit(z_var, dist_bottom_var);
_Ht = sym::RangeValidationFilterH<float>();
@@ -140,7 +137,7 @@ void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz,
}
if (fabsf(_test_ratio_lpf.getState()) > 1.f) {
PX4_INFO("_test_ratio_lpf failed (>1)");
printf("_test_ratio_lpf failed (>1)\n");
_t_since_first_sample = 0.f;
_state = KinematicState::INCONSISTENT;
return;
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+6 -4
View File
@@ -49,14 +49,16 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
}
// Get range data from buffer and check validity
bool rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress());
bool rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.sample());
_range_sensor.setSample();
_range_sensor.setDataReadiness(rng_data_ready);
if (_range_sensor.isDataReady()) {
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
_range_sensor.setMaxFogDistance(_params.rng_fog);
_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
@@ -96,7 +98,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
auto &aid_src = _aid_src_rng_hgt;
if (_range_sensor.isDataReady() && _range_sensor.getSampleAddress()) {
if (_range_sensor.isDataReady() && _range_sensor.sample()) {
updateRangeHagl(aid_src);
const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
@@ -254,7 +256,7 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
const float innov_gate = math::max(_params.range_innov_gate, 1.f);
updateAidSourceStatus(aid_src,
_range_sensor.getSampleAddress()->time_us, // sample timestamp
_range_sensor.sample()->time_us, // sample timestamp
measurement, // observation
measurement_variance, // observation variance
getHagl() - measurement, // innovation
src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp
+27 -113
View File
@@ -47,10 +47,34 @@ namespace estimator
namespace sensor
{
void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth)
void SensorRangeFinder::setSample(const rangeSample &sample)
{
updateSensorToEarthRotation(R_to_earth);
updateValidity(current_time_us);
_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)
{
_rng_valid_min_val = min_distance;
_rng_valid_max_val = max_distance;
}
void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth)
@@ -60,115 +84,5 @@ void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_ear
_cos_tilt_rng_to_earth = R_to_earth(2, 0) * _sin_pitch_offset + R_to_earth(2, 2) * _cos_pitch_offset;
}
void SensorRangeFinder::updateValidity(uint64_t current_time_us)
{
updateDtDataLpf(current_time_us);
if (_is_faulty || isSampleOutOfDate(current_time_us) || !isDataContinuous()) {
_is_sample_valid = false;
_is_regularly_sending_data = false;
return;
}
_is_regularly_sending_data = true;
// Don't run the checks unless we have retrieved new data from the buffer
if (_is_sample_ready) {
_is_sample_valid = false;
_time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us;
if (!isQualityOk(current_time_us) || !isTiltOk() || !isDataInRange()) {
return;
}
updateStuckCheck();
updateFogCheck(getDistBottom(), _sample.time_us);
if (!_is_stuck && !_is_blocked) {
_is_sample_valid = true;
_time_last_valid_us = _sample.time_us;
}
}
}
bool SensorRangeFinder::isQualityOk(uint64_t current_time_us) const
{
return current_time_us - _time_bad_quality_us > _quality_hyst_us;
}
void SensorRangeFinder::updateDtDataLpf(uint64_t current_time_us)
{
// Calculate a first order IIR low-pass filtered time of arrival between samples using a 2 second time constant.
float alpha = 0.5f * _dt_update;
_dt_data_lpf = _dt_data_lpf * (1.0f - alpha) + alpha * (current_time_us - _sample.time_us);
// Apply spike protection to the filter state.
_dt_data_lpf = fminf(_dt_data_lpf, 4e6f);
}
inline bool SensorRangeFinder::isSampleOutOfDate(uint64_t current_time_us) const
{
return (current_time_us - _sample.time_us) > 2 * RNG_MAX_INTERVAL;
}
inline bool SensorRangeFinder::isDataInRange() const
{
return (_sample.rng >= _rng_valid_min_val) && (_sample.rng <= _rng_valid_max_val);
}
void SensorRangeFinder::updateStuckCheck()
{
if (!isStuckDetectorEnabled()) {
// Stuck detector disabled
_is_stuck = false;
return;
}
// Check for "stuck" range finder measurements when range was not valid for certain period
// This handles a failure mode observed with some lidar sensors
if (((_sample.time_us - _time_last_valid_us) > (uint64_t)10e6)) {
// require a variance of rangefinder values to check for "stuck" measurements
if (_stuck_max_val - _stuck_min_val > _stuck_threshold) {
_stuck_min_val = 0.0f;
_stuck_max_val = 0.0f;
_is_stuck = false;
} else {
if (_sample.rng > _stuck_max_val) {
_stuck_max_val = _sample.rng;
}
if (_stuck_min_val < 0.1f || _sample.rng < _stuck_min_val) {
_stuck_min_val = _sample.rng;
}
_is_stuck = true;
}
}
}
void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t time_us)
{
if (_max_fog_dist > 0.f) {
const float median_dist = _median_dist.apply(dist_bottom);
const float factor = 2.f; // magic hardcoded factor
if (!_is_blocked && median_dist < _max_fog_dist && _prev_median_dist - median_dist > factor * _max_fog_dist) {
_is_blocked = true;
} else if (_is_blocked && median_dist > factor * _max_fog_dist) {
_is_blocked = false;
}
_prev_median_dist = median_dist;
} else {
_prev_median_dist = dist_bottom;
}
}
} // namespace sensor
} // namespace estimator
src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp
+29 -88
View File
@@ -41,8 +41,6 @@
#ifndef EKF_SENSOR_RANGE_FINDER_HPP
#define EKF_SENSOR_RANGE_FINDER_HPP
#include "Sensor.hpp"
#include <matrix/math.hpp>
#include <lib/mathlib/math/filter/MedianFilter.hpp>
@@ -57,105 +55,62 @@ struct rangeSample {
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
class SensorRangeFinder
{
public:
SensorRangeFinder() = default;
~SensorRangeFinder() override = default;
~SensorRangeFinder() = default;
void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth);
bool isHealthy() const override { return _is_sample_valid; }
bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; }
bool isDataReady() const { return _is_sample_ready; }
bool isRegularlySendingData() const override { return _is_regularly_sending_data; }
bool isStuckDetectorEnabled() const { return _stuck_threshold > 0.f; }
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
};
void updateParameters(Parameters& parameters) { _parameters = parameters; };
bool timedOut(uint64_t time_now) const;
void setSample(const rangeSample &sample);
void setSample(const rangeSample &sample)
{
_sample = sample;
_is_sample_ready = true;
}
// This is required because of the ring buffer
// TODO: move the ring buffer here
rangeSample *getSampleAddress() { return &_sample; }
rangeSample *sample() { return &_sample; }
void 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 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)
{
_rng_valid_min_val = min_distance;
_rng_valid_max_val = max_distance;
}
void setMaxFogDistance(float max_fog_dist) { _max_fog_dist = max_fog_dist; }
void setQualityHysteresis(float valid_quality_threshold_s)
{
_quality_hyst_us = uint64_t(valid_quality_threshold_s * 1e6f);
}
float getCosTilt() const { return _cos_tilt_rng_to_earth; }
void setRange(float rng) { _sample.rng = rng; }
float getRange() const { return _sample.rng; }
void setLimits(float min_distance, float max_distance);
// void setRange(float rng) { _sample.rng = rng; }
// float getRange() const { return _sample.rng; }
float getDistBottom() const { return _sample.rng * _cos_tilt_rng_to_earth; }
void setDataReadiness(bool is_ready) { _is_sample_ready = is_ready; }
void setValidity(bool is_valid) { _is_sample_valid = is_valid; }
float getValidMinVal() const { return _rng_valid_min_val; }
float getValidMaxVal() const { return _rng_valid_max_val; }
void setFaulty(bool faulty = true) { _is_faulty = faulty; }
private:
void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth);
void updateValidity(uint64_t current_time_us);
void updateDtDataLpf(uint64_t current_time_us);
bool isSampleOutOfDate(uint64_t current_time_us) const;
bool isDataContinuous() const { return _dt_data_lpf < 2e6f; }
bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; }
bool isDataInRange() const;
bool isQualityOk(uint64_t current_time_us) const;
void updateStuckCheck();
void updateFogCheck(const float dist_bottom, const uint64_t time_us);
private:
rangeSample _sample{};
bool _is_sample_ready{}; ///< true when new range finder data has fallen behind the fusion time horizon and is available to be fused
bool _is_sample_valid{}; ///< true if range finder sample retrieved from buffer is valid
bool _is_regularly_sending_data{false}; ///< true if the interval between two samples is less than the maximum expected interval
uint64_t _time_last_valid_us{}; ///< time the last range finder measurement was ready (uSec)
bool _is_faulty{false}; ///< the sensor should not be used anymore
/*
* Stuck check
*/
bool _is_stuck{};
float _stuck_threshold{0.1f}; ///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m), set to zero to disable
float _stuck_min_val{}; ///< minimum value for new rng measurement when being stuck
float _stuck_max_val{}; ///< maximum value for new rng measurement when being stuck
/*
* Data regularity check
*/
static constexpr float _dt_update{0.01f}; ///< delta time since last ekf update TODO: this should be a parameter
float _dt_data_lpf{}; ///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec)
Parameters _parameters{};
/*
* Tilt check
@@ -172,20 +127,6 @@ private:
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)
/*
* Quality check
*/
uint64_t _time_bad_quality_us{}; ///< timestamp at which range finder signal quality was 0 (used for hysteresis)
uint64_t _quality_hyst_us{}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us)
/*
* Fog check
*/
bool _is_blocked{false};
float _max_fog_dist{0.f}; //< maximum distance at which the range finder could detect fog (m)
math::MedianFilter<float, 5> _median_dist{};
float _prev_median_dist{0.f};
};
} // namespace sensor
src/modules/ekf2/EKF/common.h
+6 -4
View File
@@ -415,17 +415,19 @@ struct parameters {
float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec)
float range_noise{0.1f}; ///< observation noise for range finder measurements (m)
float range_innov_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD)
float rng_sens_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 range_noise_scaler{0.0f}; ///< scaling from range measurement to noise (m/m)
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 max_hagl_for_range_aid{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1)
float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1)
float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion
float range_valid_quality_s{1.0f}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
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 range_kin_consistency_gate{0.5f}; ///< gate size used by the range finder kinematic consistency check
float rng_fog{0.f}; ///< max distance which a blocked range sensor measures (fog, dirt) [m]
Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m)
float ekf2_rng_pos_x{0.f};
float ekf2_rng_pos_y{0.f};
float ekf2_rng_pos_z{0.f};
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
src/modules/ekf2/EKF/ekf.cpp
+12 -7
View File
@@ -77,13 +77,6 @@ void Ekf::reset()
_state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance;
#endif // CONFIG_EKF2_TERRAIN
#if defined(CONFIG_EKF2_RANGE_FINDER)
_range_sensor.setPitchOffset(_params.rng_sens_pitch);
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
_range_sensor.setMaxFogDistance(_params.rng_fog);
#endif // CONFIG_EKF2_RANGE_FINDER
_control_status.value = 0;
_control_status_prev.value = 0;
@@ -390,6 +383,18 @@ void Ekf::updateParameters()
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
_aux_global_position.updateParameters();
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
#if defined (CONFIG_EKF2_RANGE_FINDER)
sensor::SensorRangeFinder::Parameters params = {};
// 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
}
template<typename T>
src/modules/ekf2/EKF/estimator_interface.h
+1 -1
View File
@@ -121,7 +121,7 @@ public:
_range_sensor.setLimits(min_distance, max_distance);
}
const estimator::sensor::rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
const estimator::sensor::rangeSample &get_rng_sample_delayed() { return *(_range_sensor.sample()); }
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
src/modules/ekf2/EKF2.cpp
+5 -8
View File
@@ -155,18 +155,17 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_rng_ctrl(_params->rng_ctrl),
_param_ekf2_rng_delay(_params->range_delay_ms),
_param_ekf2_rng_noise(_params->range_noise),
_param_ekf2_rng_sfe(_params->range_noise_scaler),
_param_ekf2_rng_sfe(_params->ekf2_rng_sfe),
_param_ekf2_rng_gate(_params->range_innov_gate),
_param_ekf2_rng_pitch(_params->rng_sens_pitch),
_param_ekf2_rng_pitch(_params->ekf2_rng_pitch),
_param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid),
_param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid),
_param_ekf2_rng_a_igate(_params->range_aid_innov_gate),
_param_ekf2_rng_qlty_t(_params->range_valid_quality_s),
_param_ekf2_rng_k_gate(_params->range_kin_consistency_gate),
_param_ekf2_rng_fog(_params->rng_fog),
_param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
_param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
_param_ekf2_rng_pos_z(_params->rng_pos_body(2)),
_param_ekf2_rng_pos_x(_params->ekf2_rng_pos_x),
_param_ekf2_rng_pos_y(_params->ekf2_rng_pos_y),
_param_ekf2_rng_pos_z(_params->ekf2_rng_pos_z),
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
_param_ekf2_ev_delay(_params->ev_delay_ms),
@@ -1610,8 +1609,6 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f);
// TODO: review, we should use sensor variance not terrain
lpos.dist_bottom_var = _ekf.getTerrainVariance();
// Variance can be hardcoded to: EKF2_RNG_NOISE + (distance * EKF2_RNG_SFE)
// lpos.dist_bottom_var = _params->range_noise + (_params->range_noise_scaler);
_ekf.get_hagl_reset(&lpos.delta_dist_bottom, &lpos.dist_bottom_reset_counter);
src/modules/ekf2/EKF2.hpp
-1
View File
@@ -619,7 +619,6 @@ private:
(ParamExtFloat<px4::params::EKF2_RNG_A_VMAX>) _param_ekf2_rng_a_vmax,
(ParamExtFloat<px4::params::EKF2_RNG_A_HMAX>) _param_ekf2_rng_a_hmax,
(ParamExtFloat<px4::params::EKF2_RNG_A_IGATE>) _param_ekf2_rng_a_igate,
(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>) _param_ekf2_rng_qlty_t,
(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>) _param_ekf2_rng_k_gate,
(ParamExtFloat<px4::params::EKF2_RNG_FOG>) _param_ekf2_rng_fog,
(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x,
src/modules/ekf2/params_range_finder.yaml
-10
View File
@@ -93,16 +93,6 @@ parameters:
unit: SD
min: 0.1
max: 5.0
EKF2_RNG_QLTY_T:
description:
short: Minumum range validity period
long: Minimum duration during which the reported range finder signal quality
needs to be non-zero in order to be declared valid (s)
type: float
default: 1.0
unit: s
min: 0.1
max: 5
EKF2_RNG_K_GATE:
description:
short: Gate size used for range finder kinematic consistency check
src/modules/ekf2/test/test_SensorRangeFinder.cpp
+4 -6
View File
@@ -51,7 +51,6 @@ public:
_range_finder.setPitchOffset(0.f);
_range_finder.setCosMaxTilt(0.707f);
_range_finder.setLimits(_min_range, _max_range);
_range_finder.setMaxFogDistance(2.f);
}
// Use this method to clean up any memory, network etc. after each test
@@ -118,9 +117,8 @@ TEST_F(SensorRangeFinderTest, setRange)
sample.time_us = 1e6;
sample.quality = 9;
_range_finder.setRange(sample.rng);
_range_finder.setDataReadiness(true);
_range_finder.setValidity(true);
_range_finder.setSample(sample)
EXPECT_TRUE(_range_finder.isDataHealthy());
EXPECT_TRUE(_range_finder.isHealthy());
}
@@ -363,7 +361,7 @@ TEST_F(SensorRangeFinderTest, blockedByFog)
// WHEN: sensor is then blocked by fog
// range jumps to value below 2m
uint64_t t_now_us = _range_finder.getSampleAddress()->time_us;
uint64_t t_now_us = _range_finder.sample()->time_us;
rangeSample sample{t_now_us, 1.f, 100};
updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us);
@@ -373,7 +371,7 @@ TEST_F(SensorRangeFinderTest, blockedByFog)
// WHEN: the sensor is not blocked by fog anymore
sample.rng = 5.f;
sample.time_us = _range_finder.getSampleAddress()->time_us;
sample.time_us = _range_finder.sample()->time_us;
updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us);
// THEN: the data should be marked as healthy again