dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Retain minimum Kalman update rate

This commit is contained in:
Lorenz Meier 2015-10-14 12:12:58 +02:00
parent c599cf1256
commit 4d7cc41921
2 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -177,6 +177,7 @@ private:
hrt_abstime _last_accel; hrt_abstime _last_accel;
hrt_abstime _last_mag; hrt_abstime _last_mag;
unsigned _prediction_steps; unsigned _prediction_steps;
uint64_t _prediction_last;
struct sensor_combined_s _sensor_combined; struct sensor_combined_s _sensor_combined;

4
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -157,6 +157,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_last_accel(0), _last_accel(0),
_last_mag(0), _last_mag(0),
_prediction_steps(0), _prediction_steps(0),
_prediction_last(0),
_sensor_combined{}, _sensor_combined{},
@ -997,11 +998,12 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
_covariancePredictionDt += _ekf->dtIMU; _covariancePredictionDt += _ekf->dtIMU;
// only fuse every few steps // only fuse every few steps
if (_prediction_steps < MAX_PREDICITION_STEPS) { if (_prediction_steps < MAX_PREDICITION_STEPS && ((hrt_absolute_time() - _prediction_last) < 20 * 1000)) {
_prediction_steps++; _prediction_steps++;
return; return;
} else { } else {
_prediction_steps = 0; _prediction_steps = 0;
_prediction_last = hrt_absolute_time();
} }
// perform a covariance prediction if the total delta angle has exceeded the limit // perform a covariance prediction if the total delta angle has exceeded the limit