fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-08 18:21:36 +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-08-24 09:16:11 -04: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/control.cpp
+9 -14
View File
@@ -268,7 +268,7 @@ void Ekf::controlExternalVisionFusion()
_ev_sample_delayed.pos -= pos_offset_earth;
// Use an incremental position fusion method for EV position data if GPS is also used
if (_params.fusion_mode & SensorFusionMask::USE_GPS) {
if (_params.gnss_ctrl & GnssCtrl::HPOS) {
_fuse_hpos_as_odom = true;
} else {
@@ -572,7 +572,7 @@ void Ekf::resetOnGroundMotionForOpticalFlowChecks()
void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
{
if (!(_params.fusion_mode & SensorFusionMask::USE_GPS_YAW)
if (!(_params.gnss_ctrl & GnssCtrl::YAW)
|| _control_status.flags.gps_yaw_fault) {
stopGpsYawFusion();
@@ -731,7 +731,7 @@ void Ekf::checkHeightSensorRefFallback()
}
}
bool Ekf::isRangeAidSuitable()
bool Ekf::isConditionalRangeAidSuitable()
{
bool is_range_aid_suitable = false;
@@ -764,7 +764,7 @@ bool Ekf::isRangeAidSuitable()
void Ekf::controlBaroHeightFusion()
{
if (!(_params.fusion_mode & SensorFusionMask::USE_BARO_HGT)) {
if (!(_params.baro_ctrl == 1)) {
stopBaroHgtFusion();
return;
}
@@ -812,7 +812,7 @@ void Ekf::controlBaroHeightFusion()
void Ekf::controlGnssHeightFusion()
{
if (!(_params.fusion_mode & SensorFusionMask::USE_GPS_HGT)) {
if (!(_params.gnss_ctrl & GnssCtrl::VPOS)) {
stopGpsHgtFusion();
return;
}
@@ -855,7 +855,7 @@ void Ekf::controlGnssHeightFusion()
void Ekf::controlRangeHeightFusion()
{
if (!(_params.fusion_mode & SensorFusionMask::USE_RNG_HGT) && (_params.range_aid == 0)) {
if (!((_params.rng_ctrl == RngCtrl::CONDITIONAL) || (_params.rng_ctrl == RngCtrl::ENABLED))) {
stopRngHgtFusion();
return;
}
@@ -876,18 +876,13 @@ void Ekf::controlRangeHeightFusion()
if (_rng_data_ready) {
updateRngHgt(_aid_src_rng_hgt);
const bool do_range_aid = (_params.range_aid == 1) && isRangeAidSuitable();
const bool do_conditional_range_aid = (_params.rng_ctrl == RngCtrl::CONDITIONAL) && isConditionalRangeAidSuitable();
const bool continuing_conditions_passing = _range_sensor.isDataHealthy() && ((_params.range_aid == 0) || do_range_aid);
const bool continuing_conditions_passing = _range_sensor.isDataHealthy() && ((_params.rng_ctrl == RngCtrl::ENABLED) || do_conditional_range_aid);
const bool starting_conditions_passing = continuing_conditions_passing
&& _range_sensor.isRegularlySendingData();
if (_control_status.flags.rng_hgt) {
if (do_range_aid) {
// Force to be the height reference
_height_sensor_ref = HeightSensorRef::RANGE;
}
if (continuing_conditions_passing) {
fuseRngHgt(_aid_src_rng_hgt);
@@ -922,7 +917,7 @@ void Ekf::controlRangeHeightFusion()
void Ekf::controlEvHeightFusion()
{
if (!(_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_HGT)) {
if (!(_params.height_sensor_ref == HeightSensor::EV)) { // TODO: replace by EV control parameter
stopEvHgtFusion();
return;
}