forked from Archive/PX4-Autopilot
ekf2: add kconfig for barometer support (enabled by default)
This commit is contained in:
Daniel Agar
parent
f0224c5104
commit
1e9f0ad2c6
|
|
@ -68,7 +68,6 @@ add_subdirectory(Utility)
|
|||
|
||||
set(EKF_SRCS)
|
||||
list(APPEND EKF_SRCS
|
||||
EKF/baro_height_control.cpp
|
||||
EKF/bias_estimator.cpp
|
||||
EKF/control.cpp
|
||||
EKF/covariance.cpp
|
||||
|
|
@ -100,6 +99,12 @@ if(CONFIG_EKF2_AUXVEL)
|
|||
list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_BAROMETER)
|
||||
list(APPEND EKF_SRCS
|
||||
EKF/baro_height_control.cpp
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_EKF2_DRAG_FUSION)
|
||||
list(APPEND EKF_SRCS EKF/drag_fusion.cpp)
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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};
|
||||
|
||||
|
|
|
|||
|
|
@ -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};
|
||||
|
|
|
|||
|
|
@ -60,7 +60,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
|||
_params(_ekf.getParamHandle()),
|
||||
_param_ekf2_predict_us(_params->filter_update_interval_us),
|
||||
_param_ekf2_imu_ctrl(_params->imu_ctrl),
|
||||
_param_ekf2_baro_delay(_params->baro_delay_ms),
|
||||
_param_ekf2_gps_delay(_params->gps_delay_ms),
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
_param_ekf2_avel_delay(_params->auxvel_delay_ms),
|
||||
|
|
@ -73,10 +72,22 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
|||
_param_ekf2_gps_v_noise(_params->gps_vel_noise),
|
||||
_param_ekf2_gps_p_noise(_params->gps_pos_noise),
|
||||
_param_ekf2_noaid_noise(_params->pos_noaid_noise),
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
_param_ekf2_baro_ctrl(_params->baro_ctrl),
|
||||
_param_ekf2_baro_delay(_params->baro_delay_ms),
|
||||
_param_ekf2_baro_noise(_params->baro_noise),
|
||||
_param_ekf2_baro_gate(_params->baro_innov_gate),
|
||||
_param_ekf2_gnd_eff_dz(_params->gnd_effect_deadzone),
|
||||
_param_ekf2_gnd_max_hgt(_params->gnd_effect_max_hgt),
|
||||
# if defined(CONFIG_EKF2_BARO_COMPENSATION)
|
||||
_param_ekf2_aspd_max(_params->max_correction_airspeed),
|
||||
_param_ekf2_pcoef_xp(_params->static_pressure_coef_xp),
|
||||
_param_ekf2_pcoef_xn(_params->static_pressure_coef_xn),
|
||||
_param_ekf2_pcoef_yp(_params->static_pressure_coef_yp),
|
||||
_param_ekf2_pcoef_yn(_params->static_pressure_coef_yn),
|
||||
_param_ekf2_pcoef_z(_params->static_pressure_coef_z),
|
||||
# endif // CONFIG_EKF2_BARO_COMPENSATION
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
_param_ekf2_gps_p_gate(_params->gps_pos_innov_gate),
|
||||
_param_ekf2_gps_v_gate(_params->gps_vel_innov_gate),
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
|
|
@ -117,7 +128,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
|||
_param_ekf2_req_hdrift(_params->req_hdrift),
|
||||
_param_ekf2_req_vdrift(_params->req_vdrift),
|
||||
_param_ekf2_hgt_ref(_params->height_sensor_ref),
|
||||
_param_ekf2_baro_ctrl(_params->baro_ctrl),
|
||||
_param_ekf2_gps_ctrl(_params->gnss_ctrl),
|
||||
_param_ekf2_noaid_tout(_params->valid_timeout_max),
|
||||
#if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
|
@ -191,14 +201,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
|||
_param_ekf2_bcoef_y(_params->bcoef_y),
|
||||
_param_ekf2_mcoef(_params->mcoef),
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
|
||||
_param_ekf2_aspd_max(_params->max_correction_airspeed),
|
||||
_param_ekf2_pcoef_xp(_params->static_pressure_coef_xp),
|
||||
_param_ekf2_pcoef_xn(_params->static_pressure_coef_xn),
|
||||
_param_ekf2_pcoef_yp(_params->static_pressure_coef_yp),
|
||||
_param_ekf2_pcoef_yn(_params->static_pressure_coef_yn),
|
||||
_param_ekf2_pcoef_z(_params->static_pressure_coef_z),
|
||||
#endif // CONFIG_EKF2_BARO_COMPENSATION
|
||||
_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
|
||||
{
|
||||
// advertise expected minimal topic set immediately to ensure logging
|
||||
|
|
@ -258,12 +260,16 @@ bool EKF2::multi_init(int imu, int mag)
|
|||
_estimator_status_pub.advertise();
|
||||
_yaw_est_pub.advertise();
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
|
||||
// baro advertise
|
||||
if (_param_ekf2_baro_ctrl.get()) {
|
||||
_estimator_aid_src_baro_hgt_pub.advertise();
|
||||
_estimator_baro_bias_pub.advertise();
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
// EV advertise
|
||||
|
|
@ -440,6 +446,8 @@ void EKF2::Run()
|
|||
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
|
||||
// if using baro ensure sensor interval minimum is sufficient to accommodate system averaged baro output
|
||||
if (_params->baro_ctrl == 1) {
|
||||
float sens_baro_rate = 0.f;
|
||||
|
|
@ -456,6 +464,8 @@ void EKF2::Run()
|
|||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
||||
// if using mag ensure sensor interval minimum is sufficient to accommodate system averaged mag output
|
||||
|
|
@ -695,7 +705,9 @@ void EKF2::Run()
|
|||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
UpdateAuxVelSample(ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
UpdateBaroSample(ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
UpdateExtVisionSample(ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
|
@ -723,7 +735,9 @@ void EKF2::Run()
|
|||
PublishWindEstimate(now);
|
||||
|
||||
// publish status/logging messages
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
PublishBaroBias(now);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
PublishGnssHgtBias(now);
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
PublishRngHgtBias(now);
|
||||
|
|
@ -792,6 +806,8 @@ void EKF2::VerifyParams()
|
|||
"GPS lon/lat is required for altitude fusion", _param_ekf2_gps_ctrl.get());
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
|
||||
if ((_param_ekf2_hgt_ref.get() == HeightSensor::BARO) && (_param_ekf2_baro_ctrl.get() == 0)) {
|
||||
_param_ekf2_baro_ctrl.set(1);
|
||||
_param_ekf2_baro_ctrl.commit();
|
||||
|
|
@ -803,6 +819,8 @@ void EKF2::VerifyParams()
|
|||
"Baro enabled by EKF2_HGT_REF", _param_ekf2_baro_ctrl.get());
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
||||
if ((_param_ekf2_hgt_ref.get() == HeightSensor::RANGE) && (_param_ekf2_rng_ctrl.get() == RngCtrl::DISABLED)) {
|
||||
|
|
@ -979,8 +997,10 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp)
|
|||
// sideslip
|
||||
PublishAidSourceStatus(_ekf.aid_src_sideslip(), _status_sideslip_pub_last, _estimator_aid_src_sideslip_pub);
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
// baro height
|
||||
PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// RNG height
|
||||
|
|
@ -1064,6 +1084,7 @@ void EKF2::PublishAttitude(const hrt_abstime ×tamp)
|
|||
}
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
void EKF2::PublishBaroBias(const hrt_abstime ×tamp)
|
||||
{
|
||||
if (_ekf.aid_src_baro_hgt().timestamp_sample != 0) {
|
||||
|
|
@ -1077,6 +1098,7 @@ void EKF2::PublishBaroBias(const hrt_abstime ×tamp)
|
|||
}
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
void EKF2::PublishGnssHgtBias(const hrt_abstime ×tamp)
|
||||
{
|
||||
|
|
@ -1326,7 +1348,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
|||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
_ekf.getBaroHgtInnov(innovations.baro_vpos);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getRngHgtInnov(innovations.rng_vpos);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
|
@ -1391,8 +1415,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
|||
_preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel);
|
||||
_preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
_preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
_preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt);
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt);
|
||||
|
|
@ -1414,7 +1439,9 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp)
|
|||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
_ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getRngHgtInnovRatio(test_ratios.rng_vpos);
|
||||
_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
|
||||
|
|
@ -1466,7 +1493,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp)
|
|||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
_ekf.getBaroHgtInnovVar(variances.baro_vpos);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getRngHgtInnovVar(variances.rng_vpos);
|
||||
_ekf.getHaglRateInnovVar(variances.hagl_rate);
|
||||
|
|
@ -1779,7 +1808,9 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp)
|
|||
status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed;
|
||||
|
||||
status.accel_device_id = _device_id_accel;
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
status.baro_device_id = _device_id_baro;
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
status.gyro_device_id = _device_id_gyro;
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
|
|
@ -2097,6 +2128,7 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
}
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
{
|
||||
// EKF baro sample
|
||||
|
|
@ -2140,6 +2172,7 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
|
|||
(int64_t)ekf2_timestamps.timestamp / 100);
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps)
|
||||
|
|
|
|||
|
|
@ -77,7 +77,6 @@
|
|||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/sensor_selection.h>
|
||||
#include <uORB/topics/vehicle_air_data.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
|
|
@ -99,6 +98,10 @@
|
|||
# include <uORB/topics/landing_target_pose.h>
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
# include <uORB/topics/vehicle_air_data.h>
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
# include <uORB/topics/vehicle_magnetometer.h>
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
|
@ -170,7 +173,10 @@ private:
|
|||
|
||||
void PublishAidSourceStatus(const hrt_abstime ×tamp);
|
||||
void PublishAttitude(const hrt_abstime ×tamp);
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
void PublishBaroBias(const hrt_abstime ×tamp);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
void PublishGnssHgtBias(const hrt_abstime ×tamp);
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
|
|
@ -207,7 +213,9 @@ private:
|
|||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
|
@ -276,7 +284,6 @@ private:
|
|||
perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")};
|
||||
perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")};
|
||||
perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
|
||||
perf_counter_t _msg_missed_air_data_perf{nullptr};
|
||||
perf_counter_t _msg_missed_gps_perf{nullptr};
|
||||
perf_counter_t _msg_missed_odometry_perf{nullptr};
|
||||
|
||||
|
|
@ -289,11 +296,9 @@ private:
|
|||
float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84
|
||||
|
||||
uint8_t _accel_calibration_count{0};
|
||||
uint8_t _baro_calibration_count{0};
|
||||
uint8_t _gyro_calibration_count{0};
|
||||
|
||||
uint32_t _device_id_accel{0};
|
||||
uint32_t _device_id_baro{0};
|
||||
uint32_t _device_id_gyro{0};
|
||||
|
||||
Vector3f _last_accel_bias_published{};
|
||||
|
|
@ -302,8 +307,6 @@ private:
|
|||
hrt_abstime _last_sensor_bias_published{0};
|
||||
hrt_abstime _last_gps_status_published{0};
|
||||
|
||||
hrt_abstime _status_baro_hgt_pub_last{0};
|
||||
|
||||
hrt_abstime _status_fake_hgt_pub_last{0};
|
||||
hrt_abstime _status_fake_pos_pub_last{0};
|
||||
|
||||
|
|
@ -386,7 +389,21 @@ private:
|
|||
perf_counter_t _msg_missed_optical_flow_perf{nullptr};
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
perf_counter_t _msg_missed_air_data_perf {nullptr};
|
||||
|
||||
uint8_t _baro_calibration_count {0};
|
||||
uint32_t _device_id_baro{0};
|
||||
hrt_abstime _status_baro_hgt_pub_last{0};
|
||||
|
||||
float _last_baro_bias_published{};
|
||||
|
||||
uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
|
||||
|
||||
uORB::PublicationMulti<estimator_bias_s> _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)};
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
float _last_gnss_hgt_bias_published{};
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
|
|
@ -412,7 +429,6 @@ private:
|
|||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
|
||||
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
|
||||
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
|
|
@ -452,7 +468,6 @@ private:
|
|||
uint32_t _filter_information_event_changes{0};
|
||||
|
||||
uORB::PublicationMulti<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
|
||||
uORB::PublicationMulti<estimator_bias_s> _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
|
||||
uORB::PublicationMulti<estimator_bias_s> _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)};
|
||||
uORB::PublicationMultiData<estimator_event_flags_s> _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
|
||||
uORB::PublicationMulti<estimator_gps_status_s> _estimator_gps_status_pub{ORB_ID(estimator_gps_status)};
|
||||
|
|
@ -465,8 +480,6 @@ private:
|
|||
uORB::PublicationMulti<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
|
||||
uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)};
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
|
||||
|
||||
|
|
@ -497,8 +510,6 @@ private:
|
|||
(ParamExtInt<px4::params::EKF2_PREDICT_US>) _param_ekf2_predict_us,
|
||||
(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
|
||||
|
||||
(ParamExtFloat<px4::params::EKF2_BARO_DELAY>)
|
||||
_param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec)
|
||||
(ParamExtFloat<px4::params::EKF2_GPS_DELAY>)
|
||||
_param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec)
|
||||
|
||||
|
|
@ -526,14 +537,26 @@ private:
|
|||
_param_ekf2_gps_p_noise, ///< minimum allowed observation noise for gps position fusion (m)
|
||||
(ParamExtFloat<px4::params::EKF2_NOAID_NOISE>)
|
||||
_param_ekf2_noaid_noise, ///< observation noise for non-aiding position fusion (m)
|
||||
(ParamExtFloat<px4::params::EKF2_BARO_NOISE>)
|
||||
_param_ekf2_baro_noise, ///< observation noise for barometric height fusion (m)
|
||||
(ParamExtFloat<px4::params::EKF2_BARO_GATE>)
|
||||
_param_ekf2_baro_gate, ///< barometric height innovation consistency gate size (STD)
|
||||
(ParamExtFloat<px4::params::EKF2_GND_EFF_DZ>)
|
||||
_param_ekf2_gnd_eff_dz, ///< barometric deadzone range for negative innovations (m)
|
||||
(ParamExtFloat<px4::params::EKF2_GND_MAX_HGT>)
|
||||
_param_ekf2_gnd_max_hgt, ///< maximum height above the ground level for expected negative baro innovations (m)
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,///< barometer control selection
|
||||
(ParamExtFloat<px4::params::EKF2_BARO_DELAY>) _param_ekf2_baro_delay,
|
||||
(ParamExtFloat<px4::params::EKF2_BARO_NOISE>) _param_ekf2_baro_noise,
|
||||
(ParamExtFloat<px4::params::EKF2_BARO_GATE>) _param_ekf2_baro_gate,
|
||||
(ParamExtFloat<px4::params::EKF2_GND_EFF_DZ>) _param_ekf2_gnd_eff_dz,
|
||||
(ParamExtFloat<px4::params::EKF2_GND_MAX_HGT>) _param_ekf2_gnd_max_hgt,
|
||||
|
||||
# if defined(CONFIG_EKF2_BARO_COMPENSATION)
|
||||
// Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth
|
||||
(ParamExtFloat<px4::params::EKF2_ASPD_MAX>) _param_ekf2_aspd_max,
|
||||
(ParamExtFloat<px4::params::EKF2_PCOEF_XP>) _param_ekf2_pcoef_xp,
|
||||
(ParamExtFloat<px4::params::EKF2_PCOEF_XN>) _param_ekf2_pcoef_xn,
|
||||
(ParamExtFloat<px4::params::EKF2_PCOEF_YP>) _param_ekf2_pcoef_yp,
|
||||
(ParamExtFloat<px4::params::EKF2_PCOEF_YN>) _param_ekf2_pcoef_yn,
|
||||
(ParamExtFloat<px4::params::EKF2_PCOEF_Z>) _param_ekf2_pcoef_z,
|
||||
# endif // CONFIG_EKF2_BARO_COMPENSATION
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
(ParamExtFloat<px4::params::EKF2_GPS_P_GATE>)
|
||||
_param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD)
|
||||
(ParamExtFloat<px4::params::EKF2_GPS_V_GATE>)
|
||||
|
|
@ -596,7 +619,6 @@ private:
|
|||
(ParamInt<px4::params::EKF2_AID_MASK>)
|
||||
_param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
|
||||
(ParamExtInt<px4::params::EKF2_HGT_REF>) _param_ekf2_hgt_ref, ///< selects the primary source for height data
|
||||
(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,///< barometer control selection
|
||||
(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl, ///< GPS control selection
|
||||
|
||||
(ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
|
||||
|
|
@ -725,23 +747,6 @@ private:
|
|||
(ParamExtFloat<px4::params::EKF2_MCOEF>) _param_ekf2_mcoef, ///< propeller momentum drag coefficient (1/s)
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
|
||||
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
|
||||
// Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth
|
||||
// Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2
|
||||
(ParamExtFloat<px4::params::EKF2_ASPD_MAX>)
|
||||
_param_ekf2_aspd_max, ///< upper limit on airspeed used for correction (m/s**2)
|
||||
(ParamExtFloat<px4::params::EKF2_PCOEF_XP>)
|
||||
_param_ekf2_pcoef_xp, ///< static pressure position error coefficient along the positive X body axis
|
||||
(ParamExtFloat<px4::params::EKF2_PCOEF_XN>)
|
||||
_param_ekf2_pcoef_xn, ///< static pressure position error coefficient along the negative X body axis
|
||||
(ParamExtFloat<px4::params::EKF2_PCOEF_YP>)
|
||||
_param_ekf2_pcoef_yp, ///< static pressure position error coefficient along the positive Y body axis
|
||||
(ParamExtFloat<px4::params::EKF2_PCOEF_YN>)
|
||||
_param_ekf2_pcoef_yn, ///< static pressure position error coefficient along the negative Y body axis
|
||||
(ParamExtFloat<px4::params::EKF2_PCOEF_Z>)
|
||||
_param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis
|
||||
#endif // CONFIG_EKF2_BARO_COMPENSATION
|
||||
|
||||
(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
|
||||
|
||||
// Used by EKF-GSF experimental yaw estimator
|
||||
|
|
|
|||
|
|
@ -28,10 +28,18 @@ depends on MODULES_EKF2
|
|||
---help---
|
||||
EKF2 auxiliary velocity fusion support.
|
||||
|
||||
menuconfig EKF2_BAROMETER
|
||||
depends on MODULES_EKF2
|
||||
bool "barometer support"
|
||||
default y
|
||||
---help---
|
||||
EKF2 barometer support.
|
||||
|
||||
menuconfig EKF2_BARO_COMPENSATION
|
||||
depends on MODULES_EKF2
|
||||
bool "barometer compensation support"
|
||||
default y
|
||||
depends on EKF2_BAROMETER
|
||||
---help---
|
||||
EKF2 pressure compensation support.
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue