diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index fd5b68917e..ea1c342e49 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -722,7 +722,7 @@ void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i) longitude : loc.lng, altitude : loc.alt, ground_speed : gps.ground_speed(i), - ground_course : gps.ground_course_cd(i), + ground_course : gps.ground_course(i), vel_z : gps.velocity(i).z, used : (uint8_t)(gps.primary_sensor() == i) }; @@ -739,7 +739,8 @@ void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i) vdop : gps.get_vdop(i), hacc : (uint16_t)(hacc*100), vacc : (uint16_t)(vacc*100), - sacc : (uint16_t)(sacc*100) + sacc : (uint16_t)(sacc*100), + have_vv : (uint8_t)gps.have_vertical_velocity(i) }; WriteBlock(&pkt2, sizeof(pkt2)); } diff --git a/libraries/DataFlash/LogStructure.h b/libraries/DataFlash/LogStructure.h index a524f9499a..796453ff57 100644 --- a/libraries/DataFlash/LogStructure.h +++ b/libraries/DataFlash/LogStructure.h @@ -53,7 +53,7 @@ struct PACKED log_GPS { int32_t longitude; int32_t altitude; float ground_speed; - int32_t ground_course; + float ground_course; float vel_z; uint8_t used; }; @@ -65,6 +65,7 @@ struct PACKED log_GPA { uint16_t hacc; uint16_t vacc; uint16_t sacc; + uint8_t have_vv; }; struct PACKED log_Message { @@ -715,13 +716,13 @@ 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", "QBIHBcLLefefB", "TimeUS,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,U" }, \ + "GPS", "QBIHBcLLefffB", "TimeUS,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,U" }, \ { LOG_GPS2_MSG, sizeof(log_GPS), \ "GPS2", "QBIHBcLLefefB", "TimeUS,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,U" }, \ { LOG_GPA_MSG, sizeof(log_GPA), \ - "GPA", "QCCCC", "TimeUS,VDop,HAcc,VAcc,SAcc" }, \ + "GPA", "QCCCCB", "TimeUS,VDop,HAcc,VAcc,SAcc,VV" }, \ { LOG_GPA2_MSG, sizeof(log_GPA), \ - "GPA2", "QCCCC", "TimeUS,VDop,HAcc,VAcc,SAcc" }, \ + "GPA2", "QCCCCB", "TimeUS,VDop,HAcc,VAcc,SAcc,VV" }, \ { 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), \