aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: ensure we log both GPS at the time we receive a message

Browse Source
This commit is contained in:
Andrew Tridgell 2014-04-10 10:30:10 +10:00
parent 4d990fa6c8
commit 1189978334
1 changed files with 13 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduCopter/ArduCopter.pde
View File

@ -1183,21 +1183,28 @@ static void update_optical_flow(void)
// called at 50hz // called at 50hz
static void update_GPS(void) static void update_GPS(void)
{ {
static uint32_t last_gps_reading; // time of last gps message static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; // time of last gps message
static uint8_t ground_start_count = 10; // counter used to grab at least 10 reads before commiting the Home location static uint8_t ground_start_count = 10; // counter used to grab at least 10 reads before commiting the Home location
bool report_gps_glitch; bool report_gps_glitch;
bool gps_updated = false;
gps.update(); gps.update();
// logging and glitch protection run after every gps message // logging and glitch protection run after every gps message
if (gps.last_message_time_ms() != last_gps_reading) { for (uint8_t i=0; i<gps.num_sensors(); i++) {
last_gps_reading = gps.last_message_time_ms(); if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
last_gps_reading[i] = gps.last_message_time_ms(i);
// log GPS message // log GPS message
if (g.log_bitmask & MASK_LOG_GPS) { if (g.log_bitmask & MASK_LOG_GPS) {
DataFlash.Log_Write_GPS(gps, current_loc.alt); DataFlash.Log_Write_GPS(gps, i, current_loc.alt);
} }
gps_updated = true;
}
}
if (gps_updated) {
// run glitch protection and update AP_Notify if home has been initialised // run glitch protection and update AP_Notify if home has been initialised
if (ap.home_is_set) { if (ap.home_is_set) {
gps_glitch.check_position(); gps_glitch.check_position();