From 4b2d1690d33c2ae9248abd9fb025e8a1a30fbe84 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Fri, 18 Jan 2013 10:21:20 -0500
Subject: Set kalman_demo to only publish when it has valid info.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

(limited to 'apps/examples')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 40166f3a1..a07280515 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -239,7 +239,10 @@ void KalmanNav::update()
 	// publication
 	if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
 		_pubTimeStamp = newTimeStamp;
-		updatePublications();
+
+		if (_positionInitialized) _pos.update();
+
+		if (_attitudeInitialized) _att.update();
 	}
 
 	// output
@@ -589,6 +592,7 @@ int KalmanNav::correctAtt()
 	}
 
 	psi += xCorrect(PSI);
+
 	// attitude also affects nav velocities
 	if (_positionInitialized) {
 		vN += xCorrect(VN);
-- 
cgit v1.2.3