diff options
| author | Julian Oes <joes@student.ethz.ch> | 2013-02-06 12:41:05 -0800 |
|---|---|---|
| committer | Julian Oes <joes@student.ethz.ch> | 2013-02-06 12:41:05 -0800 |
| commit | fc4be3e7280db480b67b7c6cec11e35481969bbb (patch) | |
| tree | 90f760ad4022a3ad568a815e3a89e29cd4e47d48 /apps/examples | |
| parent | a79ad17f09ac69bf2a19a4f617fa1df16bcaa6be (diff) | |
| download | px4-firmware-fc4be3e7280db480b67b7c6cec11e35481969bbb.tar.gz px4-firmware-fc4be3e7280db480b67b7c6cec11e35481969bbb.tar.bz2 px4-firmware-fc4be3e7280db480b67b7c6cec11e35481969bbb.zip | |
Changed gps position topic mostly to SI units and float, removed counters and added specifig timestamps
Diffstat (limited to 'apps/examples')
| -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 7e89dffb2..b7f651d8a 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(); @@ -631,8 +632,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 +652,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); |