Skip to content

Commit 875aa9b

Browse files
peterbarkerpeterbarker
committed
AP_NavEKF3: move XKY0 and XKY1 to be normal log structures (from Log_Write)
1 parent dfe4684 commit 875aa9b

File tree

2 files changed

+105
-69
lines changed

2 files changed

+105
-69
lines changed

‎libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp

Lines changed: 35 additions & 69 deletions
Original file line numberDiff line numberDiff line change
@@ -424,73 +424,39 @@ void NavEKF3_core::Log_Write_GSF(uint64_t time_us)
424424
return;
425425
}
426426

427-
// @LoggerMessage: XKY0
428-
// @Description: EKF3 Yaw Estimator States
429-
// @Field: TimeUS: Time since system startup
430-
// @Field: C: EKF3 core this data is for
431-
// @Field: YC: GSF yaw estimate (rad)
432-
// @Field: YCS: GSF yaw estimate 1-Sigma uncertainty (rad)
433-
// @Field: Y0: Yaw estimate from individual EKF filter 0 (rad)
434-
// @Field: Y1: Yaw estimate from individual EKF filter 1 (rad)
435-
// @Field: Y2: Yaw estimate from individual EKF filter 2 (rad)
436-
// @Field: Y3: Yaw estimate from individual EKF filter 3 (rad)
437-
// @Field: Y4: Yaw estimate from individual EKF filter 4 (rad)
438-
// @Field: W0: Weighting applied to yaw estimate from individual EKF filter 0
439-
// @Field: W1: Weighting applied to yaw estimate from individual EKF filter 1
440-
// @Field: W2: Weighting applied to yaw estimate from individual EKF filter 2
441-
// @Field: W3: Weighting applied to yaw estimate from individual EKF filter 3
442-
// @Field: W4: Weighting applied to yaw estimate from individual EKF filter 4
443-
444-
AP::logger().Write("XKY0",
445-
"TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4",
446-
"s#rrrrrrr-----",
447-
"F-000000000000",
448-
"QBffffffffffff",
449-
time_us,
450-
DAL_CORE(_core),
451-
yaw_composite,
452-
sqrtf(MAX(yaw_composite_variance, 0.0f)),
453-
yaw[0],
454-
yaw[1],
455-
yaw[2],
456-
yaw[3],
457-
yaw[4],
458-
wgt[0],
459-
wgt[1],
460-
wgt[2],
461-
wgt[3],
462-
wgt[4]);
463-
464-
// @LoggerMessage: XKY1
465-
// @Description: EKF2 Yaw Estimator Innovations
466-
// @Field: TimeUS: Time since system startup
467-
// @Field: C: EKF3 core this data is for
468-
// @Field: IVN0: North velocity innovation from individual EKF filter 0 (m/s)
469-
// @Field: IVN1: North velocity innovation from individual EKF filter 1 (m/s)
470-
// @Field: IVN2: North velocity innovation from individual EKF filter 2 (m/s)
471-
// @Field: IVN3: North velocity innovation from individual EKF filter 3 (m/s)
472-
// @Field: IVN4: North velocity innovation from individual EKF filter 4 (m/s)
473-
// @Field: IVE0: East velocity innovation from individual EKF filter 0 (m/s)
474-
// @Field: IVE1: East velocity innovation from individual EKF filter 1 (m/s)
475-
// @Field: IVE2: East velocity innovation from individual EKF filter 2 (m/s)
476-
// @Field: IVE3: East velocity innovation from individual EKF filter 3 (m/s)
477-
// @Field: IVE4: East velocity innovation from individual EKF filter 4 (m/s)
478-
479-
AP::logger().Write("XKY1",
480-
"TimeUS,C,IVN0,IVN1,IVN2,IVN3,IVN4,IVE0,IVE1,IVE2,IVE3,IVE4",
481-
"s#nnnnnnnnnn",
482-
"F-0000000000",
483-
"QBffffffffff",
484-
time_us,
485-
DAL_CORE(_core),
486-
ivn[0],
487-
ivn[1],
488-
ivn[2],
489-
ivn[3],
490-
ivn[4],
491-
ive[0],
492-
ive[1],
493-
ive[2],
494-
ive[3],
495-
ive[4]);
427+
const struct log_XKY0 xky0{
428+
LOG_PACKET_HEADER_INIT(LOG_XKY0_MSG),
429+
time_us : time_us,
430+
core : DAL_CORE(core_index),
431+
yaw_composite : yaw_composite,
432+
yaw_composite_variance : sqrtf(MAX(yaw_composite_variance, 0.0f)),
433+
yaw0 : yaw[0],
434+
yaw1 : yaw[1],
435+
yaw2 : yaw[2],
436+
yaw3 : yaw[3],
437+
yaw4 : yaw[4],
438+
wgt0 : wgt[0],
439+
wgt1 : wgt[1],
440+
wgt2 : wgt[2],
441+
wgt3 : wgt[3],
442+
wgt4 : wgt[4],
443+
};
444+
AP::logger().WriteBlock(&xky0, sizeof(xky0));
445+
446+
const struct log_XKY1 xky1{
447+
LOG_PACKET_HEADER_INIT(LOG_XKY1_MSG),
448+
time_us : time_us,
449+
core : DAL_CORE(core_index),
450+
ivn0 : ivn[0],
451+
ivn1 : ivn[1],
452+
ivn2 : ivn[2],
453+
ivn3 : ivn[3],
454+
ivn4 : ivn[4],
455+
ive0 : ive[0],
456+
ive1 : ive[1],
457+
ive2 : ive[2],
458+
ive3 : ive[3],
459+
ive4 : ive[4],
460+
};
461+
AP::logger().WriteBlock(&xky1, sizeof(xky1));
496462
}

‎libraries/AP_NavEKF3/LogStructure.h

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,8 @@
55
#define LOG_IDS_FROM_NAVEKF3 \
66
LOG_XKT_MSG, \
77
LOG_XKFM_MSG, \
8+
LOG_XKY0_MSG, \
9+
LOG_XKY1_MSG, \
810
LOG_XKTV_MSG
911

1012
// @LoggerMessage: XKT
@@ -70,10 +72,78 @@ struct PACKED log_XKTV {
7072
float tvd;
7173
};
7274

75+
// @LoggerMessage: XKY0
76+
// @Description: EKF3 Yaw Estimator States
77+
// @Field: TimeUS: Time since system startup
78+
// @Field: C: EKF3 core this data is for
79+
// @Field: YC: GSF yaw estimate (rad)
80+
// @Field: YCS: GSF yaw estimate 1-Sigma uncertainty (rad)
81+
// @Field: Y0: Yaw estimate from individual EKF filter 0 (rad)
82+
// @Field: Y1: Yaw estimate from individual EKF filter 1 (rad)
83+
// @Field: Y2: Yaw estimate from individual EKF filter 2 (rad)
84+
// @Field: Y3: Yaw estimate from individual EKF filter 3 (rad)
85+
// @Field: Y4: Yaw estimate from individual EKF filter 4 (rad)
86+
// @Field: W0: Weighting applied to yaw estimate from individual EKF filter 0
87+
// @Field: W1: Weighting applied to yaw estimate from individual EKF filter 1
88+
// @Field: W2: Weighting applied to yaw estimate from individual EKF filter 2
89+
// @Field: W3: Weighting applied to yaw estimate from individual EKF filter 3
90+
// @Field: W4: Weighting applied to yaw estimate from individual EKF filter 4
91+
struct PACKED log_XKY0 {
92+
LOG_PACKET_HEADER;
93+
uint64_t time_us;
94+
uint8_t core;
95+
float yaw_composite;
96+
float yaw_composite_variance;
97+
float yaw0;
98+
float yaw1;
99+
float yaw2;
100+
float yaw3;
101+
float yaw4;
102+
float wgt0;
103+
float wgt1;
104+
float wgt2;
105+
float wgt3;
106+
float wgt4;
107+
};
108+
109+
// @LoggerMessage: XKY1
110+
// @Description: EKF3 Yaw Estimator Innovations
111+
// @Field: TimeUS: Time since system startup
112+
// @Field: C: EKF3 core this data is for
113+
// @Field: IVN0: North velocity innovation from individual EKF filter 0 (m/s)
114+
// @Field: IVN1: North velocity innovation from individual EKF filter 1 (m/s)
115+
// @Field: IVN2: North velocity innovation from individual EKF filter 2 (m/s)
116+
// @Field: IVN3: North velocity innovation from individual EKF filter 3 (m/s)
117+
// @Field: IVN4: North velocity innovation from individual EKF filter 4 (m/s)
118+
// @Field: IVE0: East velocity innovation from individual EKF filter 0 (m/s)
119+
// @Field: IVE1: East velocity innovation from individual EKF filter 1 (m/s)
120+
// @Field: IVE2: East velocity innovation from individual EKF filter 2 (m/s)
121+
// @Field: IVE3: East velocity innovation from individual EKF filter 3 (m/s)
122+
// @Field: IVE4: East velocity innovation from individual EKF filter 4 (m/s)
123+
struct PACKED log_XKY1 {
124+
LOG_PACKET_HEADER;
125+
uint64_t time_us;
126+
uint8_t core;
127+
float ivn0;
128+
float ivn1;
129+
float ivn2;
130+
float ivn3;
131+
float ivn4;
132+
float ive0;
133+
float ive1;
134+
float ive2;
135+
float ive3;
136+
float ive4;
137+
};
138+
73139
#define LOG_STRUCTURE_FROM_NAVEKF3 \
74140
{ LOG_XKT_MSG, sizeof(log_XKT), \
75141
"XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000"}, \
76142
{ LOG_XKTV_MSG, sizeof(log_XKTV), \
77143
"XKTV", "QBff", "TimeUS,C,TVS,TVD", "s#rr", "F-00"}, \
144+
{ LOG_XKY0_MSG, sizeof(log_XKY0), \
145+
"XKY0", "QBffffffffffff", "TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4", "s#rrrrrrr-----", "F-000000000000"}, \
146+
{ LOG_XKY1_MSG, sizeof(log_XKY1), \
147+
"XKY1", "QBffffffffff", "TimeUS,C,IVN0,IVN1,IVN2,IVN3,IVN4,IVE0,IVE1,IVE2,IVE3,IVE4", "s#nnnnnnnnnn", "F-0000000000"}, \
78148
{ LOG_XKFM_MSG, sizeof(log_XKFM), \
79149
"XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s------", "F------"},

0 commit comments

Comments
 (0)