diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 1b093c00e8..82f95af6c6 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -48,6 +48,7 @@ public: void Log_Write_Format(const struct LogStructure *structure); void Log_Write_Parameter(const char *name, float value); void Log_Write_GPS(const GPS *gps, int32_t relative_alt); + void Log_Write_GPS2(const GPS *gps); void Log_Write_IMU(const AP_InertialSensor &ins); void Log_Write_RCIN(void); void Log_Write_RCOUT(void); @@ -171,6 +172,24 @@ struct PACKED log_GPS { uint32_t apm_time; }; +struct PACKED log_GPS2 { + LOG_PACKET_HEADER; + uint8_t status; + uint32_t gps_week_ms; + uint16_t gps_week; + uint8_t num_sats; + int16_t hdop; + int32_t latitude; + int32_t longitude; + int32_t altitude; + uint32_t ground_speed; + int32_t ground_course; + float vel_z; + uint32_t apm_time; + uint8_t dgps_numch; + uint32_t dgps_age; +}; + struct PACKED log_Message { LOG_PACKET_HEADER; char msg[64]; @@ -306,6 +325,8 @@ struct PACKED log_EKF4 { "PARM", "Nf", "Name,Value" }, \ { LOG_GPS_MSG, sizeof(log_GPS), \ "GPS", "BIHBcLLeeEefI", "Status,TimeMS,Week,NSats,HDop,Lat,Lng,RelAlt,Alt,Spd,GCrs,VZ,T" }, \ + { LOG_GPS2_MSG, sizeof(log_GPS2), \ + "GPS2", "BIHBcLLeEefIBI", "Status,TimeMS,Week,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,T,DSc,DAg" }, \ { LOG_IMU_MSG, sizeof(log_IMU), \ "IMU", "Iffffff", "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ" }, \ { LOG_IMU2_MSG, sizeof(log_IMU), \ @@ -350,6 +371,7 @@ struct PACKED log_EKF4 { #define LOG_EKF2_MSG 141 #define LOG_EKF3_MSG 142 #define LOG_EKF4_MSG 143 +#define LOG_GPS2_MSG 144 #include "DataFlash_Block.h" #include "DataFlash_File.h" diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index add5ffe30b..3a38a58f92 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -673,6 +673,29 @@ void DataFlash_Class::Log_Write_GPS(const GPS *gps, int32_t relative_alt) WriteBlock(&pkt, sizeof(pkt)); } +// Write a GPS2 packet +void DataFlash_Class::Log_Write_GPS2(const GPS *gps) +{ + struct log_GPS2 pkt = { + LOG_PACKET_HEADER_INIT(LOG_GPS2_MSG), + status : (uint8_t)gps->status(), + gps_week_ms : gps->time_week_ms, + gps_week : gps->time_week, + num_sats : gps->num_sats, + hdop : gps->hdop, + latitude : gps->latitude, + longitude : gps->longitude, + altitude : gps->altitude_cm, + ground_speed : gps->ground_speed_cm, + ground_course : gps->ground_course_cd, + vel_z : gps->velocity_down(), + apm_time : hal.scheduler->millis(), + dgps_numch : 0, + dgps_age : 0 + }; + WriteBlock(&pkt, sizeof(pkt)); +} + // Write an RCIN packet void DataFlash_Class::Log_Write_RCIN(void) {