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

remove useless parameter EKF2_RNG_PITCH, remove sensor namespace, continue cleanup

Browse Source
This commit is contained in:
Jacob Dahl
2025-09-22 18:54:03 -08:00
parent 2dd864a87a
commit 349bd6175c
12 changed files with 18 additions and 87 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
+3 -5
View File
@@ -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");
}
src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp
+1 -23
View File
@@ -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
src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp
+3 -25
View File
@@ -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 &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:
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
src/modules/ekf2/EKF/common.h
-1
View File
@@ -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)
src/modules/ekf2/EKF/ekf.cpp
+1 -10
View File
@@ -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
}
src/modules/ekf2/EKF/estimator_interface.cpp
+3 -3
View File
@@ -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<sensor::rangeSample>(_obs_buffer_length);
_range_buffer = new RingBuffer<rangeSample>(_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<int64_t>(_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);
src/modules/ekf2/EKF/estimator_interface.h
+3 -3
View File
@@ -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<sensor::rangeSample> *_range_buffer {nullptr};
RingBuffer<rangeSample> *_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
src/modules/ekf2/EKF2.cpp
+2 -3
View File
@@ -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<float>(optical_flow.quality) / static_cast<float>(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,
src/modules/ekf2/EKF2.hpp
-1
View File
@@ -618,7 +618,6 @@ private:
(ParamExtFloat<px4::params::EKF2_RNG_NOISE>) _param_ekf2_rng_noise,
(ParamExtFloat<px4::params::EKF2_RNG_SFE>) _param_ekf2_rng_sfe,
(ParamExtFloat<px4::params::EKF2_RNG_GATE>) _param_ekf2_rng_gate,
(ParamExtFloat<px4::params::EKF2_RNG_PITCH>) _param_ekf2_rng_pitch,
(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_K_GATE>) _param_ekf2_rng_k_gate,
src/modules/ekf2/params_range_finder.yaml
-9
View File
@@ -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
src/modules/ekf2/test/sensor_simulator/range_finder.h
+1 -1
View File
@@ -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};
src/modules/ekf2/test/test_SensorRangeFinder.cpp
+1 -3
View File
@@ -37,7 +37,7 @@
#include "EKF/aid_sources/range_finder/sensor_range_finder.hpp"
#include <matrix/math.hpp>
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);
}