Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: log mag fusion selection to XKFS

Browse Source
This commit is contained in:
Clyde McQueen 2024-03-21 10:56:00 -07:00 committed by Peter Barker
parent 24a6e6bfa1
commit 630d3fa8a7
5 changed files with 16 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -102,7 +102,8 @@ void NavEKF3_core::Log_Write_XKFS(uint64_t time_us) const
airspeed_index : getActiveAirspeed(),
source_set : frontend->sources.getPosVelYawSourceSet(),
gps_good_to_align : gpsGoodToAlign,
wait_for_gps_checks : waitingForGpsChecks
wait_for_gps_checks : waitingForGpsChecks,
mag_fusion: (uint8_t) magFusionSel
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}

2
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -425,12 +425,14 @@ 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) {
magFusionSel = MagFuseSel::FUSE_YAW;
fuseEulerYaw(yawFusionMethod::MAGNETOMETER);
// zero the test ratio output from the inactive 3-axis magnetometer fusion
magTestRatio.zero();
} else {
magFusionSel = MagFuseSel::FUSE_MAG;
// 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

1
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -452,6 +452,7 @@ void NavEKF3_core::InitialiseVariablesMag()
#endif
needMagBodyVarReset = false;
needEarthBodyVarReset = false;
magFusionSel = MagFuseSel::NOT_FUSING;
}
/*

8
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -427,6 +427,13 @@ public:
// 6 was EXTERNAL_YAW_FALLBACK (do not use)
};
// magnetometer fusion selections
enum class MagFuseSel {
NOT_FUSING = 0,
FUSE_YAW = 1,
FUSE_MAG = 2
};
// are we using (aka fusing) a non-compass yaw?
bool using_noncompass_for_yaw(void) const;
@ -1426,6 +1433,7 @@ private:
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
MagFuseSel magFusionSel; // magnetometer fusion selection
// Used by on ground movement check required when operating on ground without a yaw reference
ftype gyro_diff; // filtered gyro difference (rad/s)

4
libraries/AP_NavEKF3/LogStructure.h
View File

@ -346,6 +346,7 @@ struct PACKED log_XKQ {
// @Field: SS: Source Set (primary=0/secondary=1/tertiary=2)
// @Field: GPS_GTA: GPS good to align
// @Field: GPS_CHK_WAIT: Waiting for GPS checks to pass
// @Field: MAG_FUSION: Magnetometer fusion (0=not fusing/1=fuse yaw/2=fuse mag)
struct PACKED log_XKFS {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -357,6 +358,7 @@ struct PACKED log_XKFS {
uint8_t source_set;
uint8_t gps_good_to_align;
uint8_t wait_for_gps_checks;
uint8_t mag_fusion;
};
// @LoggerMessage: XKTV
@ -443,7 +445,7 @@ struct PACKED log_XKV {
{ LOG_XKFM_MSG, sizeof(log_XKFM), \
"XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s#-----", "F------", true }, \
{ LOG_XKFS_MSG, sizeof(log_XKFS), \
"XKFS","QBBBBBBBB","TimeUS,C,MI,BI,GI,AI,SS,GPS_GTA,GPS_CHK_WAIT", "s#-------", "F--------" , true }, \
"XKFS","QBBBBBBBBB","TimeUS,C,MI,BI,GI,AI,SS,GPS_GTA,GPS_CHK_WAIT,MAG_FUSION", "s#--------", "F---------" , true }, \
{ LOG_XKQ_MSG, sizeof(log_XKQ), "XKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#????", "F-????" , true }, \
{ LOG_XKT_MSG, sizeof(log_XKT), \
"XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000", true }, \