mirror of https://github.com/ArduPilot/ardupilot
446 lines
16 KiB
C++
446 lines
16 KiB
C++
#include "AP_NavEKF3.h"
|
|
|
|
#include <AP_HAL/HAL.h>
|
|
#include <AP_Logger/AP_Logger.h>
|
|
|
|
void NavEKF3::Log_Write_XKF1(uint8_t _core, 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(LOG_XKF1_MSG),
|
|
time_us : time_us,
|
|
core : _core,
|
|
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_XKF2(uint8_t _core, uint64_t time_us) const
|
|
{
|
|
// Write second EKF packet
|
|
Vector3f accelBias;
|
|
Vector3f wind;
|
|
Vector3f magNED;
|
|
Vector3f magXYZ;
|
|
getAccelBias(_core,accelBias);
|
|
getWind(_core,wind);
|
|
getMagNED(_core,magNED);
|
|
getMagXYZ(_core,magXYZ);
|
|
const struct log_XKF2 pkt2{
|
|
LOG_PACKET_HEADER_INIT(LOG_XKF2_MSG),
|
|
time_us : time_us,
|
|
core : _core,
|
|
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)
|
|
};
|
|
AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
|
|
}
|
|
|
|
void NavEKF3::Log_Write_XKFS(uint8_t _core, uint64_t time_us) const
|
|
{
|
|
// Write sensor selection EKF packet
|
|
uint8_t magIndex = getActiveMag(_core);
|
|
uint8_t baroIndex = getActiveBaro(_core);
|
|
uint8_t GPSIndex = getActiveGPS(_core);
|
|
uint8_t airspeedIndex = getActiveAirspeed(_core);
|
|
const struct log_EKFS pkt {
|
|
LOG_PACKET_HEADER_INIT(LOG_XKFS_MSG),
|
|
time_us : time_us,
|
|
core : _core,
|
|
mag_index : (uint8_t)(magIndex),
|
|
baro_index : (uint8_t)(baroIndex),
|
|
gps_index : (uint8_t)(GPSIndex),
|
|
airspeed_index : (uint8_t)(airspeedIndex),
|
|
};
|
|
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
|
}
|
|
|
|
void NavEKF3::Log_Write_XKF3(uint8_t _core, 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(LOG_XKF3_MSG),
|
|
time_us : time_us,
|
|
core : _core,
|
|
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),
|
|
rerr : coreRelativeErrors[_core],
|
|
errorScore : coreErrorScores[_core]
|
|
};
|
|
AP::logger().WriteBlock(&pkt3, sizeof(pkt3));
|
|
}
|
|
|
|
void NavEKF3::Log_Write_XKF4(uint8_t _core, 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(LOG_XKF4_MSG),
|
|
time_us : time_us,
|
|
core : _core,
|
|
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 : (uint32_t)(solutionStatus.value),
|
|
gps : (uint16_t)(gpsStatus.value),
|
|
primary : (int8_t)primaryIndex
|
|
};
|
|
AP::logger().WriteBlock(&pkt4, sizeof(pkt4));
|
|
}
|
|
|
|
|
|
void NavEKF3::Log_Write_XKF5(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, uint64_t time_us) const
|
|
{
|
|
// log quaternion
|
|
Quaternion quat;
|
|
getQuaternion(_core, quat);
|
|
const struct log_Quaternion pktq1{
|
|
LOG_PACKET_HEADER_INIT(LOG_XKQ_MSG),
|
|
time_us : time_us,
|
|
core : _core,
|
|
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_XKF5(time_us);
|
|
|
|
for (uint8_t i=0; i<activeCores(); i++) {
|
|
Log_Write_XKF1(i, time_us);
|
|
Log_Write_XKF2(i, time_us);
|
|
Log_Write_XKF3(i, time_us);
|
|
Log_Write_XKF4(i, time_us);
|
|
Log_Write_XKFS(i, time_us);
|
|
Log_Write_Quaternion(i, time_us);
|
|
Log_Write_GSF(i, 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);
|
|
Log_EKF_Timing("XKT", i, time_us, timing);
|
|
}
|
|
}
|
|
}
|
|
|
|
void NavEKF3::Log_Write_GSF(uint8_t _core, uint64_t time_us) const
|
|
{
|
|
float yaw_composite;
|
|
float yaw_composite_variance;
|
|
float yaw[N_MODELS_EKFGSF];
|
|
float ivn[N_MODELS_EKFGSF];
|
|
float ive[N_MODELS_EKFGSF];
|
|
float wgt[N_MODELS_EKFGSF];
|
|
|
|
if (getDataEKFGSF(_core, yaw_composite, yaw_composite_variance, yaw, ivn, ive, wgt)) {
|
|
|
|
// @LoggerMessage: XKY0
|
|
// @Description: EKF3 Yaw Estimator States
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: C: EKF3 core this data is for
|
|
// @Field: YC: GSF yaw estimate (rad)
|
|
// @Field: YCS: GSF yaw estimate 1-Sigma uncertainty (rad)
|
|
// @Field: Y0: Yaw estimate from individual EKF filter 0 (rad)
|
|
// @Field: Y1: Yaw estimate from individual EKF filter 1 (rad)
|
|
// @Field: Y2: Yaw estimate from individual EKF filter 2 (rad)
|
|
// @Field: Y3: Yaw estimate from individual EKF filter 3 (rad)
|
|
// @Field: Y4: Yaw estimate from individual EKF filter 4 (rad)
|
|
// @Field: W0: Weighting applied to yaw estimate from individual EKF filter 0
|
|
// @Field: W1: Weighting applied to yaw estimate from individual EKF filter 1
|
|
// @Field: W2: Weighting applied to yaw estimate from individual EKF filter 2
|
|
// @Field: W3: Weighting applied to yaw estimate from individual EKF filter 3
|
|
// @Field: W4: Weighting applied to yaw estimate from individual EKF filter 4
|
|
|
|
AP::logger().Write("XKY0",
|
|
"TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4",
|
|
"s#rrrrrrr-----",
|
|
"F-000000000000",
|
|
"QBffffffffffff",
|
|
time_us,
|
|
_core,
|
|
yaw_composite,
|
|
sqrtf(MAX(yaw_composite_variance, 0.0f)),
|
|
yaw[0],
|
|
yaw[1],
|
|
yaw[2],
|
|
yaw[3],
|
|
yaw[4],
|
|
wgt[0],
|
|
wgt[1],
|
|
wgt[2],
|
|
wgt[3],
|
|
wgt[4]);
|
|
|
|
// @LoggerMessage: XKY1
|
|
// @Description: EKF2 Yaw Estimator Innovations
|
|
// @Field: TimeUS: Time since system startup
|
|
// @Field: C: EKF3 core this data is for
|
|
// @Field: IVN0: North velocity innovation from individual EKF filter 0 (m/s)
|
|
// @Field: IVN1: North velocity innovation from individual EKF filter 1 (m/s)
|
|
// @Field: IVN2: North velocity innovation from individual EKF filter 2 (m/s)
|
|
// @Field: IVN3: North velocity innovation from individual EKF filter 3 (m/s)
|
|
// @Field: IVN4: North velocity innovation from individual EKF filter 4 (m/s)
|
|
// @Field: IVE0: East velocity innovation from individual EKF filter 0 (m/s)
|
|
// @Field: IVE1: East velocity innovation from individual EKF filter 1 (m/s)
|
|
// @Field: IVE2: East velocity innovation from individual EKF filter 2 (m/s)
|
|
// @Field: IVE3: East velocity innovation from individual EKF filter 3 (m/s)
|
|
// @Field: IVE4: East velocity innovation from individual EKF filter 4 (m/s)
|
|
|
|
AP::logger().Write("XKY1",
|
|
"TimeUS,C,IVN0,IVN1,IVN2,IVN3,IVN4,IVE0,IVE1,IVE2,IVE3,IVE4",
|
|
"s#nnnnnnnnnn",
|
|
"F-0000000000",
|
|
"QBffffffffff",
|
|
time_us,
|
|
_core,
|
|
ivn[0],
|
|
ivn[1],
|
|
ivn[2],
|
|
ivn[3],
|
|
ivn[4],
|
|
ive[0],
|
|
ive[1],
|
|
ive[2],
|
|
ive[3],
|
|
ive[4]);
|
|
}
|
|
}
|