From 79109458c23585c4a74727a80eef3aabce158d79 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 2 Dec 2020 11:49:19 +1100 Subject: [PATCH] AP_NavEKF3: move EKF3 logging structures into EKF3 LogStructure.h --- libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp | 14 +- libraries/AP_NavEKF3/LogStructure.h | 357 +++++++++++++++++++- 2 files changed, 362 insertions(+), 9 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 877e1d654c..51480a2d5c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -80,7 +80,7 @@ void NavEKF3_core::Log_Write_XKF2(uint64_t time_us) const void NavEKF3_core::Log_Write_XKFS(uint64_t time_us) const { // Write sensor selection EKF packet - const struct log_EKFS pkt { + const struct log_XKFS pkt { LOG_PACKET_HEADER_INIT(LOG_XKFS_MSG), time_us : time_us, core : DAL_CORE(core_index), @@ -101,7 +101,7 @@ void NavEKF3_core::Log_Write_XKF3(uint64_t time_us) const float tasInnov = 0; float yawInnov = 0; getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov); - const struct log_NKF3 pkt3{ + const struct log_XKF3 pkt3{ LOG_PACKET_HEADER_INIT(LOG_XKF3_MSG), time_us : time_us, core : DAL_CORE(core_index), @@ -197,7 +197,7 @@ void NavEKF3_core::Log_Write_Quaternion(uint64_t time_us) const // log quaternion Quaternion quat; getQuaternion( quat); - const struct log_Quaternion pktq1{ + const struct log_XKQ pktq1{ LOG_PACKET_HEADER_INIT(LOG_XKQ_MSG), time_us : time_us, core : DAL_CORE(core_index), @@ -234,7 +234,7 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us) return; } - const struct log_RngBcnDebug pkt10{ + const struct log_XKF0 pkt10{ LOG_PACKET_HEADER_INIT(LOG_XKF10_MSG), time_us : time_us, core : DAL_CORE(core_index), @@ -267,7 +267,7 @@ void NavEKF3_core::Log_Write_BodyOdom(uint64_t time_us) static uint32_t lastUpdateTime_ms = 0; uint32_t updateTime_ms = getBodyFrameOdomDebug( velBodyInnov, velBodyInnovVar); if (updateTime_ms > lastUpdateTime_ms) { - const struct log_ekfBodyOdomDebug pkt11{ + const struct log_XKFD pkt11{ LOG_PACKET_HEADER_INIT(LOG_XKFD_MSG), time_us : time_us, core : DAL_CORE(core_index), @@ -293,7 +293,7 @@ void NavEKF3_core::Log_Write_State_Variances(uint64_t time_us) const static uint32_t lastEkfStateVarLogTime_ms = 0; if (AP::dal().millis() - lastEkfStateVarLogTime_ms > 490) { lastEkfStateVarLogTime_ms = AP::dal().millis(); - const struct log_ekfStateVar pktv1{ + const struct log_XKV pktv1{ LOG_PACKET_HEADER_INIT(LOG_XKV1_MSG), time_us : time_us, core : DAL_CORE(core_index), @@ -311,7 +311,7 @@ void NavEKF3_core::Log_Write_State_Variances(uint64_t time_us) const v11 : P[11][11] }; AP::logger().WriteBlock(&pktv1, sizeof(pktv1)); - const struct log_ekfStateVar pktv2{ + const struct log_XKV pktv2{ LOG_PACKET_HEADER_INIT(LOG_XKV2_MSG), time_us : time_us, core : DAL_CORE(core_index), diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h index e72037501e..d745176aed 100644 --- a/libraries/AP_NavEKF3/LogStructure.h +++ b/libraries/AP_NavEKF3/LogStructure.h @@ -9,6 +9,246 @@ LOG_XKY1_MSG, \ LOG_XKTV_MSG +// @LoggerMessage: XKF0 +// @Description: EKF3 beacon sensor diagnostics +// @Field: TimeUS: Time since system startup +// @Field: C: EKF3 core this data is for +// @Field: ID: Beacon sensor ID +// @Field: rng: Beacon range +// @Field: innov: Beacon range innovation +// @Field: SIV: sqrt of beacon range innovation variance +// @Field: TR: Beacon range innovation consistency test ratio +// @Field: BPN: Beacon north position +// @Field: BPE: Beacon east position +// @Field: BPD: Beacon down position +// @Field: OFH: High estimate of vertical position offset of beacons rel to EKF origin +// @Field: OFL: Low estimate of vertical position offset of beacons rel to EKF origin +// @Field: OFN: North position of receiver rel to EKF origin +// @Field: OFE: East position of receiver rel to EKF origin +// @Field: OFD: Down position of receiver rel to EKF origin +struct PACKED log_XKF0 { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t core; + uint8_t ID; // beacon identifier + int16_t rng; // beacon range (cm) + int16_t innov; // beacon range innovation (cm) + uint16_t sqrtInnovVar; // sqrt of beacon range innovation variance (cm) + uint16_t testRatio; // beacon range innovation consistency test ratio *100 + int16_t beaconPosN; // beacon north position (cm) + int16_t beaconPosE; // beacon east position (cm) + int16_t beaconPosD; // beacon down position (cm) + int16_t offsetHigh; // high estimate of vertical position offset of beacons rel to EKF origin (cm) + int16_t offsetLow; // low estimate of vertical position offset of beacons rel to EKF origin (cm) + int16_t posN; // North position of receiver rel to EKF origin (cm) + int16_t posE; // East position of receiver rel to EKF origin (cm) + int16_t posD; // Down position of receiver rel to EKF origin (cm) +}; + + +// @LoggerMessage: XKF1 +// @Description: EKF3 estimator outputs +// @Field: TimeUS: Time since system startup +// @Field: C: EKF3 core this data is for +// @Field: Roll: Estimated roll +// @Field: Pitch: Estimated pitch +// @Field: Yaw: Estimated yaw +// @Field: VN: Estimated velocity (North component) +// @Field: VE: Estimated velocity (East component) +// @Field: VD: Estimated velocity (Down component) +// @Field: dPD: Filtered derivative of vertical position (down) +// @Field: PN: Estimated distance from origin (North component) +// @Field: PE: Estimated distance from origin (East component) +// @Field: PD: Estimated distance from origin (Down component) +// @Field: GX: Estimated gyro bias, X axis +// @Field: GY: Estimated gyro bias, Y axis +// @Field: GZ: Estimated gyro bias, Z axis +// @Field: OH: Height of origin above WGS-84 +struct PACKED log_XKF1 { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t core; + int16_t roll; + int16_t pitch; + uint16_t yaw; + float velN; + float velE; + float velD; + float posD_dot; + float posN; + float posE; + float posD; + int16_t gyrX; + int16_t gyrY; + int16_t gyrZ; + int32_t originHgt; +}; + + +// @LoggerMessage: XKF2 +// @Description: EKF3 estimator secondary outputs +// @Field: TimeUS: Time since system startup +// @Field: C: EKF3 core this data is for +// @Field: AX: Estimated accelerometer X bias +// @Field: AY: Estimated accelerometer Y bias +// @Field: AZ: Estimated accelerometer Z bias +// @Field: VWN: Estimated wind velocity (North component) +// @Field: VWE: Estimated wind velocity (East component) +// @Field: MN: Magnetic field strength (North component) +// @Field: ME: Magnetic field strength (East component) +// @Field: MD: Magnetic field strength (Down component) +// @Field: MX: Magnetic field strength (body X-axis) +// @Field: MY: Magnetic field strength (body Y-axis) +// @Field: MZ: Magnetic field strength (body Z-axis) +struct PACKED log_XKF2 { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t core; + int16_t accBiasX; + int16_t accBiasY; + int16_t accBiasZ; + int16_t windN; + int16_t windE; + int16_t magN; + int16_t magE; + int16_t magD; + int16_t magX; + int16_t magY; + int16_t magZ; +}; + + +// @LoggerMessage: XKF3 +// @Description: EKF3 innovations +// @Field: TimeUS: Time since system startup +// @Field: C: EKF3 core this data is for +// @Field: IVN: Innovation in velocity (North component) +// @Field: IVE: Innovation in velocity (East component) +// @Field: IVD: Innovation in velocity (Down component) +// @Field: IPN: Innovation in position (North component) +// @Field: IPE: Innovation in position (East component) +// @Field: IPD: Innovation in position (Down component) +// @Field: IMX: Innovation in magnetic field strength (X-axis component) +// @Field: IMY: Innovation in magnetic field strength (Y-axis component) +// @Field: IMZ: Innovation in magnetic field strength (Z-axis component) +// @Field: IYAW: Innovation in vehicle yaw +// @Field: IVT: Innovation in true-airspeed +// @Field: RErr: Accumulated relative error of this core with respect to active primary core +// @Field: ErSc: A consolidated error score where higher numbers are less healthy +struct PACKED log_XKF3 { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t core; + int16_t innovVN; + int16_t innovVE; + int16_t innovVD; + int16_t innovPN; + int16_t innovPE; + int16_t innovPD; + int16_t innovMX; + int16_t innovMY; + int16_t innovMZ; + int16_t innovYaw; + int16_t innovVT; + float rerr; + float errorScore; +}; + + +// @LoggerMessage: XKF4 +// @Description: EKF3 variances +// @Field: TimeUS: Time since system startup +// @Field: C: EKF3 core this data is for +// @Field: SV: Square root of the velocity variance +// @Field: SP: Square root of the position variance +// @Field: SH: Square root of the height variance +// @Field: SM: Magnetic field variance +// @Field: SVT: Square root of the total airspeed variance +// @Field: errRP: Filtered error in roll/pitch estimate +// @Field: OFN: Most recent position recent magnitude (North component) +// @Field: OFE: Most recent position recent magnitude (East component) +// @Field: FS: Filter fault status +// @Field: TS: Filter timeout status +// @Field: SS: Filter solution status +// @Field: GPS: Filter GPS status +// @Field: PI: Primary core index +struct PACKED log_XKF4 { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t core; + int16_t sqrtvarV; + int16_t sqrtvarP; + int16_t sqrtvarH; + int16_t sqrtvarM; + int16_t sqrtvarVT; + float tiltErr; + int8_t offsetNorth; + int8_t offsetEast; + uint16_t faults; + uint8_t timeouts; + uint32_t solution; + uint16_t gps; + int8_t primary; +}; + + +// @LoggerMessage: XKF5 +// @Description: EKF3 Sensor innovations (primary core) and general dumping ground +// @Field: TimeUS: Time since system startup +// @Field: C: EKF3 core this data is for +// @Field: NI: Normalised flow variance +// @Field: FIX: Optical flow LOS rate vector innovations from the main nav filter (X-axis) +// @Field: FIY: Optical flow LOS rate vector innovations from the main nav filter (Y-axis) +// @Field: AFI: Optical flow LOS rate innovation from terrain offset estimator +// @Field: HAGL: Height above ground level +// @Field: offset: Estimated vertical position of the terrain relative to the nav filter zero datum +// @Field: RI: Range finder innovations +// @Field: rng: Measured range +// @Field: Herr: Filter ground offset state error +// @Field: eAng: Magnitude of angular error +// @Field: eVel: Magnitude of velocity error +// @Field: ePos: Magnitude of position error +struct PACKED log_XKF5 { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t core; + uint8_t normInnov; + int16_t FIX; + int16_t FIY; + int16_t AFI; + int16_t HAGL; + int16_t offset; + int16_t RI; + uint16_t meaRng; + uint16_t errHAGL; + float angErr; + float velErr; + float posErr; +}; + + +// @LoggerMessage: XKFD +// @Description: EKF3 Body Frame Odometry errors +// @Field: TimeUS: Time since system startup +// @Field: C: EKF3 core this data is for +// @Field: IX: Innovation in velocity (X-axis) +// @Field: IY: Innovation in velocity (Y-axis) +// @Field: IZ: Innovation in velocity (Z-axis) +// @Field: IVX: Variance in velocity (X-axis) +// @Field: IVY: Variance in velocity (Y-axis) +// @Field: IVZ: Variance in velocity (Z-axis) +struct PACKED log_XKFD { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t core; + float velInnovX; + float velInnovY; + float velInnovZ; + float velInnovVarX; + float velInnovVarY; + float velInnovVarZ; +}; + // @LoggerMessage: XKT // @Description: EKF3 timing information // @Field: TimeUS: Time since system startup @@ -58,6 +298,44 @@ struct PACKED log_XKFM { float accel_diff_ratio; }; + +// @LoggerMessage: XKQ +// @Description: EKF3 quaternion defining the rotation from NED to XYZ (autopilot) axes +// @Field: TimeUS: Time since system startup +// @Field: C: EKF3 core this data is for +// @Field: Q1: Quaternion a term +// @Field: Q2: Quaternion b term +// @Field: Q3: Quaternion c term +// @Field: Q4: Quaternion d term +struct PACKED log_XKQ { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t core; + float q1; + float q2; + float q3; + float q4; +}; + + +// @LoggerMessage: XKFS +// @Description: EKF3 sensor selection +// @Field: TimeUS: Time since system startup +// @Field: C: EKF3 core this data is for +// @Field: MI: compass selection index +// @Field: BI: barometer selection index +// @Field: GI: GPS selection index +// @Field: AI: airspeed selection index +struct PACKED log_XKFS { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t core; + uint8_t mag_index; + uint8_t baro_index; + uint8_t gps_index; + uint8_t airspeed_index; +}; + // @LoggerMessage: XKTV // @Description: EKF3 Yaw Estimator States // @Field: TimeUS: Time since system startup @@ -72,6 +350,58 @@ struct PACKED log_XKTV { float tvd; }; +// @LoggerMessage: XKV1 +// @Description: EKF3 State variances (primary core) +// @Field: TimeUS: Time since system startup +// @Field: C: EKF3 core this data is for +// @Field: V00: Variance for state 0 +// @Field: V01: Variance for state 1 +// @Field: V02: Variance for state 2 +// @Field: V03: Variance for state 3 +// @Field: V04: Variance for state 4 +// @Field: V05: Variance for state 5 +// @Field: V06: Variance for state 6 +// @Field: V07: Variance for state 7 +// @Field: V08: Variance for state 8 +// @Field: V09: Variance for state 9 +// @Field: V10: Variance for state 10 +// @Field: V11: Variance for state 11 + +// @LoggerMessage: XKV2 +// @Description: more EKF3 State Variances (primary core) +// @Field: TimeUS: Time since system startup +// @Field: C: EKF3 core this data is for +// @Field: V12: Variance for state 12 +// @Field: V13: Variance for state 13 +// @Field: V14: Variance for state 14 +// @Field: V15: Variance for state 15 +// @Field: V16: Variance for state 16 +// @Field: V17: Variance for state 17 +// @Field: V18: Variance for state 18 +// @Field: V19: Variance for state 19 +// @Field: V20: Variance for state 20 +// @Field: V21: Variance for state 21 +// @Field: V22: Variance for state 22 +// @Field: V23: Variance for state 23 +struct PACKED log_XKV { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t core; + float v00; + float v01; + float v02; + float v03; + float v04; + float v05; + float v06; + float v07; + float v08; + float v09; + float v10; + float v11; +}; + + // @LoggerMessage: XKY0 // @Description: EKF3 Yaw Estimator States // @Field: TimeUS: Time since system startup @@ -106,6 +436,7 @@ struct PACKED log_XKY0 { float wgt4; }; + // @LoggerMessage: XKY1 // @Description: EKF3 Yaw Estimator Innovations // @Field: TimeUS: Time since system startup @@ -136,7 +467,27 @@ struct PACKED log_XKY1 { float ive4; }; + #define LOG_STRUCTURE_FROM_NAVEKF3 \ + { LOG_XKF0_MSG, sizeof(log_XKF0), \ + "XKF0","QBBccCCcccccccc","TimeUS,C,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL,OFN,OFE,OFD", "s#-m---mmmmmmmm", "F--B---BBBBBBBB" }, \ + { LOG_XKF1_MSG, sizeof(log_XKF1), \ + "XKF1","QBccCfffffffccce","TimeUS,C,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ,OH", "s#ddhnnnnmmmkkkm", "F-BBB0000000BBBB" }, \ + { LOG_XKF2_MSG, sizeof(log_XKF2), \ + "XKF2","QBccccchhhhhh","TimeUS,C,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ", "s#---nnGGGGGG", "F----BBCCCCCC" }, \ + { LOG_XKF3_MSG, sizeof(log_XKF3), \ + "XKF3","QBcccccchhhccff","TimeUS,C,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT,RErr,ErSc", "s#nnnmmmGGG??--", "F-BBBBBBCCCBB00" }, \ + { LOG_XKF4_MSG, sizeof(log_XKF4), \ + "XKF4","QBcccccfbbHBIHb","TimeUS,C,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s#------??-----", "F-------??-----" }, \ + { LOG_XKF5_MSG, sizeof(log_XKF5), \ + "XKF5","QBBhhhcccCCfff","TimeUS,C,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,eAng,eVel,ePos", "s#----m???mrnm", "F-----BBBBB000" }, \ + { LOG_XKFD_MSG, sizeof(log_XKFD), \ + "XKFD","QBffffff","TimeUS,C,IX,IY,IZ,IVX,IVY,IVZ", "s#------", "F-------" }, \ + { LOG_XKFM_MSG, sizeof(log_XKFM), \ + "XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s------", "F------"}, \ + { LOG_XKFS_MSG, sizeof(log_XKFS), \ + "XKFS","QBBBBB","TimeUS,C,MI,BI,GI,AI", "s#----", "F-----" }, \ + { LOG_XKQ_MSG, sizeof(log_XKQ), "XKQ", QUAT_FMT, QUAT_LABELS, QUAT_UNITS, QUAT_MULTS }, \ { LOG_XKT_MSG, sizeof(log_XKT), \ "XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000"}, \ { LOG_XKTV_MSG, sizeof(log_XKTV), \ @@ -145,5 +496,7 @@ struct PACKED log_XKY1 { "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), \ - "XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s------", "F------"}, + { LOG_XKV1_MSG, sizeof(log_XKV), \ + "XKV1","QBffffffffffff","TimeUS,C,V00,V01,V02,V03,V04,V05,V06,V07,V08,V09,V10,V11", "s#------------", "F-------------" }, \ + { LOG_XKV2_MSG, sizeof(log_XKV), \ + "XKV2","QBffffffffffff","TimeUS,C,V12,V13,V14,V15,V16,V17,V18,V19,V20,V21,V22,V23", "s#------------", "F-------------" },