From d668243d1eaefede0b739b41a50fcee3e8a37582 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 18 Feb 2021 09:34:13 +1100 Subject: [PATCH] AP_NavEKF2: move GSF logging from EKF files into GSF files --- libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp | 48 +------------- libraries/AP_NavEKF2/LogStructure.h | 73 +-------------------- 2 files changed, 2 insertions(+), 119 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp index 45eb415d9d..672b580af3 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp @@ -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)); } diff --git a/libraries/AP_NavEKF2/LogStructure.h b/libraries/AP_NavEKF2/LogStructure.h index 248413e630..59d91ab921 100644 --- a/libraries/AP_NavEKF2/LogStructure.h +++ b/libraries/AP_NavEKF2/LogStructure.h @@ -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"},