From c2055557f53897c04f94295fbb77bd315adcd087 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 25 Mar 2013 16:25:24 +0900 Subject: [PATCH] Plane: add support for GPS fix type 2D --- ArduPlane/Attitude.pde | 4 ++-- ArduPlane/GCS_Mavlink.pde | 11 +++-------- ArduPlane/commands_logic.pde | 2 +- ArduPlane/navigation.pde | 2 +- ArduPlane/system.pde | 36 +++++++++++++++++++----------------- 5 files changed, 26 insertions(+), 29 deletions(-) diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 5aab62a156..eb6fc774ef 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -384,7 +384,7 @@ static void throttle_slew_limit(int16_t last_throttle) */ static bool auto_takeoff_check(void) { - if (g_gps == NULL || g_gps->status() != GPS::GPS_OK) { + if (g_gps == NULL || g_gps->status() != GPS::GPS_OK_FIX_3D) { // no auto takeoff without GPS lock return false; } @@ -448,7 +448,7 @@ static bool suppress_throttle(void) } if (g_gps != NULL && - g_gps->status() == GPS::GPS_OK && + g_gps->status() >= GPS::GPS_OK_FIX_2D && g_gps->ground_speed >= 500) { // we're moving at more than 5 m/s throttle_suppressed = false; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index a65373d4fc..fb13dff5aa 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -134,7 +134,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 @@ -250,7 +250,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(); @@ -285,15 +285,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 diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 372abc3d28..04d2786387 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -583,7 +583,7 @@ static void do_change_speed() static void do_set_home() { - if (next_nonnav_command.p1 == 1 && g_gps->status() == GPS::GPS_OK) { + if (next_nonnav_command.p1 == 1 && g_gps->status() == GPS::GPS_OK_FIX_3D) { init_home(); } else { home.id = MAV_CMD_NAV_WAYPOINT; diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 3de3f5fa46..96e8705cb8 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -100,7 +100,7 @@ static void calc_gndspeed_undershoot() { // Function is overkill, but here in case we want to add filtering // later - if (g_gps && g_gps->status() == GPS::GPS_OK) { + if (g_gps && g_gps->status() >= GPS::GPS_OK_FIX_2D) { groundspeed_undershoot = (g.min_gndspeed_cm > 0) ? (g.min_gndspeed_cm - g_gps->ground_speed) : 0; } } diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index ad94985ec9..7d96e51594 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -490,25 +490,27 @@ static void update_GPS_light(void) // GPS LED on if we have a fix or Blink GPS LED if we are receiving data // --------------------------------------------------------------------- switch (g_gps->status()) { - case (2): - digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix. - break; - - case (1): - if (g_gps->valid_read == true) { - GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock - if (GPS_light) { - digitalWrite(C_LED_PIN, LED_OFF); - } else { - digitalWrite(C_LED_PIN, LED_ON); + case GPS::NO_FIX: + case GPS::GPS_OK_FIX_2D: + // check if we've blinked since the last gps update + if (g_gps->valid_read) { + g_gps->valid_read = false; + GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock + if (GPS_light) { + digitalWrite(C_LED_PIN, LED_OFF); + }else{ + digitalWrite(C_LED_PIN, LED_ON); + } } - g_gps->valid_read = false; - } - break; + break; - default: - digitalWrite(C_LED_PIN, LED_OFF); - break; + case GPS::GPS_OK_FIX_3D: + digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix AND home is set. + break; + + default: + digitalWrite(C_LED_PIN, LED_OFF); + break; } }