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

ekf2: GPS, baro and range finder control parameters

Browse Source 
Also remove the legacy "range aid" than can be achieved by setting the
height reference to range finder and the range finder control parameter
to "conditional".

Conditional range aiding cal also be set when the height reference isn't
the range finder. This prevents the ratchetting effect due to switching
between references.
This commit is contained in:
bresch
2022-07-29 14:48:44 +02:00
committed by Daniel Agar
parent 578e1339ca
commit 8962cf2d25
13 changed files with 151 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/estimator_interface.cpp
+9 -4
View File
@@ -437,7 +437,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
float max_time_delay_ms = math::max((float)_params.sensor_interval_max_ms, _params.auxvel_delay_ms);
// using baro
if (_params.fusion_mode & SensorFusionMask::USE_BARO_HGT) {
if (_params.baro_ctrl > 0) {
max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms);
}
@@ -451,12 +451,12 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms);
}
// range aid or range height
if (_params.range_aid || (_params.fusion_mode & SensorFusionMask::USE_RNG_HGT)) {
// using range finder
if ((_params.rng_ctrl != RngCtrl::DISABLED)) {
max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
}
if (_params.fusion_mode & SensorFusionMask::USE_GPS) {
if (_params.gnss_ctrl > 0) {
max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
}
@@ -539,6 +539,11 @@ bool EstimatorInterface::isVerticalPositionAidingActive() const
return getNumberOfActiveVerticalPositionAidingSources() > 0;
}
bool EstimatorInterface::isOnlyActiveSourceOfVerticalPositionAiding(const bool aiding_flag) const
{
return aiding_flag && !isOtherSourceOfVerticalPositionAidingThan(aiding_flag);
}
int EstimatorInterface::getNumberOfActiveVerticalPositionAidingSources() const
{
return int(_control_status.flags.gps_hgt)