diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 6ff4b2af26..3a399a618a 100644 --- a/ArduCopter/ArduCopter.pde +++ b/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 diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index d9c44ea08f..a85e723229 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/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 diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde index 89010b127e..53fb235dc6 100644 --- a/ArduCopter/leds.pde +++ b/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; } }