From 95304e27c958c87bd9afb76bcccbd81e9c7de373 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 10 Apr 2014 10:29:30 +1000 Subject: [PATCH] DataFlash: added separate logging of each GPS --- libraries/DataFlash/DataFlash.h | 2 +- libraries/DataFlash/LogFile.cpp | 60 +++++++++++++++++---------------- 2 files changed, 32 insertions(+), 30 deletions(-) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index c246818e8c..e4dcc15654 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -48,7 +48,7 @@ public: void EnableWrites(bool enable) { _writes_enabled = enable; } void Log_Write_Format(const struct LogStructure *structure); void Log_Write_Parameter(const char *name, float value); - void Log_Write_GPS(const AP_GPS &gps, int32_t relative_alt); + void Log_Write_GPS(const AP_GPS &gps, uint8_t instance, int32_t relative_alt); void Log_Write_IMU(const AP_InertialSensor &ins); void Log_Write_RCIN(void); void Log_Write_RCOUT(void); diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 6330565b94..f0c9004c8e 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -662,42 +662,44 @@ void DataFlash_Class::Log_Write_Parameters(void) // Write an GPS packet -void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, int32_t relative_alt) +void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i, int32_t relative_alt) { - const struct Location &loc = gps.location(0); - struct log_GPS pkt = { - LOG_PACKET_HEADER_INIT(LOG_GPS_MSG), - status : (uint8_t)gps.status(0), - gps_week_ms : gps.time_week_ms(0), - gps_week : gps.time_week(0), - num_sats : gps.num_sats(0), - hdop : gps.get_hdop(0), - latitude : loc.lat, - longitude : loc.lng, - rel_altitude : relative_alt, - altitude : loc.alt, - ground_speed : (uint32_t)(gps.ground_speed(0) * 100), - ground_course : gps.ground_course_cd(0), - vel_z : gps.velocity(0).z, - apm_time : hal.scheduler->millis() - }; - WriteBlock(&pkt, sizeof(pkt)); + if (i == 0) { + const struct Location &loc = gps.location(i); + struct log_GPS pkt = { + LOG_PACKET_HEADER_INIT(LOG_GPS_MSG), + status : (uint8_t)gps.status(i), + gps_week_ms : gps.time_week_ms(i), + gps_week : gps.time_week(i), + num_sats : gps.num_sats(i), + hdop : gps.get_hdop(i), + latitude : loc.lat, + longitude : loc.lng, + rel_altitude : relative_alt, + altitude : loc.alt, + ground_speed : (uint32_t)(gps.ground_speed(i) * 100), + ground_course : gps.ground_course_cd(i), + vel_z : gps.velocity(i).z, + apm_time : hal.scheduler->millis() + }; + WriteBlock(&pkt, sizeof(pkt)); + } #if HAL_CPU_CLASS > HAL_CPU_CLASS_16 - if (gps.num_sensors() > 1) { - const struct Location &loc2 = gps.location(1); + if (i > 0) { + const struct Location &loc2 = gps.location(i); struct log_GPS2 pkt2 = { LOG_PACKET_HEADER_INIT(LOG_GPS2_MSG), - status : (uint8_t)gps.status(1), - gps_week_ms : gps.time_week_ms(1), - gps_week : gps.time_week(1), - num_sats : gps.num_sats(1), - hdop : gps.get_hdop(1), + status : (uint8_t)gps.status(i), + gps_week_ms : gps.time_week_ms(i), + gps_week : gps.time_week(i), + num_sats : gps.num_sats(i), + hdop : gps.get_hdop(i), latitude : loc2.lat, longitude : loc2.lng, altitude : loc2.alt, - ground_speed : (uint32_t)(gps.ground_speed(1)*100), - ground_course : gps.ground_course_cd(1), - vel_z : gps.velocity(1).z, + ground_speed : (uint32_t)(gps.ground_speed(i)*100), + ground_course : gps.ground_course_cd(i), + vel_z : gps.velocity(i).z, apm_time : hal.scheduler->millis(), dgps_numch : 0, dgps_age : 0