forked from Archive/PX4-Autopilot
Set kalman_demo to only publish when it has valid info.
This commit is contained in:
parent
dc5ddb9370
commit
4b2d1690d3
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue