zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF2: use structures for logging GSF data 

This makes it look like EKF2 and is marginally more efficient.
zr-v5.1
Peter Barker 4 years ago committed by Peter Barker
parent
f94f51d9d3
commit
c5c810b724
2 changed files with 107 additions and 72 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 104
      libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
  2. 75
      libraries/AP_NavEKF2/LogStructure.h

104
libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp
Unescape View File

@ -331,77 +331,43 @@ void NavEKF2_core::Log_Write_GSF(uint64_t time_us) const @@ -331,77 +331,43 @@ void NavEKF2_core::Log_Write_GSF(uint64_t time_us) const
float ive[N_MODELS_EKFGSF];
float wgt[N_MODELS_EKFGSF];
// @LoggerMessage: NKY0
// @Description: EKF2 Yaw Estimator States
// @Field: TimeUS: Time since system startup
// @Field: C: EKF2 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
if (!yawEstimator->getLogData(yaw_composite, yaw_composite_variance, yaw, ivn, ive, wgt)) {
return;
}
AP::logger().Write("NKY0",
"TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4",
"s#rrrrrrr-----",
"F-000000000000",
"QBffffffffffff",
time_us,
DAL_CORE(core_index),
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: NKY1
// @Description: EKF2 Yaw Estimator Innovations
// @Field: TimeUS: Time since system startup
// @Field: C: EKF2 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("NKY1",
"TimeUS,C,IVN0,IVN1,IVN2,IVN3,IVN4,IVE0,IVE1,IVE2,IVE3,IVE4",
"s#nnnnnnnnnn",
"F-0000000000",
"QBffffffffff",
time_us,
DAL_CORE(core_index),
ivn[0],
ivn[1],
ivn[2],
ivn[3],
ivn[4],
ive[0],
ive[1],
ive[2],
ive[3],
ive[4]);
const struct log_NKY0 nky0{
LOG_PACKET_HEADER_INIT(LOG_NKY0_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(&nky0, sizeof(nky0));
const struct log_NKY1 nky1{
LOG_PACKET_HEADER_INIT(LOG_NKY1_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(&nky1, sizeof(nky1));
}

75
libraries/AP_NavEKF2/LogStructure.h
Unescape View File

@ -3,7 +3,9 @@ @@ -3,7 +3,9 @@
#include <AP_Logger/LogStructure.h>
#define LOG_IDS_FROM_NAVEKF2 \
LOG_NKT_MSG
LOG_NKT_MSG, \
LOG_NKY0_MSG, \
LOG_NKY1_MSG
// @LoggerMessage: NKT
@ -34,7 +36,74 @@ struct PACKED log_NKT { @@ -34,7 +36,74 @@ struct PACKED log_NKT {
float delVelDT_max;
};
// @LoggerMessage: NKY0
// @Description: EKF2 Yaw Estimator States
// @Field: TimeUS: Time since system startup
// @Field: C: EKF2 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_NKY0 {
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: NKY1
// @Description: EKF2 Yaw Estimator Innovations
// @Field: TimeUS: Time since system startup
// @Field: C: EKF2 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_NKY1 {
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_NAVEKF2 \
{ LOG_NKT_MSG, sizeof(log_NKT), \
"NKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000"},
"NKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000"}, \
{ LOG_NKY0_MSG, sizeof(log_NKY0), \
"NKY0", "QBffffffffffff", "TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4", "s#rrrrrrr-----", "F-000000000000"}, \
{ LOG_NKY1_MSG, sizeof(log_NKY1), \
"NKY1", "QBffffffffff", "TimeUS,C,IVN0,IVN1,IVN2,IVN3,IVN4,IVE0,IVE1,IVE2,IVE3,IVE4", "s#nnnnnnnnnn", "F-0000000000"},

Write Preview
Loading…
Cancel
Save