Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: Log vertical position derivative output

Browse Source
This commit is contained in:
Paul Riseborough 2015-10-12 17:30:25 +11:00 committed by Andrew Tridgell
parent 52fd369b2f
commit 9e3d9d15fc
2 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/DataFlash/DataFlash.h
View File

@ -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), \

6
libraries/DataFlash/LogFile.cpp
View File

@ -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