From 200b085eea3acf354dc8266e8640b4b6b6e64678 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 16 Nov 2020 08:58:48 +1100 Subject: [PATCH] AP_NavEKF3: move XKY0 and XKY1 to be normal log structures (from Log_Write) --- libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp | 102 +++++++------------- libraries/AP_NavEKF3/LogStructure.h | 70 ++++++++++++++ 2 files changed, 104 insertions(+), 68 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 82945d2e76..c002eb22ee 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -424,73 +424,39 @@ void NavEKF3_core::Log_Write_GSF(uint64_t time_us) return; } - // @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 + 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)); - 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, - DAL_CORE(_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, - DAL_CORE(_core), - ivn[0], - ivn[1], - ivn[2], - ivn[3], - ivn[4], - ive[0], - ive[1], - ive[2], - ive[3], - ive[4]); + 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)); } diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h index dacbaa7fd7..e72037501e 100644 --- a/libraries/AP_NavEKF3/LogStructure.h +++ b/libraries/AP_NavEKF3/LogStructure.h @@ -5,6 +5,8 @@ #define LOG_IDS_FROM_NAVEKF3 \ LOG_XKT_MSG, \ LOG_XKFM_MSG, \ + LOG_XKY0_MSG, \ + LOG_XKY1_MSG, \ LOG_XKTV_MSG // @LoggerMessage: XKT @@ -70,10 +72,78 @@ struct PACKED log_XKTV { float tvd; }; +// @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_XKT_MSG, sizeof(log_XKT), \ "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_XKFM_MSG, sizeof(log_XKFM), \ "XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s------", "F------"},