From 5776620062ee07ab5b4e1fb9470035fb7730818a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 5 Aug 2015 15:25:48 +0900 Subject: [PATCH] DataFlash: consolidate GPS, GPS2 messages Remove unused dgps_numch, dgps_age from GPS2 Add U field (for use) to both GPS and GPS2 --- libraries/DataFlash/DataFlash.h | 27 +++----------- libraries/DataFlash/LogFile.cpp | 62 ++++++++++----------------------- 2 files changed, 24 insertions(+), 65 deletions(-) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index ca19001cf0..85866af7c9 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -184,24 +184,7 @@ struct PACKED log_GPS { uint32_t ground_speed; int32_t ground_course; float vel_z; -}; - -struct PACKED log_GPS2 { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t status; - uint32_t gps_week_ms; - uint16_t gps_week; - uint8_t num_sats; - uint16_t hdop; - int32_t latitude; - int32_t longitude; - int32_t altitude; - uint32_t ground_speed; - int32_t ground_course; - float vel_z; - uint8_t dgps_numch; - uint32_t dgps_age; + uint8_t used; }; struct PACKED log_Message { @@ -647,7 +630,7 @@ Format characters in the format string for binary log messages { LOG_PARAMETER_MSG, sizeof(log_Parameter), \ "PARM", "QNf", "TimeUS,Name,Value" }, \ { LOG_GPS_MSG, sizeof(log_GPS), \ - "GPS", "QBIHBcLLeeEef", "TimeUS,Status,GMS,GWk,NSats,HDop,Lat,Lng,RAlt,Alt,Spd,GCrs,VZ" }, \ + "GPS", "QBIHBcLLeeEefB", "TimeUS,Status,GMS,GWk,NSats,HDop,Lat,Lng,RAlt,Alt,Spd,GCrs,VZ,U" }, \ { LOG_IMU_MSG, sizeof(log_IMU), \ "IMU", "QffffffIIfBB", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp,GyHlt,AcHlt" }, \ { LOG_MESSAGE_MSG, sizeof(log_Message), \ @@ -679,8 +662,8 @@ Format characters in the format string for binary log messages // messages for more advanced boards #define LOG_EXTRA_STRUCTURES \ - { LOG_GPS2_MSG, sizeof(log_GPS2), \ - "GPS2", "QBIHBcLLeEefBI", "TimeUS,Status,GMS,GWk,NSats,HDp,Lat,Lng,Alt,Spd,GCrs,VZ,DSc,DAg" }, \ + { LOG_GPS2_MSG, sizeof(log_GPS), \ + "GPS2", "QBIHBcLLeeEefB", "TimeUS,Status,GMS,GWk,NSats,HDop,Lat,Lng,RAlt,Alt,Spd,GCrs,VZ,U" }, \ { LOG_IMU2_MSG, sizeof(log_IMU), \ "IMU2", "QffffffIIfBB", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp,GyHlt,AcHlt" }, \ { LOG_IMU3_MSG, sizeof(log_IMU), \ @@ -783,6 +766,7 @@ enum LogMessages { LOG_FORMAT_MSG = 128, LOG_PARAMETER_MSG, LOG_GPS_MSG, + LOG_GPS2_MSG, LOG_IMU_MSG, LOG_MESSAGE_MSG, LOG_RCIN_MSG, @@ -796,7 +780,6 @@ enum LogMessages { LOG_EKF2_MSG, LOG_EKF3_MSG, LOG_EKF4_MSG, - LOG_GPS2_MSG, LOG_CMD_MSG, LOG_RADIO_MSG, LOG_ATRP_MSG, diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 4abdf988ea..9abc27c314 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -706,49 +706,25 @@ void DataFlash_Class::Log_Write_Parameters(void) // Write an GPS packet void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i, int32_t relative_alt) { - if (i == 0) { - const struct Location &loc = gps.location(i); - struct log_GPS pkt = { - LOG_PACKET_HEADER_INIT(LOG_GPS_MSG), - time_us : hal.scheduler->micros64(), - 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, - }; - WriteBlock(&pkt, sizeof(pkt)); - } -#if HAL_CPU_CLASS > HAL_CPU_CLASS_16 - if (i > 0) { - const struct Location &loc2 = gps.location(i); - struct log_GPS2 pkt2 = { - LOG_PACKET_HEADER_INIT(LOG_GPS2_MSG), - time_us : hal.scheduler->micros64(), - 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(i)*100), - ground_course : gps.ground_course_cd(i), - vel_z : gps.velocity(i).z, - dgps_numch : 0, - dgps_age : 0 - }; - WriteBlock(&pkt2, sizeof(pkt2)); - } -#endif + const struct Location &loc = gps.location(i); + struct log_GPS pkt = { + LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GPS_MSG+i)), + time_us : hal.scheduler->micros64(), + 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, + used : (uint8_t)(gps.primary_sensor() == i) + }; + WriteBlock(&pkt, sizeof(pkt)); } // Write an RCIN packet