mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF2: use structures for logging GSF data
This makes it look like EKF2 and is marginally more efficient.
This commit is contained in:
Peter Barker
Peter Barker
parent
f94f51d9d3
commit
c5c810b724
|
|
@ -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]);
|
||||
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));
|
||||
|
||||
// @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_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));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 {
|
|||
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"},
|
||||
|
|
|
|||
Loading…
Reference in New Issue