Copter: add support for GPS fix type 2D

ArduCopter/ArduCopter.pde

@@ -1374,52 +1374,48 @@ static void update_GPS(void)
     g_gps->update();
     update_GPS_light();
 
-    set_gps_healthy(g_gps->status() == g_gps->GPS_OK);
+    set_gps_healthy(g_gps->status() >= GPS::GPS_OK_FIX_3D);
 
-    if (g_gps->new_data && g_gps->fix) {
+    if (g_gps->new_data && last_gps_time != g_gps->time && g_gps->status() >= GPS::GPS_OK_FIX_2D) {
         // clear new data flag
         g_gps->new_data = false;
 
-        // check for duiplicate GPS messages
-        if(last_gps_time != g_gps->time) {
-
-            // for performance monitoring
-            // --------------------------
-            gps_fix_count++;
-
-            if(ground_start_count > 1) {
-                ground_start_count--;
+        // save GPS time so we don't get duplicate reads
+        last_gps_time = g_gps->time;
 
-            } else if (ground_start_count == 1) {
+        // log location if we have at least a 2D fix
+        if (g.log_bitmask & MASK_LOG_GPS && motors.armed()) {
+            Log_Write_GPS();
+        }
 
-                // We countdown N number of good GPS fixes
-                // so that the altitude is more accurate
-                // -------------------------------------
-                if (g_gps->latitude == 0) {
-                    ground_start_count = 5;
+        // for performance monitoring
+        gps_fix_count++;
 
+        // check if we can initialise home yet
+        if (!ap.home_is_set) {
+            // if we have a 3d lock and valid location
+            if(g_gps->status() >= GPS::GPS_OK_FIX_3D && g_gps->latitude != 0) {
+                if( ground_start_count > 0 ) {
+                    ground_start_count--;
                 }else{
+                    // after 10 successful reads store home location
+                    // ap.home_is_set will be true so this will only happen once
+                    ground_start_count = 0;
+                    init_home();
                     if (g.compass_enabled) {
                         // Set compass declination automatically
                         compass.set_initial_location(g_gps->latitude, g_gps->longitude);
                     }
-                    // save home to eeprom (we must have a good fix to have reached this point)
-                    init_home();
-                    ground_start_count = 0;
                 }
-            }
-
-            if (g.log_bitmask & MASK_LOG_GPS && motors.armed()) {
-                Log_Write_GPS();
+            }else{
+                // start again if we lose 3d lock
+                ground_start_count = 10;
             }
 
 #if HIL_MODE == HIL_MODE_ATTITUDE                                                               // only execute in HIL mode
             ap_system.alt_sensor_flag = true;
 #endif
         }
-
-        // save GPS time so we don't get duplicate reads
-        last_gps_time = g_gps->time;
     }
 
     // check for loss of gps

ArduCopter/GCS_Mavlink.pde

@@ -132,7 +132,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
@@ -225,7 +225,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();
@@ -289,15 +289,10 @@ static void NOINLINE send_hwstatus(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

ArduCopter/leds.pde

@@ -19,30 +19,31 @@ 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 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;
+                ap_system.GPS_light = !ap_system.GPS_light;                     // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
+                if (ap_system.GPS_light) {
+                    digitalWriteFast(C_LED_PIN, LED_OFF);
+                }else{
+                    digitalWriteFast(C_LED_PIN, LED_ON);
+                }
+            }
+            break;
 
-    case (2):
-        if(ap.home_is_set) {                                      // JLN update
-            digitalWriteFast(C_LED_PIN, LED_ON);                  //Turn LED C on when gps has valid fix AND home is set.
-        } else {
-            digitalWriteFast(C_LED_PIN, LED_OFF);
-        }
-        break;
-
-    case (1):
-        if (g_gps->valid_read == true) {
-            ap_system.GPS_light = !ap_system.GPS_light;                     // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
-            if (ap_system.GPS_light) {
+        case GPS::GPS_OK_FIX_3D:
+            if(ap.home_is_set) {
+                digitalWriteFast(C_LED_PIN, LED_ON);                  //Turn LED C on when gps has valid fix AND home is set.
+            } else {
                 digitalWriteFast(C_LED_PIN, LED_OFF);
-            }else{
-                digitalWriteFast(C_LED_PIN, LED_ON);
             }
-            g_gps->valid_read = false;
-        }
-        break;
+            break;
 
-    default:
-        digitalWriteFast(C_LED_PIN, LED_OFF);
-        break;
+        default:
+            digitalWriteFast(C_LED_PIN, LED_OFF);
+            break;
     }
 }