fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-15 05:10:34 +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/test/sensor_simulator/ekf_wrapper.cpp
+15 -20
View File
@@ -12,17 +12,17 @@ EkfWrapper::~EkfWrapper()
void EkfWrapper::setBaroHeightRef()
{
_ekf_params->height_sensor_ref |= HeightSensorRef::BARO;
_ekf_params->height_sensor_ref = HeightSensor::BARO;
}
void EkfWrapper::enableBaroHeightFusion()
{
_ekf_params->fusion_mode |= SensorFusionMask::USE_BARO_HGT;
_ekf_params->baro_ctrl = 1;
}
void EkfWrapper::disableBaroHeightFusion()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_BARO_HGT;
_ekf_params->baro_ctrl = 0;
}
bool EkfWrapper::isIntendingBaroHeightFusion() const
@@ -32,17 +32,17 @@ bool EkfWrapper::isIntendingBaroHeightFusion() const
void EkfWrapper::setGpsHeightRef()
{
_ekf_params->height_sensor_ref |= HeightSensorRef::GPS;
_ekf_params->height_sensor_ref = HeightSensor::GNSS;
}
void EkfWrapper::enableGpsHeightFusion()
{
_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS_HGT;
_ekf_params->gnss_ctrl |= GnssCtrl::VPOS;
}
void EkfWrapper::disableGpsHeightFusion()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS_HGT;
_ekf_params->gnss_ctrl &= ~GnssCtrl::VPOS;
}
bool EkfWrapper::isIntendingGpsHeightFusion() const
@@ -52,17 +52,17 @@ bool EkfWrapper::isIntendingGpsHeightFusion() const
void EkfWrapper::setRangeHeightRef()
{
_ekf_params->height_sensor_ref |= HeightSensorRef::RANGE;
_ekf_params->height_sensor_ref = HeightSensor::RANGE;
}
void EkfWrapper::enableRangeHeightFusion()
{
_ekf_params->fusion_mode |= SensorFusionMask::USE_RNG_HGT;
_ekf_params->rng_ctrl = RngCtrl::ENABLED;
}
void EkfWrapper::disableRangeHeightFusion()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_RNG_HGT;
_ekf_params->rng_ctrl = RngCtrl::DISABLED;
}
bool EkfWrapper::isIntendingRangeHeightFusion() const
@@ -72,17 +72,12 @@ bool EkfWrapper::isIntendingRangeHeightFusion() const
void EkfWrapper::setExternalVisionHeightRef()
{
_ekf_params->height_sensor_ref |= HeightSensorRef::EV;
_ekf_params->height_sensor_ref = HeightSensor::EV;
}
void EkfWrapper::enableExternalVisionHeightFusion()
{
_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_HGT;
}
void EkfWrapper::disableExternalVisionHeightFusion()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_HGT;
setExternalVisionHeightRef(); // TODO: replace by EV control parameter
}
bool EkfWrapper::isIntendingExternalVisionHeightFusion() const
@@ -92,12 +87,12 @@ bool EkfWrapper::isIntendingExternalVisionHeightFusion() const
void EkfWrapper::enableGpsFusion()
{
_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS;
_ekf_params->gnss_ctrl |= GnssCtrl::HPOS | GnssCtrl::VEL;
}
void EkfWrapper::disableGpsFusion()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS;
_ekf_params->gnss_ctrl &= ~(GnssCtrl::HPOS | GnssCtrl::VEL);
}
bool EkfWrapper::isIntendingGpsFusion() const
@@ -107,12 +102,12 @@ bool EkfWrapper::isIntendingGpsFusion() const
void EkfWrapper::enableGpsHeadingFusion()
{
_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS_YAW;
_ekf_params->gnss_ctrl |= GnssCtrl::YAW;
}
void EkfWrapper::disableGpsHeadingFusion()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS_YAW;
_ekf_params->gnss_ctrl &= ~GnssCtrl::YAW;
}
bool EkfWrapper::isIntendingGpsHeadingFusion() const
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
+1 -1
View File
@@ -66,7 +66,7 @@ public:
void setExternalVisionHeightRef();
void enableExternalVisionHeightFusion();
void disableExternalVisionHeightFusion();
/* void disableExternalVisionHeightFusion(); */
bool isIntendingExternalVisionHeightFusion() const;
void enableGpsFusion();