Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 02:17:07 +08:00
Code Issues Actions 33 Packages Projects Releases Wiki Activity

Changed gps position topic mostly to SI units and float, removed counters and added specifig timestamps

Browse Source
This commit is contained in:
Julian Oes
2013-02-06 12:41:05 -08:00
parent a79ad17f09
commit fc4be3e728
9 changed files with 139 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/examples/kalman_demo/KalmanNav.cpp
+12 -11
View File
@@ -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);