diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 24861543bac861..73c5135e8f37e2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -4,6 +4,7 @@ #include "AP_NavEKF3_core.h" #include +#include #include // minimum GPS horizontal speed required to use GPS ground course for yaw alignment (m/s) @@ -425,12 +426,24 @@ void NavEKF3_core::SelectMagFusion() if (dataReady) { // use the simple method of declination to maintain heading if we cannot use the magnetic field states if(inhibitMagStates || magStateResetRequest || !magStateInitComplete) { +#if HAL_LOGGING_ENABLED + if (magFusionSel != MagFuseSel::FUSE_YAW) { + magFusionSel = MagFuseSel::FUSE_YAW; + WriteMagFusionSelection(); + } +#endif fuseEulerYaw(yawFusionMethod::MAGNETOMETER); // zero the test ratio output from the inactive 3-axis magnetometer fusion magTestRatio.zero(); } else { +#if HAL_LOGGING_ENABLED + if (magFusionSel != MagFuseSel::FUSE_MAG) { + magFusionSel = MagFuseSel::FUSE_MAG; + WriteMagFusionSelection(); + } +#endif // if we are not doing aiding with earth relative observations (eg GPS) then the declination is // maintained by fusing declination as a synthesised observation // We also fuse declination if we are using the WMM tables @@ -462,6 +475,20 @@ void NavEKF3_core::SelectMagFusion() } } +#if HAL_LOGGING_ENABLED +// log changes to magnetometer fusion selection +void NavEKF3_core::WriteMagFusionSelection() +{ + const struct log_XKMF pkt{ + LOG_PACKET_HEADER_INIT(LOG_XKMF_MSG), + time_us : AP_HAL::micros64(), + core : core_index, + fuse_sel : static_cast(magFusionSel) + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); +} +#endif + /* * Fuse magnetometer measurements using explicit algebraic equations generated with Matlab symbolic toolbox. * The script file used to generate these and other equations in this filter can be found here: diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 008c9e2c01c7e5..ed52e033c36901 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -446,6 +446,9 @@ void NavEKF3_core::InitialiseVariablesMag() #endif needMagBodyVarReset = false; needEarthBodyVarReset = false; +#if HAL_LOGGING_ENABLED + magFusionSel = MagFuseSel::NOT_FUSING; +#endif } /* diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index b6b21be2a0b3b1..af7cf125aec438 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -422,6 +422,15 @@ class NavEKF3_core : public NavEKF_core_common // 6 was EXTERNAL_YAW_FALLBACK (do not use) }; +#if HAL_LOGGING_ENABLED + // magnetometer fusion selections + enum class MagFuseSel { + NOT_FUSING = 0, + FUSE_YAW = 1, + FUSE_MAG = 2 + }; +#endif + // are we using (aka fusing) a non-compass yaw? bool using_noncompass_for_yaw(void) const; @@ -800,6 +809,11 @@ class NavEKF3_core : public NavEKF_core_common // determine when to perform fusion of magnetometer measurements void SelectMagFusion(); +#if HAL_LOGGING_ENABLED + // log changes to magnetometer fusion selection + void WriteMagFusionSelection(); +#endif + // determine when to perform fusion of true airspeed measurements void SelectTasFusion(); @@ -1418,6 +1432,9 @@ class NavEKF3_core : public NavEKF_core_common ftype posDownAtLastMagReset; // vertical position last time the mag states were reset (m) ftype yawInnovAtLastMagReset; // magnetic yaw innovation last time the yaw and mag field states were reset (rad) QuaternionF quatAtLastMagReset; // quaternion states last time the mag states were reset +#if HAL_LOGGING_ENABLED + MagFuseSel magFusionSel; // magnetometer fusion selection +#endif // Used by on ground movement check required when operating on ground without a yaw reference ftype gyro_diff; // filtered gyro difference (rad/s) diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h index 59ea98f9e42b5b..a11c0576baabf9 100644 --- a/libraries/AP_NavEKF3/LogStructure.h +++ b/libraries/AP_NavEKF3/LogStructure.h @@ -15,6 +15,7 @@ LOG_XKQ_MSG, \ LOG_XKT_MSG, \ LOG_XKTV_MSG, \ + LOG_XKMF_MSG, \ LOG_XKV1_MSG, \ LOG_XKV2_MSG, \ LOG_XKY0_MSG, \ @@ -373,6 +374,18 @@ struct PACKED log_XKTV { float tvd; }; +// @LoggerMessage: XKMF +// @Description: EKF3 Magnetometer Fusion Selection +// @Field: TimeUS: Time since system startup +// @Field: C: EKF3 core this data is for +// @Field: S: Magnetometer fusion selection: 0 not fusing, 1 fuse yaw, 2 fuse mag +struct PACKED log_XKMF { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t core; + uint8_t fuse_sel; +}; + // @LoggerMessage: XKV1 // @Description: EKF3 State variances (primary core) // @Field: TimeUS: Time since system startup @@ -449,6 +462,8 @@ struct PACKED log_XKV { "XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000", true }, \ { LOG_XKTV_MSG, sizeof(log_XKTV), \ "XKTV", "QBff", "TimeUS,C,TVS,TVD", "s#rr", "F-00", true }, \ + { LOG_XKMF_MSG, sizeof(log_XKMF), \ + "XKMF", "QBB", "TimeUS,C,S", "s#-", "F--", true }, \ { LOG_XKV1_MSG, sizeof(log_XKV), \ "XKV1","QBffffffffffff","TimeUS,C,V00,V01,V02,V03,V04,V05,V06,V07,V08,V09,V10,V11", "s#------------", "F-------------" , true }, \ { LOG_XKV2_MSG, sizeof(log_XKV), \