diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-02-16 18:01:15 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-02-16 18:01:15 +0100 |
commit | 1c98343e7bdb27839ea9528854315e23b67c82e2 (patch) | |
tree | c8eb9b64a52cd356958dce5c69426e80ed57f29e /apps/examples | |
parent | a0780a20b5e10eafa1ddf7e16bce788b94514bba (diff) | |
parent | 781845587cf98e5aafbccdb4ace45e014cc2d043 (diff) | |
download | px4-firmware-1c98343e7bdb27839ea9528854315e23b67c82e2.tar.gz px4-firmware-1c98343e7bdb27839ea9528854315e23b67c82e2.tar.bz2 px4-firmware-1c98343e7bdb27839ea9528854315e23b67c82e2.zip |
Merge branch 'master' of github.com:PX4/Firmware
Diffstat (limited to 'apps/examples')
-rw-r--r-- | apps/examples/kalman_demo/KalmanNav.cpp | 24 |
1 files changed, 12 insertions, 12 deletions
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 7e89dffb2..db851221b 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -190,11 +190,12 @@ void KalmanNav::update() if (!_positionInitialized && _attitudeInitialized && // wait for attitude first gpsUpdate && - _gps.fix_type > 2 && - _gps.counter_pos_valid > 10) { - vN = _gps.vel_n; - vE = _gps.vel_e; - vD = _gps.vel_d; + _gps.fix_type > 2 + //&& _gps.counter_pos_valid > 10 + ) { + vN = _gps.vel_n_m_s; + vE = _gps.vel_e_m_s; + vD = _gps.vel_d_m_s; setLatDegE7(_gps.lat); setLonDegE7(_gps.lon); setAltE3(_gps.alt); @@ -259,7 +260,7 @@ void KalmanNav::updatePublications() // position publication _pos.timestamp = _pubTimeStamp; - _pos.time_gps_usec = _gps.timestamp; + _pos.time_gps_usec = _gps.timestamp_position; _pos.valid = true; _pos.lat = getLatDegE7(); _pos.lon = getLonDegE7(); @@ -290,7 +291,6 @@ void KalmanNav::updatePublications() _att.R_valid = true; _att.q_valid = true; - _att.counter = _navFrames; // selectively update publications, // do NOT call superblock do-all method @@ -631,8 +631,8 @@ int KalmanNav::correctPos() // residual Vector y(5); - y(0) = _gps.vel_n - vN; - y(1) = _gps.vel_e - vE; + y(0) = _gps.vel_n_m_s - vN; + y(1) = _gps.vel_e_m_s - vE; y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG; y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG; y(4) = double(_gps.alt) / 1.0e3 - alt; @@ -651,9 +651,9 @@ int KalmanNav::correctPos() // abort correction and return printf("[kalman_demo] numerical failure in gps correction\n"); // fallback to GPS - vN = _gps.vel_n; - vE = _gps.vel_e; - vD = _gps.vel_d; + vN = _gps.vel_n_m_s; + vE = _gps.vel_e_m_s; + vD = _gps.vel_d_m_s; setLatDegE7(_gps.lat); setLonDegE7(_gps.lon); setAltE3(_gps.alt); |