fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 02:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: add kconfig for barometer support (enabled by default)

Browse Source
This commit is contained in:
Daniel Agar
2023-09-15 10:02:09 -04:00
parent f0224c5104
commit 1e9f0ad2c6
13 changed files with 168 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/CMakeLists.txt
+6 -1
View File
@@ -33,7 +33,6 @@
set(EKF_SRCS)
list(APPEND EKF_SRCS
baro_height_control.cpp
bias_estimator.cpp
control.cpp
covariance.cpp
@@ -65,6 +64,12 @@ if(CONFIG_EKF2_AUXVEL)
list(APPEND EKF_SRCS auxvel_fusion.cpp)
endif()
if(CONFIG_EKF2_BAROMETER)
list(APPEND EKF_SRCS
baro_height_control.cpp
)
endif()
if(CONFIG_EKF2_DRAG_FUSION)
list(APPEND EKF_SRCS drag_fusion.cpp)
endif()
src/modules/ekf2/EKF/baro_height_control.cpp
+5
View File
@@ -51,7 +51,12 @@ void Ekf::controlBaroHeightFusion()
if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_us, &baro_sample)) {
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
const float measurement = compensateBaroForDynamicPressure(baro_sample.hgt);
#else
const float measurement = baro_sample.hgt;
#endif
const float measurement_var = sq(_params.baro_noise);
const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
src/modules/ekf2/EKF/covariance.cpp
+4
View File
@@ -423,7 +423,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
#endif // CONFIG_EKF2_EXTERNAL_VISION
if (bad_vz_gps || bad_vz_ev) {
#if defined(CONFIG_EKF2_BAROMETER)
bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.0f);
#else
bool bad_z_baro = false;
#endif
bool bad_z_gps = _control_status.flags.gps_hgt && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f);
#if defined(CONFIG_EKF2_RANGE_FINDER)
src/modules/ekf2/EKF/ekf.cpp
+4
View File
@@ -109,7 +109,9 @@ void Ekf::reset()
_gps_checks_passed = false;
_gps_alt_ref = NAN;
#if defined(CONFIG_EKF2_BAROMETER)
_baro_counter = 0;
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_MAGNETOMETER)
_mag_counter = 0;
@@ -119,7 +121,9 @@ void Ekf::reset()
_time_good_vert_accel = 0;
_clip_counter = 0;
#if defined(CONFIG_EKF2_BAROMETER)
resetEstimatorAidStatus(_aid_src_baro_hgt);
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_AIRSPEED)
resetEstimatorAidStatus(_aid_src_airspeed);
#endif // CONFIG_EKF2_AIRSPEED
src/modules/ekf2/EKF/ekf.h
+22 -9
View File
@@ -91,9 +91,14 @@ public:
void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_BAROMETER)
const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _aid_src_baro_hgt.innovation; }
void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; }
void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; }
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_TERRAIN)
// terrain estimate
@@ -503,7 +508,7 @@ public:
bool isYawEmergencyEstimateAvailable() const;
uint8_t getHeightSensorRef() const { return _height_sensor_ref; }
const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); }
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
@@ -520,8 +525,6 @@ public:
const auto &aid_src_sideslip() const { return _aid_src_sideslip; }
#endif // CONFIG_EKF2_SIDESLIP
const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
const auto &aid_src_fake_hgt() const { return _aid_src_fake_hgt; }
const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
@@ -669,7 +672,6 @@ private:
bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
#endif // CONFIG_EKF2_OPTICAL_FLOW
estimator_aid_source1d_s _aid_src_baro_hgt{};
#if defined(CONFIG_EKF2_AIRSPEED)
estimator_aid_source1d_s _aid_src_airspeed{};
#endif // CONFIG_EKF2_AIRSPEED
@@ -724,12 +726,20 @@ private:
// Variables used by the initial filter alignment
bool _is_first_imu_sample{true};
uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation
AlphaFilter<Vector3f> _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s)
AlphaFilter<Vector3f> _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
#if defined(CONFIG_EKF2_BAROMETER)
estimator_aid_source1d_s _aid_src_baro_hgt{};
// Variables used to perform in flight resets and switch between height sources
AlphaFilter<float> _baro_lpf{0.1f}; ///< filtered barometric height measurement (m)
uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation
HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref};
bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_MAGNETOMETER)
float _mag_heading_prev{}; ///< previous value of mag heading (rad)
@@ -773,7 +783,6 @@ private:
Vector3f _prev_accel_bias_var{}; ///< saved accel XYZ bias variances
// height sensor status
bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags
bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent
// imu fault status
@@ -1044,7 +1053,9 @@ private:
// and a scalar innovation value
void fuse(const Vector24f &K, float innovation);
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const;
#endif // CONFIG_EKF2_BARO_COMPENSATION
// calculate the earth rotation vector from a given latitude
Vector3f calcEarthRateNED(float lat_rad) const;
@@ -1135,13 +1146,16 @@ private:
// control for combined height fusion mode (implemented for switching between baro and range height)
void controlHeightFusion(const imuSample &imu_delayed);
void checkHeightSensorRefFallback();
void controlBaroHeightFusion();
void controlGnssHeightFusion(const gpsSample &gps_sample);
#if defined(CONFIG_EKF2_BAROMETER)
void controlBaroHeightFusion();
void stopBaroHgtFusion();
void stopGpsHgtFusion();
void updateGroundEffect();
#endif // CONFIG_EKF2_BAROMETER
void stopGpsHgtFusion();
// gravity fusion: heuristically enable / disable gravity fusion
void controlGravityFusion(const imuSample &imu_delayed);
@@ -1198,7 +1212,6 @@ private:
uint8_t _height_sensor_ref{HeightSensor::UNKNOWN};
uint8_t _position_sensor_ref{static_cast<uint8_t>(PositionSensor::GNSS)};
HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref};
HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
src/modules/ekf2/EKF/ekf_helper.cpp
+8 -2
View File
@@ -200,7 +200,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
_state_reset_status.reset_count.posD++;
#if defined(CONFIG_EKF2_BAROMETER)
_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
#endif // CONFIG_EKF2_EXTERNAL_VISION
@@ -241,9 +243,9 @@ void Ekf::constrainStates()
_state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f);
}
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
{
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
if (_control_status.flags.wind && local_position_is_valid()) {
// calculate static pressure error = Pmeas - Ptruth
// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
@@ -271,11 +273,11 @@ float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated)
// correct baro measurement using pressure error estimate and assuming sea level gravity
return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G);
}
#endif // CONFIG_EKF2_BARO_COMPENSATION
// otherwise return the uncorrected baro measurement
return baro_alt_uncompensated;
}
#endif // CONFIG_EKF2_BARO_COMPENSATION
// calculate the earth rotation vector
Vector3f Ekf::calcEarthRateNED(float lat_rad) const
@@ -706,10 +708,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
float hgt_sum = 0.f;
int n_hgt_sources = 0;
#if defined(CONFIG_EKF2_BAROMETER)
if (_control_status.flags.baro_hgt) {
hgt_sum += sqrtf(_aid_src_baro_hgt.test_ratio);
n_hgt_sources++;
}
#endif // CONFIG_EKF2_BAROMETER
if (_control_status.flags.gps_hgt) {
hgt_sum += sqrtf(_aid_src_gnss_hgt.test_ratio);
@@ -913,6 +917,7 @@ float Ekf::getYawVar() const
return yaw_var;
}
#if defined(CONFIG_EKF2_BAROMETER)
void Ekf::updateGroundEffect()
{
if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {
@@ -935,6 +940,7 @@ void Ekf::updateGroundEffect()
_control_status.flags.gnd_effect = false;
}
}
#endif // CONFIG_EKF2_BAROMETER
void Ekf::increaseQuatYawErrVariance(float yaw_variance)
{
src/modules/ekf2/EKF/estimator_interface.cpp
+6
View File
@@ -50,7 +50,9 @@ EstimatorInterface::~EstimatorInterface()
#if defined(CONFIG_EKF2_MAGNETOMETER)
delete _mag_buffer;
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_BAROMETER)
delete _baro_buffer;
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_RANGE_FINDER)
delete _range_buffer;
#endif // CONFIG_EKF2_RANGE_FINDER
@@ -221,6 +223,7 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
}
}
#if defined(CONFIG_EKF2_BAROMETER)
void EstimatorInterface::setBaroData(const baroSample &baro_sample)
{
if (!_initialised) {
@@ -256,6 +259,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us);
}
}
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_AIRSPEED)
void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
@@ -717,9 +721,11 @@ void EstimatorInterface::print_status()
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_BAROMETER)
if (_baro_buffer) {
printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size());
}
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_RANGE_FINDER)
if (_range_buffer) {
src/modules/ekf2/EKF/estimator_interface.h
+6 -2
View File
@@ -92,7 +92,9 @@ public:
void setGpsData(const gpsMessage &gps);
#if defined(CONFIG_EKF2_BAROMETER)
void setBaroData(const baroSample &baro_sample);
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_AIRSPEED)
void setAirspeedData(const airspeedSample &airspeed_sample);
@@ -422,8 +424,6 @@ protected:
uint64_t _time_last_mag_buffer_push{0};
#endif // CONFIG_EKF2_MAGNETOMETER
RingBuffer<baroSample> *_baro_buffer{nullptr};
#if defined(CONFIG_EKF2_AIRSPEED)
RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
#endif // CONFIG_EKF2_AIRSPEED
@@ -438,7 +438,11 @@ protected:
RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
uint64_t _time_last_gps_buffer_push{0};
#if defined(CONFIG_EKF2_BAROMETER)
RingBuffer<baroSample> *_baro_buffer{nullptr};
uint64_t _time_last_baro_buffer_push{0};
#endif // CONFIG_EKF2_BAROMETER
uint64_t _time_last_gnd_effect_on{0};
src/modules/ekf2/EKF/height_control.cpp
+6
View File
@@ -41,10 +41,14 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed)
{
checkVerticalAccelerationHealth(imu_delayed);
#if defined(CONFIG_EKF2_BAROMETER)
updateGroundEffect();
controlBaroHeightFusion();
#endif // CONFIG_EKF2_BAROMETER
controlGnssHeightFusion(_gps_sample_delayed);
#if defined(CONFIG_EKF2_RANGE_FINDER)
controlRangeHeightFusion();
#endif // CONFIG_EKF2_RANGE_FINDER
@@ -175,9 +179,11 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
bool failed_lim{false};
} checks[6] {};
#if defined(CONFIG_EKF2_BAROMETER)
if (_control_status.flags.baro_hgt) {
checks[0] = {ReferenceType::PRESSURE, _aid_src_baro_hgt.innovation, _aid_src_baro_hgt.innovation_variance};
}
#endif // CONFIG_EKF2_BAROMETER
if (_control_status.flags.gps_hgt) {
checks[1] = {ReferenceType::GNSS, _aid_src_gnss_hgt.innovation, _aid_src_gnss_hgt.innovation_variance};