fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 15:23: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-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/common.h
+21 -11
View File
@@ -113,21 +113,30 @@ enum HeightSensor : uint8_t {
UNKNOWN = 4
};
enum GnssCtrl : uint8_t {
HPOS = (1<<0),
VPOS = (1<<1),
VEL = (1<<2),
YAW = (1<<3)
};
enum RngCtrl : uint8_t {
DISABLED = 0,
CONDITIONAL = 1,
ENABLED = 2
};
enum SensorFusionMask : uint16_t {
// Bit locations for fusion_mode
USE_GPS = (1<<0), ///< set to true to use GPS data
/* USE_GPS = (1<<0), ///< set to true to use GPS data */
USE_OPT_FLOW = (1<<1), ///< set to true to use optical flow data
INHIBIT_ACC_BIAS = (1<<2), ///< set to true to inhibit estimation of accelerometer delta velocity bias
USE_EXT_VIS_POS = (1<<3), ///< set to true to use external vision position data
USE_EXT_VIS_YAW = (1<<4), ///< set to true to use external vision quaternion data for yaw
USE_DRAG = (1<<5), ///< set to true to use the multi-rotor drag model to estimate wind
ROTATE_EXT_VIS = (1<<6), ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
USE_GPS_YAW = (1<<7), ///< set to true to use GPS yaw data if available
/* USE_GPS_YAW = (1<<7), ///< set to true to use GPS yaw data if available */
USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data
USE_BARO_HGT = (1<<9), ///< set to true to use baro data
USE_GPS_HGT = (1<<10), ///< set to true to use GPS height data
USE_RNG_HGT = (1<<11), ///< set to true to use range height data
USE_EXT_VIS_HGT = (1<<12) ///< set to true to use external vision height data
};
struct gpsMessage {
@@ -251,9 +260,11 @@ struct parameters {
int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds
// measurement source control
int32_t fusion_mode{SensorFusionMask::USE_GPS |
SensorFusionMask::USE_BARO_HGT}; ///< bitmasked integer that selects which aiding sources will be used
int32_t fusion_mode{}; ///< bitmasked integer that selects some aiding sources
int32_t height_sensor_ref{HeightSensor::BARO};
int32_t baro_ctrl{1};
int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
int32_t rng_ctrl{RngCtrl::CONDITIONAL};
int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
@@ -335,9 +346,8 @@ struct parameters {
float rng_sens_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 range_noise_scaler{0.0f}; ///< scaling from range measurement to noise (m/m)
const float vehicle_variance_scaler{0.0f}; ///< gain applied to vehicle height variance used in calculation of height above ground observation variance
float max_hagl_for_range_aid{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if range_aid == 1)
float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if range_aid == 1)
int32_t range_aid{0}; ///< allow switching primary height source to range finder if certain conditions are met
float max_hagl_for_range_aid{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1)
float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1)
float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion
float range_valid_quality_s{1.0f}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
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
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;
}
src/modules/ekf2/EKF/ekf.h
+1 -2
View File
@@ -948,8 +948,7 @@ private:
void controlRangeHeightFusion();
void controlEvHeightFusion();
// determine if flight condition is suitable to use range finder instead of the primary height sensor
bool isRangeAidSuitable();
bool isConditionalRangeAidSuitable();
void stopMagFusion();
void stopMag3DFusion();
src/modules/ekf2/EKF/ekf_helper.cpp
+1 -1
View File
@@ -860,7 +860,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
const float rangefinder_hagl_max = 0.75f * _range_sensor.getValidMaxVal();
// TODO : calculate visual odometry limits
const bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid;
const bool relying_on_rangefinder = isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.rng_hgt);
const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow);
// Keep within range sensor limit when using rangefinder as primary height source
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)
src/modules/ekf2/EKF/estimator_interface.h
+1
View File
@@ -183,6 +183,7 @@ public:
bool isOtherSourceOfVerticalPositionAidingThan(bool aiding_flag) const;
bool isVerticalPositionAidingActive() const;
bool isOnlyActiveSourceOfVerticalPositionAiding(const bool aiding_flag) const;
int getNumberOfActiveVerticalPositionAidingSources() const;
bool isVerticalVelocityAidingActive() const;
src/modules/ekf2/EKF/gps_control.cpp
+8 -3
View File
@@ -41,7 +41,7 @@
void Ekf::controlGpsFusion()
{
if (!(_params.fusion_mode & SensorFusionMask::USE_GPS)) {
if (!((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))) {
stopGpsFusion();
return;
}
@@ -72,8 +72,13 @@ void Ekf::controlGpsFusion()
if (continuing_conditions_passing
|| !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
fuseGpsVel();
fuseGpsPos();
if (_params.gnss_ctrl & GnssCtrl::VEL) {
fuseGpsVel();
}
if ((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VPOS)) {
fuseGpsPos();
}
if (shouldResetGpsFusion()) {
const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);