Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp

360 lines
13 KiB
C++
Raw Blame History

#include "AP_NavEKF3.h"
#include <AP_HAL/HAL.h>
#include <AP_Logger/AP_Logger.h>
void NavEKF3::Log_Write_EKF1(uint8_t _core, LogMessages msg_id, uint64_t time_us) const
{
// Write first EKF packet
Vector3f euler;
Vector2f posNE;
float posD;
Vector3f velNED;
Vector3f gyroBias;
float posDownDeriv;
Location originLLH;
getEulerAngles(_core,euler);
getVelNED(_core,velNED);
getPosNE(_core,posNE);
getPosD(_core,posD);
getGyroBias(_core,gyroBias);
posDownDeriv = getPosDownDerivative(_core);
if (!getOriginLLH(_core,originLLH)) {
originLLH.alt = 0;
}
const struct log_EKF1 pkt{
LOG_PACKET_HEADER_INIT(msg_id),
time_us : time_us,
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)(posNE.x), // metres North
posE : (float)(posNE.y), // metres East
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
originHgt : originLLH.alt // WGS-84 altitude of EKF origin in cm
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
void NavEKF3::Log_Write_NKF2a(uint8_t _core, LogMessages msg_id, uint64_t time_us) const
{
// Write second EKF packet
Vector3f accelBias;
Vector3f wind;
Vector3f magNED;
Vector3f magXYZ;
uint8_t magIndex = getActiveMag(_core);
getAccelBias(_core,accelBias);
getWind(_core,wind);
getMagNED(_core,magNED);
getMagXYZ(_core,magXYZ);
const struct log_NKF2a pkt2{
LOG_PACKET_HEADER_INIT(msg_id),
time_us : time_us,
accBiasX : (int16_t)(100*accelBias.x),
accBiasY : (int16_t)(100*accelBias.y),
accBiasZ : (int16_t)(100*accelBias.z),
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),
index : (uint8_t)(magIndex)
};
AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
}
void NavEKF3::Log_Write_NKF3(uint8_t _core, LogMessages msg_id, uint64_t time_us) const
{
// Write third EKF packet
Vector3f velInnov;
Vector3f posInnov;
Vector3f magInnov;
float tasInnov = 0;
float yawInnov = 0;
getInnovations(_core,velInnov, posInnov, magInnov, tasInnov, yawInnov);
const struct log_NKF3 pkt3{
LOG_PACKET_HEADER_INIT(msg_id),
time_us : time_us,
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),
innovYaw : (int16_t)(100*degrees(yawInnov)),
innovVT : (int16_t)(100*tasInnov)
};
AP::logger().WriteBlock(&pkt3, sizeof(pkt3));
}
void NavEKF3::Log_Write_NKF4(uint8_t _core, LogMessages msg_id, uint64_t time_us) const
{
// Write fourth EKF packet
float velVar = 0;
float posVar = 0;
float hgtVar = 0;
Vector3f magVar;
float tasVar = 0;
Vector2f offset;
uint16_t faultStatus=0;
uint8_t timeoutStatus=0;
nav_filter_status solutionStatus {};
nav_gps_status gpsStatus {};
getVariances(_core,velVar, posVar, hgtVar, magVar, tasVar, offset);
float tempVar = fmaxf(fmaxf(magVar.x,magVar.y),magVar.z);
getFilterFaults(_core,faultStatus);
getFilterTimeouts(_core,timeoutStatus);
getFilterStatus(_core,solutionStatus);
getFilterGpsStatus(_core,gpsStatus);
float tiltError;
getTiltError(_core,tiltError);
uint8_t primaryIndex = getPrimaryCoreIndex();
const struct log_NKF4 pkt4{
LOG_PACKET_HEADER_INIT(msg_id),
time_us : time_us,
sqrtvarV : (int16_t)(100*velVar),
sqrtvarP : (int16_t)(100*posVar),
sqrtvarH : (int16_t)(100*hgtVar),
sqrtvarM : (int16_t)(100*tempVar),
sqrtvarVT : (int16_t)(100*tasVar),
tiltErr : (float)tiltError,
offsetNorth : (int8_t)(offset.x),
offsetEast : (int8_t)(offset.y),
faults : (uint16_t)(faultStatus),
timeouts : (uint8_t)(timeoutStatus),
solution : (uint16_t)(solutionStatus.value),
gps : (uint16_t)(gpsStatus.value),
primary : (int8_t)primaryIndex
};
AP::logger().WriteBlock(&pkt4, sizeof(pkt4));
}
void NavEKF3::Log_Write_NKF5(uint64_t time_us) const
{
// Write fifth EKF packet - take data from the primary instance
float normInnov=0; // normalised innovation variance ratio for optical flow observations fused by the main nav filter
float gndOffset=0; // estimated vertical position of the terrain relative to the nav filter zero datum
float flowInnovX=0, flowInnovY=0; // optical flow LOS rate vector innovations from the main nav filter
float auxFlowInnov=0; // optical flow LOS rate innovation from terrain offset estimator
float HAGL=0; // height above ground level
float rngInnov=0; // range finder innovations
float range=0; // measured range
float gndOffsetErr=0; // filter ground offset state error
Vector3f predictorErrors; // output predictor angle, velocity and position tracking error
getFlowDebug(-1,normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
getOutputTrackingError(-1,predictorErrors);
const struct log_NKF5 pkt5{
LOG_PACKET_HEADER_INIT(LOG_XKF5_MSG),
time_us : time_us,
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),
angErr : (float)predictorErrors.x,
velErr : (float)predictorErrors.y,
posErr : (float)predictorErrors.z
};
AP::logger().WriteBlock(&pkt5, sizeof(pkt5));
}
void NavEKF3::Log_Write_Quaternion(uint8_t _core, LogMessages msg_id, uint64_t time_us) const
{
// log quaternion
Quaternion quat;
getQuaternion(_core, quat);
const struct log_Quaternion pktq1{
LOG_PACKET_HEADER_INIT(msg_id),
time_us : time_us,
q1 : quat.q1,
q2 : quat.q2,
q3 : quat.q3,
q4 : quat.q4
};
AP::logger().WriteBlock(&pktq1, sizeof(pktq1));
}
void NavEKF3::Log_Write_Beacon(uint64_t time_us) const
{
// write range beacon fusion debug packet if the range value is non-zero
uint8_t ID;
float rng;
float innovVar;
float innov;
float testRatio;
Vector3f beaconPosNED;
float bcnPosOffsetHigh;
float bcnPosOffsetLow;
Vector3f posNED;
if (getRangeBeaconDebug(-1, ID, rng, innov, innovVar, testRatio, beaconPosNED, bcnPosOffsetHigh, bcnPosOffsetLow, posNED)) {
if (rng > 0.0f) {
const struct log_RngBcnDebug pkt10{
LOG_PACKET_HEADER_INIT(LOG_XKF10_MSG),
time_us : time_us,
ID : (uint8_t)ID,
rng : (int16_t)(100*rng),
innov : (int16_t)(100*innov),
sqrtInnovVar : (uint16_t)(100*sqrtf(innovVar)),
testRatio : (uint16_t)(100*constrain_float(testRatio,0.0f,650.0f)),
beaconPosN : (int16_t)(100*beaconPosNED.x),
beaconPosE : (int16_t)(100*beaconPosNED.y),
beaconPosD : (int16_t)(100*beaconPosNED.z),
offsetHigh : (int16_t)(100*bcnPosOffsetHigh),
offsetLow : (int16_t)(100*bcnPosOffsetLow),
posN : (int16_t)(100*posNED.x),
posE : (int16_t)(100*posNED.y),
posD : (int16_t)(100*posNED.z)
};
AP::logger().WriteBlock(&pkt10, sizeof(pkt10));
}
}
}
void NavEKF3::Log_Write_BodyOdom(uint64_t time_us) const
{
Vector3f velBodyInnov,velBodyInnovVar;
static uint32_t lastUpdateTime_ms = 0;
uint32_t updateTime_ms = getBodyFrameOdomDebug(-1, velBodyInnov, velBodyInnovVar);
if (updateTime_ms > lastUpdateTime_ms) {
const struct log_ekfBodyOdomDebug pkt11{
LOG_PACKET_HEADER_INIT(LOG_XKFD_MSG),
time_us : time_us,
velInnovX : velBodyInnov.x,
velInnovY : velBodyInnov.y,
velInnovZ : velBodyInnov.z,
velInnovVarX : velBodyInnovVar.x,
velInnovVarY : velBodyInnovVar.y,
velInnovVarZ : velBodyInnovVar.z
};
AP::logger().WriteBlock(&pkt11, sizeof(pkt11));
lastUpdateTime_ms = updateTime_ms;
}
}
void NavEKF3::Log_Write_State_Variances(uint64_t time_us) const
{
static uint32_t lastEkfStateVarLogTime_ms = 0;
if (AP_HAL::millis() - lastEkfStateVarLogTime_ms > 490) {
lastEkfStateVarLogTime_ms = AP_HAL::millis();
float stateVar[24];
getStateVariances(-1, stateVar);
const struct log_ekfStateVar pktv1{
LOG_PACKET_HEADER_INIT(LOG_XKV1_MSG),
time_us : time_us,
v00 : stateVar[0],
v01 : stateVar[1],
v02 : stateVar[2],
v03 : stateVar[3],
v04 : stateVar[4],
v05 : stateVar[5],
v06 : stateVar[6],
v07 : stateVar[7],
v08 : stateVar[8],
v09 : stateVar[9],
v10 : stateVar[10],
v11 : stateVar[11]
};
AP::logger().WriteBlock(&pktv1, sizeof(pktv1));
const struct log_ekfStateVar pktv2{
LOG_PACKET_HEADER_INIT(LOG_XKV2_MSG),
time_us : time_us,
v00 : stateVar[12],
v01 : stateVar[13],
v02 : stateVar[14],
v03 : stateVar[15],
v04 : stateVar[16],
v05 : stateVar[17],
v06 : stateVar[18],
v07 : stateVar[19],
v08 : stateVar[20],
v09 : stateVar[21],
v10 : stateVar[22],
v11 : stateVar[23]
};
AP::logger().WriteBlock(&pktv2, sizeof(pktv2));
}
}
void NavEKF3::Log_Write()
{
// only log if enabled
if (activeCores() <= 0) {
return;
}
uint64_t time_us = AP_HAL::micros64();
Log_Write_EKF1(0, LOG_XKF1_MSG, time_us);
Log_Write_NKF2a(0, LOG_XKF2_MSG, time_us);
Log_Write_NKF3(0, LOG_XKF3_MSG, time_us);
Log_Write_NKF4(0, LOG_XKF4_MSG, time_us);
Log_Write_NKF5(time_us);
Log_Write_Quaternion(0, LOG_XKQ1_MSG, time_us);
// log EKF state info for the second EFK core if enabled
if (activeCores() >= 2) {
Log_Write_EKF1(1, LOG_XKF6_MSG, time_us);
Log_Write_NKF2a(1, LOG_XKF7_MSG, time_us);
Log_Write_NKF3(1, LOG_XKF8_MSG, time_us);
Log_Write_NKF4(1, LOG_XKF9_MSG, time_us);
Log_Write_Quaternion(1, LOG_XKQ2_MSG, time_us);
}
// log EKF state info for the third EFK core if enabled
if (activeCores() >= 3) {
Log_Write_EKF1(2, LOG_XKF11_MSG, time_us);
Log_Write_NKF2a(2, LOG_XKF12_MSG, time_us);
Log_Write_NKF3(2, LOG_XKF13_MSG, time_us);
Log_Write_NKF4(2, LOG_XKF14_MSG, time_us);
Log_Write_Quaternion(2, LOG_XKQ3_MSG, time_us);
}
// write range beacon fusion debug packet if the range value is non-zero
Log_Write_Beacon(time_us);
// write debug data for body frame odometry fusion
Log_Write_BodyOdom(time_us);
// log state variances every 0.49s
Log_Write_State_Variances(time_us);
// log EKF timing statistics every 5s
static uint32_t lastTimingLogTime_ms = 0;
if (AP_HAL::millis() - lastTimingLogTime_ms > 5000) {
lastTimingLogTime_ms = AP_HAL::millis();
struct ekf_timing timing;
for (uint8_t i=0; i<activeCores(); i++) {
getTimingStatistics(i, timing);
if (i == 0) {
Log_EKF_Timing("XKT1", time_us, timing);
} else if (i == 1) {
Log_EKF_Timing("XKT2", time_us, timing);
} else if (i == 2) {
Log_EKF_Timing("XKT3", time_us, timing);
}
}
}
}
Reference in New Issue View Git Blame Copy Permalink