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: Only log EKF messages when enabled

Browse Source
This commit is contained in:
Paul Riseborough 2015-11-06 22:52:52 +11:00 committed by Andrew Tridgell
parent 624f169b9b
commit 46e4c45537
1 changed files with 144 additions and 141 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

285
libraries/DataFlash/LogFile.cpp
View File

@ -1111,148 +1111,151 @@ void DataFlash_Class::Log_Write_POS(AP_AHRS &ahrs)
#if AP_AHRS_NAVEKF_AVAILABLE #if AP_AHRS_NAVEKF_AVAILABLE
void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
{ {
// Write first EKF packet // only log EKF if enabled
Vector3f euler; if (ahrs.get_NavEKF().enabled()) {
Vector3f posNED; // Write first EKF packet
Vector3f velNED; Vector3f euler;
Vector3f dAngBias; Vector3f posNED;
Vector3f dVelBias; Vector3f velNED;
Vector3f gyroBias; Vector3f dAngBias;
float posDownDeriv; Vector3f dVelBias;
ahrs.get_NavEKF().getEulerAngles(euler); Vector3f gyroBias;
ahrs.get_NavEKF().getVelNED(velNED); float posDownDeriv;
ahrs.get_NavEKF().getPosNED(posNED); ahrs.get_NavEKF().getEulerAngles(euler);
ahrs.get_NavEKF().getGyroBias(gyroBias); ahrs.get_NavEKF().getVelNED(velNED);
posDownDeriv = ahrs.get_NavEKF().getPosDownDerivative(); ahrs.get_NavEKF().getPosNED(posNED);
struct log_EKF1 pkt = { ahrs.get_NavEKF().getGyroBias(gyroBias);
LOG_PACKET_HEADER_INIT(LOG_EKF1_MSG), posDownDeriv = ahrs.get_NavEKF().getPosDownDerivative();
time_us : hal.scheduler->micros64(), struct log_EKF1 pkt = {
roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string) LOG_PACKET_HEADER_INIT(LOG_EKF1_MSG),
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)
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
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_NavEKF().getIMU1Weighting(ratio);
ahrs.get_NavEKF().getAccelZBias(az1bias, az2bias);
ahrs.get_NavEKF().getWind(wind);
ahrs.get_NavEKF().getMagNED(magNED);
ahrs.get_NavEKF().getMagXYZ(magXYZ);
struct log_EKF2 pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_EKF2_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;
ahrs.get_NavEKF().getInnovations(velInnov, posInnov, magInnov, tasInnov);
struct log_EKF3 pkt3 = {
LOG_PACKET_HEADER_INIT(LOG_EKF3_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;
nav_gps_status gpsStatus {};
ahrs.get_NavEKF().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
ahrs.get_NavEKF().getFilterFaults(faultStatus);
ahrs.get_NavEKF().getFilterTimeouts(timeoutStatus);
ahrs.get_NavEKF().getFilterStatus(solutionStatus);
ahrs.get_NavEKF().getFilterGpsStatus(gpsStatus);
struct log_EKF4 pkt4 = {
LOG_PACKET_HEADER_INIT(LOG_EKF4_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),
gps : (uint16_t)(gpsStatus.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_NavEKF().getFlowDebug(normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
struct log_EKF5 pkt5 = {
LOG_PACKET_HEADER_INIT(LOG_EKF5_MSG),
time_us : hal.scheduler->micros64(), time_us : hal.scheduler->micros64(),
normInnov : (uint8_t)(min(100*normInnov,255)), roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string)
FIX : (int16_t)(1000*flowInnovX), pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string)
FIY : (int16_t)(1000*flowInnovY), yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string)
AFI : (int16_t)(1000*auxFlowInnov), velN : (float)(velNED.x), // velocity North (m/s)
HAGL : (int16_t)(100*HAGL), velE : (float)(velNED.y), // velocity East (m/s)
offset : (int16_t)(100*gndOffset), velD : (float)(velNED.z), // velocity Down (m/s)
RI : (int16_t)(100*rngInnov), posD_dot : (float)(posDownDeriv), // first derivative of down position
meaRng : (uint16_t)(100*range), posN : (float)(posNED.x), // metres North
errHAGL : (uint16_t)(100*gndOffsetErr) posE : (float)(posNED.y), // metres East
}; posD : (float)(posNED.z), // metres Down
WriteBlock(&pkt5, sizeof(pkt5)); 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_NavEKF().getIMU1Weighting(ratio);
ahrs.get_NavEKF().getAccelZBias(az1bias, az2bias);
ahrs.get_NavEKF().getWind(wind);
ahrs.get_NavEKF().getMagNED(magNED);
ahrs.get_NavEKF().getMagXYZ(magXYZ);
struct log_EKF2 pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_EKF2_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;
ahrs.get_NavEKF().getInnovations(velInnov, posInnov, magInnov, tasInnov);
struct log_EKF3 pkt3 = {
LOG_PACKET_HEADER_INIT(LOG_EKF3_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;
nav_gps_status gpsStatus {};
ahrs.get_NavEKF().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
ahrs.get_NavEKF().getFilterFaults(faultStatus);
ahrs.get_NavEKF().getFilterTimeouts(timeoutStatus);
ahrs.get_NavEKF().getFilterStatus(solutionStatus);
ahrs.get_NavEKF().getFilterGpsStatus(gpsStatus);
struct log_EKF4 pkt4 = {
LOG_PACKET_HEADER_INIT(LOG_EKF4_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),
gps : (uint16_t)(gpsStatus.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_NavEKF().getFlowDebug(normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
struct log_EKF5 pkt5 = {
LOG_PACKET_HEADER_INIT(LOG_EKF5_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));
}
} }
// do EKF2 as well if enabled // do EKF2 as well if enabled