aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

DataFlash: Log EKF origin height

Browse Source
This commit is contained in:
priseborough 2017-05-30 11:59:55 +10:00 committed by Francisco Ferreira
parent a3483d0d34
commit 798d5f85a2
2 changed files with 27 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/DataFlash/LogFile.cpp
View File

@ -1117,12 +1117,16 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
Vector3f dVelBias;
Vector3f gyroBias;
float posDownDeriv;
Location originLLH;
ahrs.get_NavEKF2().getEulerAngles(0,euler);
ahrs.get_NavEKF2().getVelNED(0,velNED);
ahrs.get_NavEKF2().getPosNE(0,posNE);
ahrs.get_NavEKF2().getPosD(0,posD);
ahrs.get_NavEKF2().getGyroBias(0,gyroBias);
posDownDeriv = ahrs.get_NavEKF2().getPosDownDerivative(0);
if (!ahrs.get_NavEKF2().getOriginLLH(0,originLLH)) {
originLLH.alt = 0;
}
struct log_EKF1 pkt = {
LOG_PACKET_HEADER_INIT(LOG_NKF1_MSG),
time_us : time_us,
@ -1138,7 +1142,8 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
posD : (float)(posD), // metres Down
gyrX : (int16_t)(100*degrees(gyroBias.x)), // cd/sec, displayed as deg/sec due to format string
gyrY : (int16_t)(100*degrees(gyroBias.y)), // cd/sec, displayed as deg/sec due to format string
gyrZ : (int16_t)(100*degrees(gyroBias.z)) // cd/sec, displayed as deg/sec due to format string
gyrZ : (int16_t)(100*degrees(gyroBias.z)), // cd/sec, displayed as deg/sec due to format string
originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm
};
WriteBlock(&pkt, sizeof(pkt));
@ -1288,6 +1293,9 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
ahrs.get_NavEKF2().getPosD(1,posD);
ahrs.get_NavEKF2().getGyroBias(1,gyroBias);
posDownDeriv = ahrs.get_NavEKF2().getPosDownDerivative(1);
if (!ahrs.get_NavEKF2().getOriginLLH(1,originLLH)) {
originLLH.alt = 0;
}
struct log_EKF1 pkt6 = {
LOG_PACKET_HEADER_INIT(LOG_NKF6_MSG),
time_us : time_us,
@ -1303,7 +1311,8 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
posD : (float)(posD), // metres Down
gyrX : (int16_t)(100*degrees(gyroBias.x)), // cd/sec, displayed as deg/sec due to format string
gyrY : (int16_t)(100*degrees(gyroBias.y)), // cd/sec, displayed as deg/sec due to format string
gyrZ : (int16_t)(100*degrees(gyroBias.z)) // cd/sec, displayed as deg/sec due to format string
gyrZ : (int16_t)(100*degrees(gyroBias.z)), // cd/sec, displayed as deg/sec due to format string
originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm
};
WriteBlock(&pkt6, sizeof(pkt6));
@ -1450,12 +1459,16 @@ void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
Vector3f dVelBias;
Vector3f gyroBias;
float posDownDeriv;
Location originLLH;
ahrs.get_NavEKF3().getEulerAngles(0,euler);
ahrs.get_NavEKF3().getVelNED(0,velNED);
ahrs.get_NavEKF3().getPosNE(0,posNE);
ahrs.get_NavEKF3().getPosD(0,posD);
ahrs.get_NavEKF3().getGyroBias(0,gyroBias);
posDownDeriv = ahrs.get_NavEKF3().getPosDownDerivative(0);
if (!ahrs.get_NavEKF3().getOriginLLH(0,originLLH)) {
originLLH.alt = 0;
}
struct log_EKF1 pkt = {
LOG_PACKET_HEADER_INIT(LOG_XKF1_MSG),
time_us : time_us,
@ -1471,7 +1484,8 @@ void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
posD : (float)(posD), // metres Down
gyrX : (int16_t)(100*degrees(gyroBias.x)), // cd/sec, displayed as deg/sec due to format string
gyrY : (int16_t)(100*degrees(gyroBias.y)), // cd/sec, displayed as deg/sec due to format string
gyrZ : (int16_t)(100*degrees(gyroBias.z)) // cd/sec, displayed as deg/sec due to format string
gyrZ : (int16_t)(100*degrees(gyroBias.z)), // cd/sec, displayed as deg/sec due to format string
originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm
};
WriteBlock(&pkt, sizeof(pkt));
@ -1618,6 +1632,9 @@ void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
ahrs.get_NavEKF3().getPosD(1,posD);
ahrs.get_NavEKF3().getGyroBias(1,gyroBias);
posDownDeriv = ahrs.get_NavEKF3().getPosDownDerivative(1);
if (!ahrs.get_NavEKF3().getOriginLLH(1,originLLH)) {
originLLH.alt = 0;
}
struct log_EKF1 pkt6 = {
LOG_PACKET_HEADER_INIT(LOG_XKF6_MSG),
time_us : time_us,
@ -1633,7 +1650,8 @@ void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
posD : (float)(posD), // metres Down
gyrX : (int16_t)(100*degrees(gyroBias.x)), // cd/sec, displayed as deg/sec due to format string
gyrY : (int16_t)(100*degrees(gyroBias.y)), // cd/sec, displayed as deg/sec due to format string
gyrZ : (int16_t)(100*degrees(gyroBias.z)) // cd/sec, displayed as deg/sec due to format string
gyrZ : (int16_t)(100*degrees(gyroBias.z)), // cd/sec, displayed as deg/sec due to format string
originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm
};
WriteBlock(&pkt6, sizeof(pkt6));

9
libraries/DataFlash/LogStructure.h
View File

@ -241,6 +241,7 @@ struct PACKED log_EKF1 {
int16_t gyrX;
int16_t gyrY;
int16_t gyrZ;
int32_t originHgt;
};
struct PACKED log_EKF2 {
@ -978,7 +979,7 @@ Format characters in the format string for binary log messages
{ LOG_SIMSTATE_MSG, sizeof(log_AHRS), \
"SIM","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4" }, \
{ LOG_NKF1_MSG, sizeof(log_EKF1), \
"NKF1","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \
"NKF1","QccCfffffffccce","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH" }, \
{ LOG_NKF2_MSG, sizeof(log_NKF2), \
"NKF2","QbccccchhhhhhB","TimeUS,AZbias,GSX,GSY,GSZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI" }, \
{ LOG_NKF3_MSG, sizeof(log_NKF3), \
@ -988,7 +989,7 @@ Format characters in the format string for binary log messages
{ LOG_NKF5_MSG, sizeof(log_NKF5), \
"NKF5","QBhhhcccCCfff","TimeUS,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,eAng,eVel,ePos" }, \
{ LOG_NKF6_MSG, sizeof(log_EKF1), \
"NKF6","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \
"NKF6","QccCfffffffccce","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH" }, \
{ LOG_NKF7_MSG, sizeof(log_NKF2), \
"NKF7","QbccccchhhhhhB","TimeUS,AZbias,GSX,GSY,GSZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI" }, \
{ LOG_NKF8_MSG, sizeof(log_NKF3), \
@ -1000,7 +1001,7 @@ Format characters in the format string for binary log messages
{ LOG_NKQ1_MSG, sizeof(log_Quaternion), "NKQ1", QUAT_FMT, QUAT_LABELS }, \
{ LOG_NKQ2_MSG, sizeof(log_Quaternion), "NKQ2", QUAT_FMT, QUAT_LABELS }, \
{ LOG_XKF1_MSG, sizeof(log_EKF1), \
"XKF1","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \
"XKF1","QccCfffffffccce","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH" }, \
{ LOG_XKF2_MSG, sizeof(log_NKF2a), \
"XKF2","QccccchhhhhhB","TimeUS,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI" }, \
{ LOG_XKF3_MSG, sizeof(log_NKF3), \
@ -1010,7 +1011,7 @@ Format characters in the format string for binary log messages
{ LOG_XKF5_MSG, sizeof(log_NKF5), \
"XKF5","QBhhhcccCCfff","TimeUS,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,eAng,eVel,ePos" }, \
{ LOG_XKF6_MSG, sizeof(log_EKF1), \
"XKF6","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \
"XKF6","QccCfffffffccce","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH" }, \
{ LOG_XKF7_MSG, sizeof(log_NKF2a), \
"XKF7","QccccchhhhhhB","TimeUS,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI" }, \
{ LOG_XKF8_MSG, sizeof(log_NKF3), \