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

Set kalman_demo to only publish when it has valid info.

This commit is contained in:
James Goppert 2013-01-18 10:21:20 -05:00
parent dc5ddb9370
commit 4b2d1690d3
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
apps/examples/kalman_demo/KalmanNav.cpp
View File

@ -239,7 +239,10 @@ void KalmanNav::update()
// publication // publication
if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
_pubTimeStamp = newTimeStamp; _pubTimeStamp = newTimeStamp;
updatePublications();
if (_positionInitialized) _pos.update();
if (_attitudeInitialized) _att.update();
} }
// output // output
@ -589,6 +592,7 @@ int KalmanNav::correctAtt()
} }
psi += xCorrect(PSI); psi += xCorrect(PSI);
// attitude also affects nav velocities // attitude also affects nav velocities
if (_positionInitialized) { if (_positionInitialized) {
vN += xCorrect(VN); vN += xCorrect(VN);