zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

DataFlash: Only log EKF messages when enabled

mission-4.1.18
Paul Riseborough 9 years ago committed by Andrew Tridgell
parent
624f169b9b
commit
46e4c45537
1 changed files with 142 additions and 139 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 281
      libraries/DataFlash/LogFile.cpp

281
libraries/DataFlash/LogFile.cpp
Unescape View File

@ -1111,148 +1111,151 @@ void DataFlash_Class::Log_Write_POS(AP_AHRS &ahrs) @@ -1111,148 +1111,151 @@ void DataFlash_Class::Log_Write_POS(AP_AHRS &ahrs)
#if AP_AHRS_NAVEKF_AVAILABLE
void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
{
// Write first EKF packet
Vector3f euler;
Vector3f posNED;
Vector3f velNED;
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);
posDownDeriv = ahrs.get_NavEKF().getPosDownDerivative();
struct log_EKF1 pkt = {
LOG_PACKET_HEADER_INIT(LOG_EKF1_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)
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));
// only log EKF if enabled
if (ahrs.get_NavEKF().enabled()) {
// Write first EKF packet
Vector3f euler;
Vector3f posNED;
Vector3f velNED;
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);
posDownDeriv = ahrs.get_NavEKF().getPosDownDerivative();
struct log_EKF1 pkt = {
LOG_PACKET_HEADER_INIT(LOG_EKF1_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)
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 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),
// 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(),
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));
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

Write Preview
Loading…
Cancel
Save