From 65b780fad5a892adfe1e9f6f5d0bad4d11cc5b45 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 18 Feb 2021 09:34:14 +1100 Subject: [PATCH] AP_NavEKF3: move GSF logging from EKF files into GSF files --- libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp | 48 +------------- libraries/AP_NavEKF3/LogStructure.h | 71 --------------------- 2 files changed, 1 insertion(+), 118 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 8e63dff10b..435ea4e414 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -423,51 +423,5 @@ void NavEKF3_core::Log_Write_GSF(uint64_t time_us) 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_XKY0 xky0{ - LOG_PACKET_HEADER_INIT(LOG_XKY0_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(&xky0, sizeof(xky0)); - - const struct log_XKY1 xky1{ - LOG_PACKET_HEADER_INIT(LOG_XKY1_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(&xky1, sizeof(xky1)); + yawEstimator->Log_Write(time_us, LOG_XKY0_MSG, LOG_XKY1_MSG, DAL_CORE(core_index)); } diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h index 60356caebc..ee772ebfd6 100644 --- a/libraries/AP_NavEKF3/LogStructure.h +++ b/libraries/AP_NavEKF3/LogStructure.h @@ -418,73 +418,6 @@ struct PACKED log_XKV { float v11; }; - -// @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 -struct PACKED log_XKY0 { - 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: XKY1 -// @Description: EKF3 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) -struct PACKED log_XKY1 { - 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_NAVEKF3 \ { LOG_XKF0_MSG, sizeof(log_XKF0), \ "XKF0","QBBccCCcccccccc","TimeUS,C,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL,OFN,OFE,OFD", "s#-m---mmmmmmmm", "F--B---BBBBBBBB" }, \ @@ -509,10 +442,6 @@ struct PACKED log_XKY1 { "XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000"}, \ { LOG_XKTV_MSG, sizeof(log_XKTV), \ "XKTV", "QBff", "TimeUS,C,TVS,TVD", "s#rr", "F-00"}, \ - { LOG_XKY0_MSG, sizeof(log_XKY0), \ - "XKY0", "QBffffffffffff", "TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4", "s#rrrrrrr-----", "F-000000000000"}, \ - { LOG_XKY1_MSG, sizeof(log_XKY1), \ - "XKY1", "QBffffffffff", "TimeUS,C,IVN0,IVN1,IVN2,IVN3,IVN4,IVE0,IVE1,IVE2,IVE3,IVE4", "s#nnnnnnnnnn", "F-0000000000"}, \ { LOG_XKV1_MSG, sizeof(log_XKV), \ "XKV1","QBffffffffffff","TimeUS,C,V00,V01,V02,V03,V04,V05,V06,V07,V08,V09,V10,V11", "s#------------", "F-------------" }, \ { LOG_XKV2_MSG, sizeof(log_XKV), \