diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 9749c8c21c..d62c61ecb1 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -358,6 +358,7 @@ struct PACKED log_EKF1 { float velN; float velE; float velD; + float posD_dot; float posN; float posE; float posD; @@ -817,7 +818,7 @@ Format characters in the format string for binary log messages { LOG_SIMSTATE_MSG, sizeof(log_AHRS), \ "SIM","QccCfLL","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \ { LOG_EKF1_MSG, sizeof(log_EKF1), \ - "EKF1","QccCffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,PN,PE,PD,GX,GY,GZ" }, \ + "EKF1","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \ { LOG_EKF2_MSG, sizeof(log_EKF2), \ "EKF2","Qbbbcchhhhhh","TimeUS,Ratio,AZ1bias,AZ2bias,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, \ { LOG_EKF3_MSG, sizeof(log_EKF3), \ @@ -827,7 +828,7 @@ Format characters in the format string for binary log messages { LOG_EKF5_MSG, sizeof(log_EKF5), \ "EKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \ { LOG_NKF1_MSG, sizeof(log_EKF1), \ - "NKF1","QccCffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,PN,PE,PD,GX,GY,GZ" }, \ + "NKF1","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \ { LOG_NKF2_MSG, sizeof(log_NKF2), \ "NKF2","Qbccccchhhhhh","TimeUS,AZbias,GSX,GSY,GSZ,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, \ { LOG_NKF3_MSG, sizeof(log_NKF3), \ diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index b96aa6bc32..93a3402912 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -1144,10 +1144,12 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) Vector3f dAngBias; Vector3f dVelBias; Vector3f gyroBias; + float posDownDeriv; ahrs.get_NavEKF().getEulerAngles(euler); ahrs.get_NavEKF().getVelNED(velNED); ahrs.get_NavEKF().getPosNED(posNED); ahrs.get_NavEKF().getGyroBias(gyroBias); + ahrs.get_NavEKF().getPosDownDerivative(posDownDeriv); struct log_EKF1 pkt = { LOG_PACKET_HEADER_INIT(LOG_EKF1_MSG), time_us : hal.scheduler->micros64(), @@ -1157,6 +1159,7 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) velN : (float)(velNED.x), // velocity North (m/s) velE : (float)(velNED.y), // velocity East (m/s) velD : (float)(velNED.z), // velocity Down (m/s) + posD_dot : (float)(posDownDeriv), // first derivative of down position posN : (float)(posNED.x), // metres North posE : (float)(posNED.y), // metres East posD : (float)(posNED.z), // metres Down @@ -1294,10 +1297,12 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) Vector3f dAngBias; Vector3f dVelBias; Vector3f gyroBias; + float posDownDeriv; ahrs.get_NavEKF2().getEulerAngles(euler); ahrs.get_NavEKF2().getVelNED(velNED); ahrs.get_NavEKF2().getPosNED(posNED); ahrs.get_NavEKF2().getGyroBias(gyroBias); + ahrs.get_NavEKF2().getPosDownDerivative(posDownDeriv); struct log_EKF1 pkt = { LOG_PACKET_HEADER_INIT(LOG_NKF1_MSG), time_us : hal.scheduler->micros64(), @@ -1307,6 +1312,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) velN : (float)(velNED.x), // velocity North (m/s) velE : (float)(velNED.y), // velocity East (m/s) velD : (float)(velNED.z), // velocity Down (m/s) + posD_dot : (float)(posDownDeriv), // first derivative of down position posN : (float)(posNED.x), // metres North posE : (float)(posNED.y), // metres East posD : (float)(posNED.z), // metres Down