EKF: Fixed high CPU load

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;
 

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);