diff --git a/libraries/AP_NavEKF/EKFGSF_Logging.cpp b/libraries/AP_NavEKF/EKFGSF_Logging.cpp new file mode 100644 index 0000000000..541da91918 --- /dev/null +++ b/libraries/AP_NavEKF/EKFGSF_Logging.cpp @@ -0,0 +1,48 @@ +#include "EKFGSF_yaw.h" + +#include + +void EKFGSF_yaw::Log_Write(uint64_t time_us, LogMessages id0, LogMessages id1, uint8_t core_index) +{ + if (!vel_fuse_running) { + return; + } + + static_assert(N_MODELS_EKFGSF >= 5, "Logging will break on <5 EKFGSF models"); + + const struct log_KY0 ky0{ + LOG_PACKET_HEADER_INIT(id0), + time_us : time_us, + core : core_index, + yaw_composite : GSF.yaw, + yaw_composite_variance : sqrtf(MAX(GSF.yaw_variance, 0.0f)), + yaw0 : EKF[0].X[2], + yaw1 : EKF[1].X[2], + yaw2 : EKF[2].X[2], + yaw3 : EKF[3].X[2], + yaw4 : EKF[4].X[2], + wgt0 : GSF.weights[0], + wgt1 : GSF.weights[1], + wgt2 : GSF.weights[2], + wgt3 : GSF.weights[3], + wgt4 : GSF.weights[4], + }; + AP::logger().WriteBlock(&ky0, sizeof(ky0)); + + const struct log_KY1 ky1{ + LOG_PACKET_HEADER_INIT(id1), + time_us : time_us, + core : core_index, + ivn0 : EKF[0].innov[0], + ivn1 : EKF[1].innov[0], + ivn2 : EKF[2].innov[0], + ivn3 : EKF[3].innov[0], + ivn4 : EKF[4].innov[0], + ive0 : EKF[0].innov[1], + ive1 : EKF[1].innov[1], + ive2 : EKF[2].innov[1], + ive3 : EKF[3].innov[1], + ive4 : EKF[4].innov[1], + }; + AP::logger().WriteBlock(&ky1, sizeof(ky1)); +} diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.cpp b/libraries/AP_NavEKF/EKFGSF_yaw.cpp index 01c9227021..0d9bd8104e 100644 --- a/libraries/AP_NavEKF/EKFGSF_yaw.cpp +++ b/libraries/AP_NavEKF/EKFGSF_yaw.cpp @@ -577,22 +577,6 @@ float EKFGSF_yaw::gaussianDensity(const uint8_t mdl_idx) const return normDist; } -bool EKFGSF_yaw::getLogData(float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const -{ - if (vel_fuse_running) { - yaw_composite = GSF.yaw; - yaw_composite_variance = GSF.yaw_variance; - for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { - yaw[mdl_idx] = EKF[mdl_idx].X[2]; - innov_VN[mdl_idx] = EKF[mdl_idx].innov[0]; - innov_VE[mdl_idx] = EKF[mdl_idx].innov[1]; - weight[mdl_idx] = GSF.weights[mdl_idx]; - } - return true; - } - return false; -} - void EKFGSF_yaw::forceSymmetry(const uint8_t mdl_idx) { float P01 = 0.5f * (EKF[mdl_idx].P[0][1] + EKF[mdl_idx].P[1][0]); diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.h b/libraries/AP_NavEKF/EKFGSF_yaw.h index 2fee668ff5..1887ae559b 100644 --- a/libraries/AP_NavEKF/EKFGSF_yaw.h +++ b/libraries/AP_NavEKF/EKFGSF_yaw.h @@ -5,6 +5,7 @@ #include #include #include +#include #define IMU_DT_MIN_SEC 0.001f // Minimum delta time between IMU samples (sec) @@ -30,10 +31,6 @@ public: // set the gyro bias in rad/sec void setGyroBias(Vector3f &gyroBias); - // get solution data for logging - // return false if yaw estimation is inactive - bool getLogData(float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const; - // get yaw estimated and corresponding variance // return false if yaw estimation is inactive bool getYawData(float &yaw, float &yawVariance) const; @@ -42,6 +39,10 @@ public: // return false if not available bool getVelInnovLength(float &velInnovLength) const; + // log EKFGSF data on behalf of an EKF caller. id0 and id1 are the + // IDs of the messages to log, e.g. LOG_NKY0_MSG, LOG_NKY1_MSG + void Log_Write(uint64_t time_us, LogMessages id0, LogMessages id1, uint8_t core_index); + private: typedef float ftype; diff --git a/libraries/AP_NavEKF/LogStructure.h b/libraries/AP_NavEKF/LogStructure.h new file mode 100644 index 0000000000..c0ee7d9327 --- /dev/null +++ b/libraries/AP_NavEKF/LogStructure.h @@ -0,0 +1,88 @@ +#pragma once + +#include + +// @LoggerMessage: XKY0,NKY0 +// @Description: EKF Yaw Estimator States +// @Field: TimeUS: Time since system startup +// @Field: C: EKF 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_KY0 { + 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,NKY1 +// @Description: EKF Yaw Estimator Innovations +// @Field: TimeUS: Time since system startup +// @Field: C: EKF 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_KY1 { + 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 KY0_FMT "QBffffffffffff" +#define KY0_LABELS "TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4" +#define KY0_UNITS "s#rrrrrrr-----" +#define KY0_MULTS "F-000000000000" + +#define KY1_FMT "QBffffffffff" +#define KY1_LABELS "TimeUS,C,IVN0,IVN1,IVN2,IVN3,IVN4,IVE0,IVE1,IVE2,IVE3,IVE4" +#define KY1_UNITS "s#nnnnnnnnnn" +#define KY1_MULTS "F-0000000000" + +#define LOG_STRUCTURE_FROM_NAVEKF \ + { LOG_XKY0_MSG, sizeof(log_KY0), \ + "XKY0", KY0_FMT, KY0_LABELS, KY0_UNITS, KY0_MULTS}, \ + { LOG_XKY1_MSG, sizeof(log_KY1), \ + "XKY1", KY1_FMT, KY1_LABELS, KY1_UNITS, KY1_MULTS }, \ + { LOG_NKY0_MSG, sizeof(log_KY0), \ + "NKY0", KY0_FMT, KY0_LABELS, KY0_UNITS, KY0_MULTS}, \ + { LOG_NKY1_MSG, sizeof(log_KY1), \ + "NKY1", KY1_FMT, KY1_LABELS, KY1_UNITS, KY1_MULTS },