diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-02-18 16:46:05 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-02-18 16:46:05 +0100 |
| commit | 663ca58063a281d23dbc92a6fbd19011c3fbde41 (patch) | |
| tree | 5a2e9f58a8f41db94ef221e12acead09c9828233 /apps/examples/kalman_demo/KalmanNav.cpp | |
| parent | 104d5aa3654545b354f25750d3980181da8f6a0b (diff) | |
| parent | 520a2b417410bed7db6f08a3a69f3bcccc55910b (diff) | |
| download | px4-firmware-663ca58063a281d23dbc92a6fbd19011c3fbde41.tar.gz px4-firmware-663ca58063a281d23dbc92a6fbd19011c3fbde41.tar.bz2 px4-firmware-663ca58063a281d23dbc92a6fbd19011c3fbde41.zip | |
Merge branch 'master' of github.com:PX4/Firmware
Diffstat (limited to 'apps/examples/kalman_demo/KalmanNav.cpp')
| -rw-r--r-- | apps/examples/kalman_demo/KalmanNav.cpp | 23 |
1 files changed, 12 insertions, 11 deletions
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 1d59f9677..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(); @@ -630,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; @@ -650,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); |