zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

c415-sdk
Peter Barker 4 years ago
parent
e1a033b296
commit
200b085eea
2 changed files with 105 additions and 69 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 104
      libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
  2. 70
      libraries/AP_NavEKF3/LogStructure.h

104
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
Unescape 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().Write("XKY0", AP::logger().WriteBlock(&xky0, sizeof(xky0));
"TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4",
"s#rrrrrrr-----", const struct log_XKY1 xky1{
"F-000000000000", LOG_PACKET_HEADER_INIT(LOG_XKY1_MSG),
"QBffffffffffff", time_us : time_us,
time_us, core : DAL_CORE(core_index),
DAL_CORE(_core), ivn0 : ivn[0],
yaw_composite, ivn1 : ivn[1],
sqrtf(MAX(yaw_composite_variance, 0.0f)), ivn2 : ivn[2],
yaw[0], ivn3 : ivn[3],
yaw[1], ivn4 : ivn[4],
yaw[2], ive0 : ive[0],
yaw[3], ive1 : ive[1],
yaw[4], ive2 : ive[2],
wgt[0], ive3 : ive[3],
wgt[1], ive4 : ive[4],
wgt[2], };
wgt[3], AP::logger().WriteBlock(&xky1, sizeof(xky1));
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
Unescape 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------"},

Write Preview
Loading…
Cancel
Save