zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_GPS: check lock type before accepting position and speed information

mission-4.1.18
Holger Steinhaus 10 years ago
parent
a9defa3508
commit
df7c4baed9
1 changed files with 24 additions and 17 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 41
      libraries/AP_GPS/AP_GPS_PX4.cpp

41
libraries/AP_GPS/AP_GPS_PX4.cpp
Unescape View File

@ -57,23 +57,30 @@ AP_GPS_PX4::read(void) @@ -57,23 +57,30 @@ AP_GPS_PX4::read(void)
state.num_sats = _gps_pos.satellites_used;
state.hdop = uint16_t(_gps_pos.eph*100.0f + .5f);
state.location.lat = _gps_pos.lat;
state.location.lng = _gps_pos.lon;
state.location.alt = _gps_pos.alt/10;
state.ground_speed = _gps_pos.vel_m_s;
state.ground_course_cd = int32_t(double(_gps_pos.cog_rad) / M_PI * 18000. +.5);
state.have_vertical_velocity = _gps_pos.vel_ned_valid;
state.velocity.x = _gps_pos.vel_n_m_s;
state.velocity.y = _gps_pos.vel_e_m_s;
state.velocity.z = _gps_pos.vel_d_m_s;
// convert epoch timestamp back to gps epoch - evil hack until we get the genuine
// raw week information (or APM switches to Posix epoch ;-) )
uint64_t epoch_ms = uint64_t(_gps_pos.time_gps_usec/1000.+.5);
uint64_t gps_ms = epoch_ms - DELTA_POSIX_GPS_EPOCH + LEAP_MS_GPS_2014;
state.time_week = uint16_t(gps_ms / uint64_t(MS_PER_WEEK));
state.time_week_ms = uint32_t(gps_ms - uint64_t(state.time_week)*MS_PER_WEEK);
if (_gps_pos.fix_type >= 2) {
state.location.lat = _gps_pos.lat;
state.location.lng = _gps_pos.lon;
state.location.alt = _gps_pos.alt/10;
state.ground_speed = _gps_pos.vel_m_s;
state.ground_course_cd = int32_t(double(_gps_pos.cog_rad) / M_PI * 18000. +.5);
// convert epoch timestamp back to gps epoch - evil hack until we get the genuine
// raw week information (or APM switches to Posix epoch ;-) )
uint64_t epoch_ms = uint64_t(_gps_pos.time_gps_usec/1000.+.5);
uint64_t gps_ms = epoch_ms - DELTA_POSIX_GPS_EPOCH + LEAP_MS_GPS_2014;
state.time_week = uint16_t(gps_ms / uint64_t(MS_PER_WEEK));
state.time_week_ms = uint32_t(gps_ms - uint64_t(state.time_week)*MS_PER_WEEK);
}
if (_gps_pos.fix_type >= 3) {
state.have_vertical_velocity = _gps_pos.vel_ned_valid;
state.velocity.x = _gps_pos.vel_n_m_s;
state.velocity.y = _gps_pos.vel_e_m_s;
state.velocity.z = _gps_pos.vel_d_m_s;
}
else {
state.have_vertical_velocity = false;
}
}
}

Write Preview
Loading…
Cancel
Save