diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index a07280515e..7e89dffb20 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -240,9 +240,7 @@ void KalmanNav::update() if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz _pubTimeStamp = newTimeStamp; - if (_positionInitialized) _pos.update(); - - if (_attitudeInitialized) _att.update(); + updatePublications(); } // output @@ -294,8 +292,13 @@ void KalmanNav::updatePublications() _att.q_valid = true; _att.counter = _navFrames; - // update publications - SuperBlock::updatePublications(); + // selectively update publications, + // do NOT call superblock do-all method + if (_positionInitialized) + _pos.update(); + + if (_attitudeInitialized) + _att.update(); } int KalmanNav::predictState(float dt)