fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 07:11:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: baro bias publish minor cleanup

Browse Source 
- naming consistency (estimator prefix as "namespace")
 - only publish if baro is available and bias is changing as a small logging optimization
 - avoid unnecessary copying (get const reference to status directly)
 - trivial code style fixes
This commit is contained in:
Daniel Agar
2021-08-02 13:59:38 -04:00
parent d997a8d308
commit 93aa6e3f78
9 changed files with 37 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+2 -1
View File
@@ -298,7 +298,8 @@ public:
// returns false when data is not available
bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
void getBaroBiasEstimatorStatus(float &bias, float &bias_var, float &innov, float &innov_var, float &innov_test_ratio);
const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
private:
src/modules/ekf2/EKF/ekf_helper.cpp
+3 -14
View File
@@ -1320,9 +1320,8 @@ void Ekf::updateBaroHgtOffset()
// apply a 10 second first order low pass filter to baro offset
const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
;
const float offset_rate_correction = 0.1f * (unbiased_baro + _state.pos(2) -
_baro_hgt_offset);
const float offset_rate_correction = 0.1f * (unbiased_baro + _state.pos(2) - _baro_hgt_offset);
_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
}
}
@@ -1348,7 +1347,7 @@ void Ekf::updateBaroHgtBias()
}
if (_gps_data_ready && !_gps_hgt_intermittent
&& _gps_checks_passed &&_NED_origin_initialised
&& _gps_checks_passed && _NED_origin_initialised
&& !_baro_hgt_faulty) {
// Use GPS altitude as a reference to compute the baro bias measurement
const float baro_bias = (_baro_sample_delayed.hgt - _baro_hgt_offset)
@@ -1358,16 +1357,6 @@ void Ekf::updateBaroHgtBias()
}
}
void Ekf::getBaroBiasEstimatorStatus(float &bias, float &bias_var, float &innov, float &innov_var, float &innov_test_ratio)
{
BaroBiasEstimator::status status = _baro_b_est.getStatus();
bias = status.bias;
bias_var = status.bias_var;
innov = status.innov;
innov_var = status.innov_var;
innov_test_ratio = status.innov_test_ratio;
}
Vector3f Ekf::getVisionVelocityInEkfFrame() const
{
Vector3f vel;