mirror of https://github.com/ArduPilot/ardupilot
Rover: add support for GPS fix type 2D
This commit is contained in:
parent
27309a553f
commit
6cb47cee6b
|
@ -800,7 +800,7 @@ static void update_GPS(void)
|
|||
|
||||
have_position = ahrs.get_position(¤t_loc);
|
||||
|
||||
if (g_gps->new_data && g_gps->status() == GPS::GPS_OK) {
|
||||
if (g_gps->new_data && g_gps->status() >= GPS::GPS_OK_FIX_3D) {
|
||||
gps_fix_count++;
|
||||
|
||||
if(ground_start_count > 1){
|
||||
|
|
|
@ -112,7 +112,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
|||
control_sensors_present |= (1<<2); // compass present
|
||||
}
|
||||
control_sensors_present |= (1<<3); // absolute pressure sensor present
|
||||
if (g_gps != NULL && g_gps->status() == GPS::GPS_OK) {
|
||||
if (g_gps != NULL && g_gps->status() >= GPS::NO_FIX) {
|
||||
control_sensors_present |= (1<<5); // GPS present
|
||||
}
|
||||
control_sensors_present |= (1<<10); // 3D angular rate control
|
||||
|
@ -213,7 +213,7 @@ static void NOINLINE send_location(mavlink_channel_t chan)
|
|||
// positions.
|
||||
// If we don't have a GPS fix then we are dead reckoning, and will
|
||||
// use the current boot time as the fix time.
|
||||
if (g_gps->status() == GPS::GPS_OK) {
|
||||
if (g_gps->status() >= GPS::GPS_OK_FIX_2D) {
|
||||
fix_time = g_gps->last_fix_time;
|
||||
} else {
|
||||
fix_time = millis();
|
||||
|
@ -248,15 +248,10 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
|
|||
|
||||
static void NOINLINE send_gps_raw(mavlink_channel_t chan)
|
||||
{
|
||||
uint8_t fix = g_gps->status();
|
||||
if (fix == GPS::GPS_OK) {
|
||||
fix = 3;
|
||||
}
|
||||
|
||||
mavlink_msg_gps_raw_int_send(
|
||||
chan,
|
||||
g_gps->last_fix_time*(uint64_t)1000,
|
||||
fix,
|
||||
g_gps->status(),
|
||||
g_gps->latitude, // in 1E7 degrees
|
||||
g_gps->longitude, // in 1E7 degrees
|
||||
g_gps->altitude * 10, // in mm
|
||||
|
|
Loading…
Reference in New Issue