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

AP_NavEKF3: move XKY0 and XKY1 to be normal log structures (from Log_Write)

This commit is contained in:
Peter Barker 2020-11-16 08:58:48 +11:00
parent e1a033b296
commit 200b085eea
2 changed files with 104 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -424,73 +424,39 @@ void NavEKF3_core::Log_Write_GSF(uint64_t time_us)
return; return;
} }
// @LoggerMessage: XKY0 const struct log_XKY0 xky0{
// @Description: EKF3 Yaw Estimator States LOG_PACKET_HEADER_INIT(LOG_XKY0_MSG),
// @Field: TimeUS: Time since system startup time_us : time_us,
// @Field: C: EKF3 core this data is for core : DAL_CORE(core_index),
// @Field: YC: GSF yaw estimate (rad) yaw_composite : yaw_composite,
// @Field: YCS: GSF yaw estimate 1-Sigma uncertainty (rad) yaw_composite_variance : sqrtf(MAX(yaw_composite_variance, 0.0f)),
// @Field: Y0: Yaw estimate from individual EKF filter 0 (rad) yaw0 : yaw[0],
// @Field: Y1: Yaw estimate from individual EKF filter 1 (rad) yaw1 : yaw[1],
// @Field: Y2: Yaw estimate from individual EKF filter 2 (rad) yaw2 : yaw[2],
// @Field: Y3: Yaw estimate from individual EKF filter 3 (rad) yaw3 : yaw[3],
// @Field: Y4: Yaw estimate from individual EKF filter 4 (rad) yaw4 : yaw[4],
// @Field: W0: Weighting applied to yaw estimate from individual EKF filter 0 wgt0 : wgt[0],
// @Field: W1: Weighting applied to yaw estimate from individual EKF filter 1 wgt1 : wgt[1],
// @Field: W2: Weighting applied to yaw estimate from individual EKF filter 2 wgt2 : wgt[2],
// @Field: W3: Weighting applied to yaw estimate from individual EKF filter 3 wgt3 : wgt[3],
// @Field: W4: Weighting applied to yaw estimate from individual EKF filter 4 wgt4 : wgt[4],
};
AP::logger().WriteBlock(&xky0, sizeof(xky0));
AP::logger().Write("XKY0", const struct log_XKY1 xky1{
"TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4", LOG_PACKET_HEADER_INIT(LOG_XKY1_MSG),
"s#rrrrrrr-----", time_us : time_us,
"F-000000000000", core : DAL_CORE(core_index),
"QBffffffffffff", ivn0 : ivn[0],
time_us, ivn1 : ivn[1],
DAL_CORE(_core), ivn2 : ivn[2],
yaw_composite, ivn3 : ivn[3],
sqrtf(MAX(yaw_composite_variance, 0.0f)), ivn4 : ivn[4],
yaw[0], ive0 : ive[0],
yaw[1], ive1 : ive[1],
yaw[2], ive2 : ive[2],
yaw[3], ive3 : ive[3],
yaw[4], ive4 : ive[4],
wgt[0], };
wgt[1], AP::logger().WriteBlock(&xky1, sizeof(xky1));
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]);
} }

70
libraries/AP_NavEKF3/LogStructure.h
View File

@ -5,6 +5,8 @@
#define LOG_IDS_FROM_NAVEKF3 \ #define LOG_IDS_FROM_NAVEKF3 \
LOG_XKT_MSG, \ LOG_XKT_MSG, \
LOG_XKFM_MSG, \ LOG_XKFM_MSG, \
LOG_XKY0_MSG, \
LOG_XKY1_MSG, \
LOG_XKTV_MSG LOG_XKTV_MSG
// @LoggerMessage: XKT // @LoggerMessage: XKT
@ -70,10 +72,78 @@ struct PACKED log_XKTV {
float tvd; 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 \ #define LOG_STRUCTURE_FROM_NAVEKF3 \
{ LOG_XKT_MSG, sizeof(log_XKT), \ { LOG_XKT_MSG, sizeof(log_XKT), \
"XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000"}, \ "XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000"}, \
{ LOG_XKTV_MSG, sizeof(log_XKTV), \ { LOG_XKTV_MSG, sizeof(log_XKTV), \
"XKTV", "QBff", "TimeUS,C,TVS,TVD", "s#rr", "F-00"}, \ "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), \ { LOG_XKFM_MSG, sizeof(log_XKFM), \
"XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s------", "F------"}, "XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s------", "F------"},