mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-22 08:38:36 -04:00
DataFlash: Only log EKF messages when enabled
This commit is contained in:
Paul Riseborough
Andrew Tridgell
parent
624f169b9b
commit
46e4c45537
@ -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));
|
||||
|
||||
|
||||
// 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),
|
||||
// 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(),
|
||||
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));
|
||||
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));
|
||||
|
||||
|
||||
// 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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user