Browse Source

added check for duplicate time-stamped GPS messages.

master
Jason Short 13 years ago
parent
commit
8d075015e1
  1. 18
      ArduCopter/ArduCopter.pde

18
ArduCopter/ArduCopter.pde

@ -869,7 +869,8 @@ static int16_t superslow_loopCounter;
static uint32_t loiter_timer; static uint32_t loiter_timer;
// disarms the copter while in Acro or Stabilize mode after 30 seconds of no flight // disarms the copter while in Acro or Stabilize mode after 30 seconds of no flight
static uint8_t auto_disarming_counter; static uint8_t auto_disarming_counter;
// prevents duplicate GPS messages from entering system
static uint32_t last_gps_time;
// Tracks if GPS is enabled based on statup routine // Tracks if GPS is enabled based on statup routine
// If we do not detect GPS at startup, we stop trying and assume GPS is not connected // If we do not detect GPS at startup, we stop trying and assume GPS is not connected
@ -1351,17 +1352,22 @@ static void update_GPS(void)
} }
if (g_gps->new_data && g_gps->fix) { if (g_gps->new_data && g_gps->fix) {
// clear new data flag // clear new data flag
g_gps->new_data = false; g_gps->new_data = false;
// check for duiplicate GPS messages
if(last_gps_time != g_gps->time){
// look for broken GPS
// ---------------
gps_watchdog = 0; gps_watchdog = 0;
// OK to run the nav routines // OK to run the nav routines
// ---------------
nav_ok = true; nav_ok = true;
// for performance // for performance monitoring
// --------------- // --------------------------
gps_fix_count++; gps_fix_count++;
// used to calculate speed in X and Y, iterms // used to calculate speed in X and Y, iterms
@ -1409,6 +1415,10 @@ static void update_GPS(void)
alt_sensor_flag = true; alt_sensor_flag = true;
#endif #endif
} }
// save GPS time so we don't get duplicate reads
last_gps_time = g_gps->time;
}
} }
void update_yaw_mode(void) void update_yaw_mode(void)

Loading…
Cancel
Save