diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index e4d924396a..57df913069 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -133,6 +133,8 @@ public: */ int set_debuglevel(unsigned debug) { _debug = debug; return 0; } + static constexpr unsigned MAX_PREDICITION_STEPS = 3; /**< maximum number of prediction steps between updates */ + private: bool _task_should_exit; /**< if true, sensor task should exit */ bool _task_running; /**< if true, task is running in its mainloop */ @@ -174,6 +176,7 @@ private: hrt_abstime _last_accel; hrt_abstime _last_mag; + unsigned _prediction_steps; struct sensor_combined_s _sensor_combined; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index d23dd1b9e9..6bf8656dd9 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -162,6 +162,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _last_accel(0), _last_mag(0), + _prediction_steps(0), _sensor_combined{}, @@ -1001,6 +1002,14 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; _covariancePredictionDt += _ekf->dtIMU; + // only fuse every few steps + if (_prediction_steps < MAX_PREDICITION_STEPS) { + _prediction_steps++; + return; + } else { + _prediction_steps = 0; + } + // perform a covariance prediction if the total delta angle has exceeded the limit // or the time limit will be exceeded at the next IMU update if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) @@ -1117,7 +1126,7 @@ int AttitudePositionEstimatorEKF::start() _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 4800, + 4200, (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr);