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

AP_NavEKF2: move GSF logging from EKF files into GSF files

This commit is contained in:
Peter Barker 2021-02-18 09:34:13 +11:00 committed by Andrew Tridgell
parent a17a8faf42
commit d668243d1e
2 changed files with 2 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
View File

@ -326,51 +326,5 @@ void NavEKF2_core::Log_Write_GSF(uint64_t time_us) const
if (yawEstimator == nullptr) {
return;
}
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 (!yawEstimator->getLogData(yaw_composite, yaw_composite_variance, yaw, ivn, ive, wgt)) {
return;
}
const struct log_NKY0 nky0{
LOG_PACKET_HEADER_INIT(LOG_NKY0_MSG),
time_us : time_us,
core : DAL_CORE(core_index),
yaw_composite : yaw_composite,
yaw_composite_variance : sqrtf(MAX(yaw_composite_variance, 0.0f)),
yaw0 : yaw[0],
yaw1 : yaw[1],
yaw2 : yaw[2],
yaw3 : yaw[3],
yaw4 : yaw[4],
wgt0 : wgt[0],
wgt1 : wgt[1],
wgt2 : wgt[2],
wgt3 : wgt[3],
wgt4 : wgt[4],
};
AP::logger().WriteBlock(&nky0, sizeof(nky0));
const struct log_NKY1 nky1{
LOG_PACKET_HEADER_INIT(LOG_NKY1_MSG),
time_us : time_us,
core : DAL_CORE(core_index),
ivn0 : ivn[0],
ivn1 : ivn[1],
ivn2 : ivn[2],
ivn3 : ivn[3],
ivn4 : ivn[4],
ive0 : ive[0],
ive1 : ive[1],
ive2 : ive[2],
ive3 : ive[3],
ive4 : ive[4],
};
AP::logger().WriteBlock(&nky1, sizeof(nky1));
yawEstimator->Log_Write(time_us, LOG_NKY0_MSG, LOG_NKY1_MSG, DAL_CORE(core_index));
}

73
libraries/AP_NavEKF2/LogStructure.h
View File

@ -283,73 +283,6 @@ struct PACKED log_NKT {
float delVelDT_max;
};
// @LoggerMessage: NKY0
// @Description: EKF2 Yaw Estimator States
// @Field: TimeUS: Time since system startup
// @Field: C: EKF2 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
struct PACKED log_NKY0 {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t core;
float yaw_composite;
float yaw_composite_variance;
float yaw0;
float yaw1;
float yaw2;
float yaw3;
float yaw4;
float wgt0;
float wgt1;
float wgt2;
float wgt3;
float wgt4;
};
// @LoggerMessage: NKY1
// @Description: EKF2 Yaw Estimator Innovations
// @Field: TimeUS: Time since system startup
// @Field: C: EKF2 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)
struct PACKED log_NKY1 {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t core;
float ivn0;
float ivn1;
float ivn2;
float ivn3;
float ivn4;
float ive0;
float ive1;
float ive2;
float ive3;
float ive4;
};
#define LOG_STRUCTURE_FROM_NAVEKF2 \
{ LOG_NKF0_MSG, sizeof(log_NKF0), \
"NKF0","QBBccCCcccccccc","TimeUS,C,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL,OFN,OFE,OFD", "s#-m---mmmmmmmm", "F--B---BBBBBBBB" }, \
@ -365,8 +298,4 @@ struct PACKED log_NKY1 {
"NKF5","QBBhhhcccCCfff","TimeUS,C,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,eAng,eVel,ePos", "s#----m???mrnm", "F-----BBBBB000" }, \
{ LOG_NKQ_MSG, sizeof(log_NKQ), "NKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#????", "F-????" }, \
{ LOG_NKT_MSG, sizeof(log_NKT), \
"NKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000"}, \
{ LOG_NKY0_MSG, sizeof(log_NKY0), \
"NKY0", "QBffffffffffff", "TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4", "s#rrrrrrr-----", "F-000000000000"}, \
{ LOG_NKY1_MSG, sizeof(log_NKY1), \
"NKY1", "QBffffffffff", "TimeUS,C,IVN0,IVN1,IVN2,IVN3,IVN4,IVE0,IVE1,IVE2,IVE3,IVE4", "s#nnnnnnnnnn", "F-0000000000"},
"NKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000"},