mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 09:58:28 -04:00
DataFlash: added logging of EKF2
This commit is contained in:
Andrew Tridgell
parent
13f72e5ba9
commit
ce9fa45b3a
@ -102,6 +102,7 @@ public:
|
||||
void Log_Write_POS(AP_AHRS &ahrs);
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||
void Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled);
|
||||
void Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled);
|
||||
#endif
|
||||
bool Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd);
|
||||
void Log_Write_Radio(const mavlink_radio_t &packet);
|
||||
@ -781,6 +782,18 @@ Format characters in the format string for binary log messages
|
||||
"EKF3","Qcccccchhhc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \
|
||||
{ LOG_EKF4_MSG, sizeof(log_EKF4), \
|
||||
"EKF4","QcccccccbbBBH","TimeUS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,TS,SS" }, \
|
||||
{ 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" }, \
|
||||
{ LOG_NKF2_MSG, sizeof(log_EKF2), \
|
||||
"NKF2","Qbbbcchhhhhh","TimeUS,Ratio,AZ1bias,AZ2bias,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, \
|
||||
{ LOG_NKF3_MSG, sizeof(log_EKF3), \
|
||||
"NKF3","Qcccccchhhc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \
|
||||
{ LOG_NKF4_MSG, sizeof(log_EKF4), \
|
||||
"NKF4","QcccccccbbBBH","TimeUS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,TS,SS" }, \
|
||||
{ LOG_NKF5_MSG, sizeof(log_EKF5), \
|
||||
"NKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \
|
||||
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
|
||||
"TERR","QBLLHffHH","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded" }, \
|
||||
{ LOG_UBX1_MSG, sizeof(log_Ubx1), \
|
||||
@ -813,8 +826,6 @@ Format characters in the format string for binary log messages
|
||||
"ESC7", "Qcccc", "TimeUS,RPM,Volt,Curr,Temp" }, \
|
||||
{ LOG_ESC8_MSG, sizeof(log_Esc), \
|
||||
"ESC8", "Qcccc", "TimeUS,RPM,Volt,Curr,Temp" }, \
|
||||
{ LOG_EKF5_MSG, sizeof(log_EKF5), \
|
||||
"EKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \
|
||||
{ LOG_COMPASS2_MSG, sizeof(log_Compass), \
|
||||
"MAG2","QhhhhhhhhhB", "TimeUS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ,Health" }, \
|
||||
{ LOG_COMPASS3_MSG, sizeof(log_Compass), \
|
||||
@ -938,6 +949,11 @@ enum LogMessages {
|
||||
LOG_GPA2_MSG,
|
||||
LOG_RFND_MSG,
|
||||
LOG_BAR3_MSG,
|
||||
LOG_NKF1_MSG,
|
||||
LOG_NKF2_MSG,
|
||||
LOG_NKF3_MSG,
|
||||
LOG_NKF4_MSG,
|
||||
LOG_NKF5_MSG,
|
||||
};
|
||||
|
||||
enum LogOriginType {
|
||||
|
||||
@ -1274,6 +1274,154 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
|
||||
};
|
||||
WriteBlock(&pkt5, sizeof(pkt5));
|
||||
}
|
||||
|
||||
// do EKF2 as well if enabled
|
||||
if (ahrs.get_NavEKF2().enabled()) {
|
||||
Log_Write_EKF2(ahrs, optFlowEnabled);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
|
||||
{
|
||||
// Write first EKF packet
|
||||
Vector3f euler;
|
||||
Vector3f posNED;
|
||||
Vector3f velNED;
|
||||
Vector3f dAngBias;
|
||||
Vector3f dVelBias;
|
||||
Vector3f gyroBias;
|
||||
ahrs.get_NavEKF2().getEulerAngles(euler);
|
||||
ahrs.get_NavEKF2().getVelNED(velNED);
|
||||
ahrs.get_NavEKF2().getPosNED(posNED);
|
||||
ahrs.get_NavEKF2().getGyroBias(gyroBias);
|
||||
struct log_EKF1 pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_NKF1_MSG),
|
||||
time_us : hal.scheduler->micros64(),
|
||||
roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string)
|
||||
pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string)
|
||||
yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string)
|
||||
velN : (float)(velNED.x), // velocity North (m/s)
|
||||
velE : (float)(velNED.y), // velocity East (m/s)
|
||||
velD : (float)(velNED.z), // velocity Down (m/s)
|
||||
posN : (float)(posNED.x), // metres North
|
||||
posE : (float)(posNED.y), // metres East
|
||||
posD : (float)(posNED.z), // 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
|
||||
};
|
||||
WriteBlock(&pkt, sizeof(pkt));
|
||||
|
||||
// Write second EKF packet
|
||||
float ratio;
|
||||
float az1bias, az2bias;
|
||||
Vector3f wind;
|
||||
Vector3f magNED;
|
||||
Vector3f magXYZ;
|
||||
ahrs.get_NavEKF2().getIMU1Weighting(ratio);
|
||||
ahrs.get_NavEKF2().getAccelZBias(az1bias, az2bias);
|
||||
ahrs.get_NavEKF2().getWind(wind);
|
||||
ahrs.get_NavEKF2().getMagNED(magNED);
|
||||
ahrs.get_NavEKF2().getMagXYZ(magXYZ);
|
||||
struct log_EKF2 pkt2 = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_NKF2_MSG),
|
||||
time_us : hal.scheduler->micros64(),
|
||||
Ratio : (int8_t)(100*ratio),
|
||||
AZ1bias : (int8_t)(100*az1bias),
|
||||
AZ2bias : (int8_t)(100*az2bias),
|
||||
windN : (int16_t)(100*wind.x),
|
||||
windE : (int16_t)(100*wind.y),
|
||||
magN : (int16_t)(magNED.x),
|
||||
magE : (int16_t)(magNED.y),
|
||||
magD : (int16_t)(magNED.z),
|
||||
magX : (int16_t)(magXYZ.x),
|
||||
magY : (int16_t)(magXYZ.y),
|
||||
magZ : (int16_t)(magXYZ.z)
|
||||
};
|
||||
WriteBlock(&pkt2, sizeof(pkt2));
|
||||
|
||||
// Write third EKF packet
|
||||
Vector3f velInnov;
|
||||
Vector3f posInnov;
|
||||
Vector3f magInnov;
|
||||
float tasInnov;
|
||||
float yawInnov;
|
||||
ahrs.get_NavEKF2().getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
|
||||
struct log_EKF3 pkt3 = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_NKF3_MSG),
|
||||
time_us : hal.scheduler->micros64(),
|
||||
innovVN : (int16_t)(100*velInnov.x),
|
||||
innovVE : (int16_t)(100*velInnov.y),
|
||||
innovVD : (int16_t)(100*velInnov.z),
|
||||
innovPN : (int16_t)(100*posInnov.x),
|
||||
innovPE : (int16_t)(100*posInnov.y),
|
||||
innovPD : (int16_t)(100*posInnov.z),
|
||||
innovMX : (int16_t)(magInnov.x),
|
||||
innovMY : (int16_t)(magInnov.y),
|
||||
innovMZ : (int16_t)(magInnov.z),
|
||||
innovVT : (int16_t)(100*tasInnov)
|
||||
};
|
||||
WriteBlock(&pkt3, sizeof(pkt3));
|
||||
|
||||
// Write fourth EKF packet
|
||||
float velVar;
|
||||
float posVar;
|
||||
float hgtVar;
|
||||
Vector3f magVar;
|
||||
float tasVar;
|
||||
Vector2f offset;
|
||||
uint8_t faultStatus, timeoutStatus;
|
||||
nav_filter_status solutionStatus;
|
||||
ahrs.get_NavEKF2().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
|
||||
ahrs.get_NavEKF2().getFilterFaults(faultStatus);
|
||||
ahrs.get_NavEKF2().getFilterTimeouts(timeoutStatus);
|
||||
ahrs.get_NavEKF2().getFilterStatus(solutionStatus);
|
||||
struct log_EKF4 pkt4 = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG),
|
||||
time_us : hal.scheduler->micros64(),
|
||||
sqrtvarV : (int16_t)(100*velVar),
|
||||
sqrtvarP : (int16_t)(100*posVar),
|
||||
sqrtvarH : (int16_t)(100*hgtVar),
|
||||
sqrtvarMX : (int16_t)(100*magVar.x),
|
||||
sqrtvarMY : (int16_t)(100*magVar.y),
|
||||
sqrtvarMZ : (int16_t)(100*magVar.z),
|
||||
sqrtvarVT : (int16_t)(100*tasVar),
|
||||
offsetNorth : (int8_t)(offset.x),
|
||||
offsetEast : (int8_t)(offset.y),
|
||||
faults : (uint8_t)(faultStatus),
|
||||
timeouts : (uint8_t)(timeoutStatus),
|
||||
solution : (uint16_t)(solutionStatus.value)
|
||||
};
|
||||
WriteBlock(&pkt4, sizeof(pkt4));
|
||||
|
||||
|
||||
// Write fifth EKF packet
|
||||
if (optFlowEnabled) {
|
||||
float normInnov; // normalised innovation variance ratio for optical flow observations fused by the main nav filter
|
||||
float gndOffset; // estimated vertical position of the terrain relative to the nav filter zero datum
|
||||
float flowInnovX, flowInnovY; // optical flow LOS rate vector innovations from the main nav filter
|
||||
float auxFlowInnov; // optical flow LOS rate innovation from terrain offset estimator
|
||||
float HAGL; // height above ground level
|
||||
float rngInnov; // range finder innovations
|
||||
float range; // measured range
|
||||
float gndOffsetErr; // filter ground offset state error
|
||||
ahrs.get_NavEKF2().getFlowDebug(normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
|
||||
struct log_EKF5 pkt5 = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG),
|
||||
time_us : hal.scheduler->micros64(),
|
||||
normInnov : (uint8_t)(min(100*normInnov,255)),
|
||||
FIX : (int16_t)(1000*flowInnovX),
|
||||
FIY : (int16_t)(1000*flowInnovY),
|
||||
AFI : (int16_t)(1000*auxFlowInnov),
|
||||
HAGL : (int16_t)(100*HAGL),
|
||||
offset : (int16_t)(100*gndOffset),
|
||||
RI : (int16_t)(100*rngInnov),
|
||||
meaRng : (uint16_t)(100*range),
|
||||
errHAGL : (uint16_t)(100*gndOffsetErr)
|
||||
};
|
||||
WriteBlock(&pkt5, sizeof(pkt5));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user