Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

ekf2: move range buffer pop to controlRangeHeightFusion()

Browse Source
This commit is contained in:
Daniel Agar
2023-02-21 10:37:04 -05:00
parent 08f111f694
commit 60856ebe62
3 changed files with 50 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
-44
View File
@@ -130,50 +130,6 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
}
}
if (_range_buffer) {
// Get range data from buffer and check validity
_rng_data_ready = _range_buffer->pop_first_older_than(imu_delayed.time_us, _range_sensor.getSampleAddress());
_range_sensor.setDataReadiness(_rng_data_ready);
// update range sensor angle parameters in case they have changed
_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.runChecks(imu_delayed.time_us, _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;
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
// Run the kinematic consistency check when not moving horizontally
if (_control_status.flags.in_air && !_control_status.flags.fixed_wing
&& (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) {
const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
const float var = sq(_params.range_noise) + dist_dependant_var;
_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), imu_delayed.time_us);
}
} else {
// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
// and are on the ground, then synthesise a measurement at the expected on ground value
if (!_control_status.flags.in_air
&& _range_sensor.isRegularlySendingData()
&& _range_sensor.isDataReady()) {
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setValidity(true); // bypass the checks
}
}
_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
}
// run EKF-GSF yaw estimator once per imu_delayed update after all main EKF data samples available
runYawEKFGSF(imu_delayed);
src/modules/ekf2/EKF/ekf.h
-1
View File
@@ -488,7 +488,6 @@ private:
// booleans true when fresh sensor data is available at the fusion time horizon
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
bool _rng_data_ready{false};
bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
bool _tas_data_ready{false}; ///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
src/modules/ekf2/EKF/range_height_control.cpp
+50 -1
View File
@@ -42,12 +42,61 @@ void Ekf::controlRangeHeightFusion()
{
static constexpr const char *HGT_SRC_NAME = "RNG";
bool rng_data_ready = false;
if (_range_buffer) {
// Get range data from buffer and check validity
rng_data_ready = _range_buffer->pop_first_older_than(_time_delayed_us, _range_sensor.getSampleAddress());
_range_sensor.setDataReadiness(rng_data_ready);
// update range sensor angle parameters in case they have changed
_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.runChecks(_time_delayed_us, _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;
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
// Run the kinematic consistency check when not moving horizontally
if (_control_status.flags.in_air && !_control_status.flags.fixed_wing
&& (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) {
const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
const float var = sq(_params.range_noise) + dist_dependant_var;
_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), _time_delayed_us);
}
} else {
// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
// and are on the ground, then synthesise a measurement at the expected on ground value
if (!_control_status.flags.in_air
&& _range_sensor.isRegularlySendingData()
&& _range_sensor.isDataReady()) {
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setValidity(true); // bypass the checks
}
}
_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
} else {
return;
}
auto &aid_src = _aid_src_rng_hgt;
HeightBiasEstimator &bias_est = _rng_hgt_b_est;
bias_est.predict(_dt_ekf_avg);
if (_rng_data_ready && _range_sensor.getSampleAddress()) {
if (rng_data_ready && _range_sensor.getSampleAddress()) {
const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
const float measurement_var = sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom());