Perform autocal with EKF mag bias estimate

Remove separate mag bias estimator
fix
2026-06-24 15:10:35 +08:00 · 2024-09-17 13:48:40 +02:00 · 2024-09-16 15:26:58 +02:00 · 2024-09-16 15:26:58 +02:00
87 changed files with 132 additions and 780 deletions
@@ -90,15 +90,6 @@ then
 	gyro_calibration start
 fi

-
-if param compare -s MBE_ENABLE 1
-then
-	# conservative mag bias estimation
-	param set-default MBE_LEARN_GAIN 5
-	param set-default IMU_GYRO_CUTOFF 20
-	mag_bias_estimator start
-fi
-
 param set-default SENS_MAG_RATE 100

 sensors start
@@ -441,12 +441,6 @@ else
 	# Note: rc.vehicle_setup is the entry point for all vehicle type specific setup.
 	. ${R}etc/init.d/rc.vehicle_setup

-	# Pre-takeoff continuous magnetometer calibration
-	if param compare -s MBE_ENABLE 1
-	then
-		mag_bias_estimator start
-	fi
-
 	#
 	# Optional board mavlink streams: rc.board_mavlink
 	#
@@ -50,7 +50,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -56,7 +56,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -21,7 +21,6 @@ CONFIG_UAVCANNODE_SAFETY_BUTTON=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 # CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
@@ -4,7 +4,6 @@
 #------------------------------------------------------------------------------

 param set-default CBRK_IO_SAFETY 0
-param set-default MBE_ENABLE 1
 param set-default SENS_IMU_CLPNOTI 0

 safety_button start
@@ -22,7 +22,6 @@ CONFIG_UAVCANNODE_SAFETY_BUTTON=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 # CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
@@ -7,7 +7,6 @@ param set-default CBRK_IO_SAFETY 0
 param set-default CANNODE_SUB_MBD 1
 param set-default CANNODE_SUB_RTCM 1
 param set-default GPS_1_GNSS 63
-param set-default MBE_ENABLE 1
 param set-default SENS_IMU_CLPNOTI 0

 safety_button start
@@ -57,7 +57,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MAVLINK_DIALECT="development"
@@ -38,7 +38,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MAVLINK_DIALECT="development"
@@ -22,7 +22,6 @@ CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 # CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
@@ -6,7 +6,6 @@
 param set-default CBRK_IO_SAFETY 0
 param set-default CANNODE_SUB_MBD 1
 param set-default CANNODE_SUB_RTCM 1
-param set-default MBE_ENABLE 1
 param set-default SENS_IMU_CLPNOTI 0

 safety_button start
@@ -21,7 +21,6 @@ CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -52,7 +52,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -40,7 +40,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -23,7 +23,6 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -24,7 +24,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -58,7 +58,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -59,7 +59,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -56,7 +56,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -57,7 +57,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -56,7 +56,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -42,7 +42,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -20,7 +20,6 @@ CONFIG_UAVCANNODE_SAFETY_BUTTON=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 # CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
@@ -48,7 +48,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -51,7 +51,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -21,7 +21,6 @@ CONFIG_UAVCANNODE_SAFETY_BUTTON=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 # CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
@@ -52,7 +52,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -54,7 +54,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -53,7 +53,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -54,7 +54,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -60,7 +60,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -21,7 +21,6 @@ CONFIG_UAVCANNODE_SAFETY_BUTTON=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
 # CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
@@ -39,7 +39,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -41,7 +41,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -41,7 +41,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -44,7 +44,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -58,7 +58,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -50,7 +50,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -55,7 +55,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -54,7 +54,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -54,7 +54,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -51,7 +51,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -52,7 +52,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -52,7 +52,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -55,7 +55,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -56,7 +56,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -57,7 +57,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -58,7 +58,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -42,7 +42,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
@@ -29,7 +29,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
@@ -33,7 +33,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
@@ -61,7 +61,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -61,7 +61,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -57,7 +57,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -63,7 +63,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -67,7 +67,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MAVLINK_DIALECT="development"
@@ -52,7 +52,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -56,7 +56,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -64,7 +64,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MAVLINK_DIALECT="development"
@@ -65,7 +65,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -302,7 +302,6 @@
 *(.text._ZThn24_N22MulticopterRateControl3RunEv)
 *(.text._ZN26MavlinkStreamManualControl4sendEv)
 *(.text._ZN27MavlinkStreamOpticalFlowRad4sendEv)
-*(.text._ZN18mag_bias_estimator16MagBiasEstimator3RunEv)
 *(.text._ZN4uORB7Manager11orb_publishEPK12orb_metadataPvPKv)
 *(.text._ZN24MavlinkParametersManager18send_untransmittedEv)
 *(.text._ZN10MavlinkFTP4sendEv)
@@ -677,7 +676,6 @@
 *(.text._ZN13BatteryStatus21parameter_update_pollEb)
 *(.text._ZN3RTL18updateDatamanCacheEv)
 *(.text.__ieee754_sqrtf)
-*(.text._ZThn24_N18mag_bias_estimator16MagBiasEstimator3RunEv)
 *(.text.__kernel_sin)
 *(.text._ZN11MissionBase17parameters_updateEv)
 *(.text.nx_start)
@@ -40,7 +40,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -31,7 +31,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MAVLINK_DIALECT="development"
@@ -24,7 +24,6 @@ CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -42,7 +42,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -46,7 +46,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -54,7 +54,6 @@ CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -29,7 +29,6 @@ CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -54,7 +54,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -54,7 +54,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -46,7 +46,6 @@ CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
-CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -85,7 +85,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)

 		if (global_origin().isInitialized()) {

-			bool origin_newer_than_last_mag = (global_origin().getProjectionReferenceTimestamp() > aid_src.time_last_fuse);
+			bool origin_newer_than_last_mag = (global_origin().getProjectionReferenceTimestamp() >= aid_src.time_last_fuse);

 			if (global_origin_valid()
 			    && (origin_newer_than_last_mag || (local_position_is_valid() && isTimedOut(_wmm_mag_time_last_checked, 10e6)))
@@ -163,21 +163,23 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 						     && mag_sample.mag.isAllFinite();

 		const bool starting_conditions_passing = continuing_conditions_passing
-				&& checkMagField(mag_sample.mag)
 				&& (_mag_counter > 3) // wait until we have more than a few samples through the filter
 				&& (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame
 				&& (_state_reset_status.reset_count.quat ==
 				    _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset
 				&& isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL);

-		checkMagHeadingConsistency(mag_sample);
+		checkMagHeadingConsistency(mag_sample.mag - _state.mag_B);

 		const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
+		const bool mag_field_checks_passing = checkMagField(mag_sample.mag - _state.mag_B);


 		{
 			const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding;
+
 			const bool common_conditions_passing = _control_status.flags.mag
+							       && mag_field_checks_passing
 							       && ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding)
 									       || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
 							       && !_control_status.flags.mag_fault
@@ -194,8 +196,6 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 							    || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D));
 		}

-		// TODO: allow clearing mag_fault if mag_3d is good?
-
 		if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {
 			ECL_INFO("starting mag 3D fusion");

@@ -210,11 +210,13 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)

 		if (_control_status.flags.mag) {

-			if (continuing_conditions_passing && _control_status.flags.yaw_align) {
+			if (continuing_conditions_passing) {

 				if (checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_not_moving)) {
 					ECL_INFO("reset to %s", AID_SRC_NAME);
-					resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
+					const bool reset_heading = !_control_status.flags.yaw_align || _control_status.flags.mag_hdg
+								   || _control_status.flags.mag_3D;
+					resetMagStates(_mag_lpf.getState(), reset_heading);
 					aid_src.time_last_fuse = imu_sample.time_us;

 				} else {
@@ -223,7 +225,11 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 					// declination angle over time.
 					const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg;
 					const bool update_tilt = _control_status.flags.mag_3D;
-					fuseMag(mag_sample.mag, R_MAG, H, aid_src, update_all_states, update_tilt);
+					const bool fused = fuseMag(mag_sample.mag, R_MAG, H, aid_src, update_all_states, update_tilt);
+
+					if (!_control_status.flags.yaw_align && fused && mag_field_checks_passing) {
+						_control_status.flags.yaw_align = true;
+					}

 					// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
 					if (update_all_states && update_tilt) {
@@ -296,7 +302,12 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 					aid_src.time_last_fuse = imu_sample.time_us;

 					if (reset_heading) {
-						_control_status.flags.yaw_align = true;
+						if (mag_field_checks_passing) {
+							// Just initialize the heading to start estimating the
+							// mag states but do not consider the heading as aligned
+							_control_status.flags.yaw_align = true;
+						}
+
 						resetAidSourceStatusZeroInnovation(aid_src);
 					}

@@ -455,23 +466,15 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
 	}
 }

-void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
+void Ekf::checkMagHeadingConsistency(const Vector3f &mag)
 {
-	// use mag bias if variance good
-	Vector3f mag_bias{0.f, 0.f, 0.f};
-	const Vector3f mag_bias_var = getMagBiasVariance();
-
-	if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) {
-		mag_bias = _state.mag_B;
-	}
-
 	// calculate mag heading
 	// Rotate the measurements into earth frame using the zero yaw angle
 	const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);

 	// the angle of the projection onto the horizontal gives the yaw angle
 	// calculate the yaw innovation and wrap to the interval between +-pi
-	const Vector3f mag_earth_pred = R_to_earth * (mag_sample.mag - mag_bias);
+	const Vector3f mag_earth_pred = R_to_earth * mag;
 	const float declination = getMagDeclination();
 	const float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination;

@@ -497,17 +500,21 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
 	}
 }

-bool Ekf::checkMagField(const Vector3f &mag_sample)
+bool Ekf::checkMagField(const Vector3f &mag)
 {
 	_control_status.flags.mag_field_disturbed = false;

+	const Vector3f mag_earth = _R_to_earth * mag;
+
+	_mag_strength = mag.length();
+	_mag_inclination = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f));
+
 	if (_params.mag_check == 0) {
 		// skip all checks
 		return true;
 	}

 	bool is_check_failing = false;
-	_mag_strength = mag_sample.length();

 	if (_params.mag_check & static_cast<int32_t>(MagCheckMask::STRENGTH)) {
 		if (PX4_ISFINITE(_wmm_field_strength_gauss)) {
@@ -523,16 +530,13 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
 			constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
 			constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss

-			if (!isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size)) {
+			if (!isMeasuredMatchingExpected(mag.length(), average_earth_mag_field_strength, average_earth_mag_gate_size)) {
 				_control_status.flags.mag_field_disturbed = true;
 				is_check_failing = true;
 			}
 		}
 	}

-	const Vector3f mag_earth = _R_to_earth * mag_sample;
-	_mag_inclination = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f));
-
 	if (_params.mag_check & static_cast<int32_t>(MagCheckMask::INCLINATION)) {
 		if (PX4_ISFINITE(_wmm_inclination_rad)) {
 			const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg);
@@ -648,10 +652,10 @@ bool Ekf::updateWorldMagneticModel(const double latitude_deg, const double longi
 		    || strength_changed
 		   ) {

-			ECL_DEBUG("WMM declination updated %.3f -> %.3f deg (lat=%.6f, lon=%.6f)",
-				  (double)math::degrees(_wmm_declination_rad), (double)math::degrees(declination_rad),
-				  (double)latitude_deg, (double)longitude_deg
-				 );
+			ECL_INFO("WMM declination updated %.3f -> %.3f deg (lat=%.6f, lon=%.6f)",
+				 (double)math::degrees(_wmm_declination_rad), (double)math::degrees(declination_rad),
+				 (double)latitude_deg, (double)longitude_deg
+				);

 			_wmm_declination_rad = declination_rad;
 			_wmm_inclination_rad = inclination_rad;
@@ -1063,7 +1063,7 @@ private:
 	void resetMagStates(const Vector3f &mag, bool reset_heading = true);
 	bool haglYawResetReq();

-	void checkMagHeadingConsistency(const magSample &mag_sample);
+	void checkMagHeadingConsistency(const Vector3f &mag);

 	bool checkMagField(const Vector3f &mag);
 	static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
@@ -200,6 +200,11 @@ bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
 	return _ekf->control_status_flags().ev_yaw;
 }

+bool EkfWrapper::isIntendingMagFusion() const
+{
+	return _ekf->control_status_flags().mag;
+}
+
 bool EkfWrapper::isIntendingMagHeadingFusion() const
 {
 	return _ekf->control_status_flags().mag_hdg;
@@ -100,6 +100,7 @@ public:
 	void disableExternalVisionHeadingFusion();
 	bool isIntendingExternalVisionHeadingFusion() const;

+	bool isIntendingMagFusion() const;
 	bool isIntendingMagHeadingFusion() const;
 	bool isIntendingMag3DFusion() const;
 	bool isMagHeadingConsistent() const;
@@ -261,7 +261,7 @@ TEST_F(EkfGpsHeadingTest, fallBackToYawEmergencyEstimator)
 	checkConvergence(true_heading, 5.f);
 }

-TEST_F(EkfGpsHeadingTest, yawJmpOnGround)
+TEST_F(EkfGpsHeadingTest, yawJumpOnGround)
 {
 	// GIVEN: the GPS yaw fusion activated
 	float gps_heading = _ekf_wrapper.getYawAngle();
@@ -269,21 +269,28 @@ TEST_F(EkfGpsHeadingTest, yawJmpOnGround)
 	_sensor_simulator.runSeconds(1);
 	_ekf->set_in_air_status(false);

+	// Mag data is fused to estimate the mag bias and earth mag filed states
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingMagFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
+
 	// WHEN: the measurement suddenly changes
 	const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
 	gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(45.f));
 	_sensor_simulator._gps.setYaw(gps_heading);
 	_sensor_simulator.runSeconds(8);

-	// THEN: the fusion should stop, reset to mag
+	// THEN: the fusion should stop and mag heading fusion should start without reset as
+	// mag fusion is already running
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
 	EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
-	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter);

 	// AND THEN: restart GNSS yaw fusion
 	_sensor_simulator.runSeconds(5);
 	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
-	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2);
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
 	EXPECT_LT(fabsf(matrix::wrap_pi(_ekf_wrapper.getYawAngle() - gps_heading)), math::radians(1.f));
 }

@@ -115,11 +115,13 @@ TEST_F(EkfMagTest, noInitLargeStrength)
 	const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
 	_sensor_simulator.runSeconds(_init_duration_s);

-	// THEN: the fusion shouldn't start
+	// THEN: mag heading and mag 3d fusion shouldn't start
 	EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
 	EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
+
+	// BUT: the heading is reset because mag fusion started to estimate the mag states
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().yaw_align);
-	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter);
 }

 TEST_F(EkfMagTest, suddenLargeStrength)
@@ -159,15 +161,25 @@ TEST_F(EkfMagTest, noInitLargeInclination)
 	_sensor_simulator._mag.setData(mag_data);

 	const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
-	_sensor_simulator.runSeconds(_init_duration_s + 10.f); // live some extra time fo GNSS checks to pass
+	_sensor_simulator.runSeconds(_init_duration_s);

-	// THEN: the fusion shouldn't start
+	// THEN: the heading is reset because mag fusion started to estimate the mag states but
+	// no other mag fusion can start for now
 	EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
 	EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().yaw_align);
-	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter);
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);

-	// BUT then: as soon as there is some meaningful data
+	// BUT WHEN: the WMM is obtained, the inclination check can be performed
+	_sensor_simulator.runSeconds(_init_duration_s + 10.f); // give some extra time fo GNSS checks to pass
+
+	// THEN: mag heading and mag 3d fusion shouldn't start because the inclination check is failing
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
+	EXPECT_EQ(0, (int) _ekf->control_status_flags().yaw_align);
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2);
+
+	// BUT WHEN: there is some meaningful data
 	const float mag_heading = -M_PI_F / 7.f;
 	mag_data = Vector3f(0.2f * cosf(mag_heading), -0.2f * sinf(mag_heading), 0.4f);
 	_sensor_simulator._mag.setData(mag_data);
@@ -177,11 +189,9 @@ TEST_F(EkfMagTest, noInitLargeInclination)
 	float decl_deg = 0.f;
 	_ekf->get_mag_decl_deg(decl_deg);

-	// THEN: the fusion initializes using the mag data and runs normally
-	EXPECT_NEAR(_ekf_wrapper.getYawAngle(), mag_heading + radians(decl_deg), radians(1.f));
+	// THEN: the mag heading fusion can start
 	EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
 	EXPECT_EQ(1, (int) _ekf->control_status_flags().yaw_align);
-	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
 }

 TEST_F(EkfMagTest, suddenInclinationChange)
@@ -87,7 +87,6 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("landing_gear_wheel", 100);
 	add_optional_topic("landing_target_pose", 1000);
 	add_optional_topic("launch_detection_status", 200);
-	add_optional_topic("magnetometer_bias_estimate", 200);
 	add_topic("manual_control_setpoint", 200);
 	add_topic("manual_control_switches");
 	add_topic("mission_result");
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-px4_add_module(
-	MODULE modules__mag_bias_estimator
-	MAIN mag_bias_estimator
-	COMPILE_FLAGS
-		#-DDEBUG_BUILD
-	SRCS
-		MagBiasEstimator.cpp
-		MagBiasEstimator.hpp
-	DEPENDS
-		px4_work_queue
-)
@@ -1,5 +0,0 @@
-menuconfig MODULES_MAG_BIAS_ESTIMATOR
-	bool "mag_bias_estimator"
-	default n
-	---help---
-		Enable support for mag_bias_estimator
@@ -1,299 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "MagBiasEstimator.hpp"
-
-using namespace time_literals;
-using matrix::Vector3f;
-
-namespace mag_bias_estimator
-{
-
-MagBiasEstimator::MagBiasEstimator() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
-{
-	_magnetometer_bias_estimate_pub.advertise();
-}
-
-MagBiasEstimator::~MagBiasEstimator()
-{
-	perf_free(_cycle_perf);
-}
-
-int MagBiasEstimator::task_spawn(int argc, char *argv[])
-{
-	MagBiasEstimator *obj = new MagBiasEstimator();
-
-	if (!obj) {
-		PX4_ERR("alloc failed");
-		return -1;
-	}
-
-	_object.store(obj);
-	_task_id = task_id_is_work_queue;
-
-	/* Schedule a cycle to start things. */
-	obj->start();
-
-	return 0;
-}
-
-void MagBiasEstimator::start()
-{
-	ScheduleOnInterval(20_ms); // 50 Hz
-}
-
-void MagBiasEstimator::Run()
-{
-	if (should_exit()) {
-		ScheduleClear();
-		exit_and_cleanup();
-	}
-
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status;
-
-		if (_vehicle_status_sub.copy(&vehicle_status)) {
-			if (_arming_state != vehicle_status.arming_state) {
-				_arming_state = vehicle_status.arming_state;
-
-				// reset on any arming state change
-				for (auto &reset : _reset_field_estimator) {
-					reset = true;
-				}
-
-				if (_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
-					ScheduleOnInterval(1_s);
-
-				} else {
-					// restore 50 Hz scheduling
-					ScheduleOnInterval(20_ms);
-				}
-			}
-
-			bool system_calibrating = vehicle_status.calibration_enabled;
-
-			if (system_calibrating != _system_calibrating) {
-				_system_calibrating = system_calibrating;
-
-				for (auto &reset : _reset_field_estimator) {
-					reset = true;
-				}
-			}
-		}
-	}
-
-	// only run when disarmed
-	if (_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
-		return;
-	}
-
-	// check for parameter updates
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
-
-		// update parameters from storage
-		updateParams();
-
-		for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
-			const auto calibration_count = _calibration[mag_index].calibration_count();
-			_calibration[mag_index].ParametersUpdate();
-
-			if (calibration_count != _calibration[mag_index].calibration_count()) {
-				_reset_field_estimator[mag_index] = true;
-			}
-
-			_bias_estimator[mag_index].setLearningGain(_param_mbe_learn_gain.get());
-		}
-	}
-
-	// do nothing during regular sensor calibration
-	if (_system_calibrating) {
-		return;
-	}
-
-	perf_begin(_cycle_perf);
-
-	// Assume a constant angular velocity during two mag samples
-	vehicle_angular_velocity_s vehicle_angular_velocity;
-
-	if (_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) {
-
-		const Vector3f angular_velocity{vehicle_angular_velocity.xyz};
-
-		bool updated = false;
-
-		for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
-			int sensor_mag_updates = 0;
-			sensor_mag_s sensor_mag;
-
-			while ((sensor_mag_updates < sensor_mag_s::ORB_QUEUE_LENGTH) && _sensor_mag_subs[mag_index].update(&sensor_mag)) {
-				sensor_mag_updates++;
-				updated = true;
-
-				// apply existing mag calibration
-				_calibration[mag_index].set_device_id(sensor_mag.device_id);
-
-				const Vector3f mag_calibrated = _calibration[mag_index].Correct(Vector3f{sensor_mag.x, sensor_mag.y, sensor_mag.z});
-
-				float dt = (sensor_mag.timestamp_sample - _timestamp_last_update[mag_index]) * 1e-6f;
-				_timestamp_last_update[mag_index] = sensor_mag.timestamp_sample;
-
-				if (dt < 0.001f || dt > 0.2f) {
-					_reset_field_estimator[mag_index] = true;
-				}
-
-				if (_reset_field_estimator[mag_index]) {
-					// reset
-					_bias_estimator[mag_index].setBias(Vector3f{});
-					_bias_estimator[mag_index].setField(mag_calibrated);
-
-					_reset_field_estimator[mag_index] = false;
-					_valid[mag_index] = false;
-					_time_valid[mag_index] = 0;
-
-				} else {
-					updated = true;
-
-					const Vector3f bias_prev = _bias_estimator[mag_index].getBias();
-
-					_bias_estimator[mag_index].updateEstimate(angular_velocity, mag_calibrated, dt);
-
-					const Vector3f &bias = _bias_estimator[mag_index].getBias();
-					const Vector3f bias_rate = (bias - bias_prev) / dt;
-
-					if (!bias.isAllFinite() || bias.longerThan(5.f)) {
-						_reset_field_estimator[mag_index] = true;
-						_valid[mag_index] = false;
-						_time_valid[mag_index] = 0;
-
-					} else {
-
-						Vector3f fitness{
-							fabsf(angular_velocity(0)) / fmaxf(fabsf(bias_rate(1)) + fabsf(bias_rate(2)), 0.02f),
-							fabsf(angular_velocity(1)) / fmaxf(fabsf(bias_rate(0)) + fabsf(bias_rate(2)), 0.02f),
-							fabsf(angular_velocity(2)) / fmaxf(fabsf(bias_rate(0)) + fabsf(bias_rate(1)), 0.02f)
-						};
-
-						const bool bias_significant = bias.longerThan(0.04f);
-						const bool has_converged = fitness(0) > 20.f || fitness(1) > 20.f || fitness(2) > 20.f;
-
-						if (bias_significant && has_converged) {
-							if (!_valid[mag_index]) {
-								_time_valid[mag_index] = hrt_absolute_time();
-							}
-
-							_valid[mag_index] = true;
-						}
-					}
-				}
-			}
-		}
-
-		if (updated) {
-			publishMagBiasEstimate();
-		}
-	}
-
-	perf_end(_cycle_perf);
-}
-
-void MagBiasEstimator::publishMagBiasEstimate()
-{
-	magnetometer_bias_estimate_s mag_bias_est{};
-
-	for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
-		const Vector3f &bias = _bias_estimator[mag_index].getBias();
-		mag_bias_est.bias_x[mag_index] = bias(0);
-		mag_bias_est.bias_y[mag_index] = bias(1);
-		mag_bias_est.bias_z[mag_index] = bias(2);
-
-		mag_bias_est.valid[mag_index] = _valid[mag_index];
-
-		if (_valid[mag_index]) {
-			mag_bias_est.valid[mag_index] = true;
-			mag_bias_est.stable[mag_index] = (hrt_elapsed_time(&_time_valid[mag_index]) > 30_s);
-		}
-	}
-
-	mag_bias_est.timestamp = hrt_absolute_time();
-	_magnetometer_bias_estimate_pub.publish(mag_bias_est);
-}
-
-int MagBiasEstimator::print_status()
-{
-	for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
-		if (_calibration[mag_index].device_id() != 0) {
-
-			_calibration[mag_index].PrintStatus();
-
-			const Vector3f &bias = _bias_estimator[mag_index].getBias();
-
-			PX4_INFO("%d (%" PRIu32 ") bias: [% 05.3f % 05.3f % 05.3f]",
-				 mag_index, _calibration[mag_index].device_id(),
-				 (double)bias(0),
-				 (double)bias(1),
-				 (double)bias(2));
-		}
-	}
-
-	return 0;
-}
-
-int MagBiasEstimator::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_ERR("%s\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-Online magnetometer bias estimator.
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("mag_bias_estimator", "system");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-	return 0;
-}
-
-extern "C" __EXPORT int mag_bias_estimator_main(int argc, char *argv[])
-{
-	return MagBiasEstimator::main(argc, argv);
-}
-
-} // namespace load_mon
@@ -1,113 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <drivers/drv_hrt.h>
-#include <lib/field_sensor_bias_estimator/FieldSensorBiasEstimator.hpp>
-#include <lib/mathlib/mathlib.h>
-#include <lib/perf/perf_counter.h>
-#include <lib/sensor_calibration/Magnetometer.hpp>
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/module.h>
-#include <px4_platform_common/module_params.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionMultiArray.hpp>
-#include <uORB/topics/magnetometer_bias_estimate.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_mag.h>
-#include <uORB/topics/vehicle_angular_velocity.h>
-#include <uORB/topics/vehicle_status.h>
-
-namespace mag_bias_estimator
-{
-
-class MagBiasEstimator : public ModuleBase<MagBiasEstimator>, public ModuleParams, public px4::ScheduledWorkItem
-{
-public:
-	MagBiasEstimator();
-	~MagBiasEstimator() override;
-
-	static int task_spawn(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[])
-	{
-		return print_usage("unknown command");
-	}
-
-	/** @see ModuleBase */
-	static int print_usage(const char *reason = nullptr);
-
-	/** @see ModuleBase::print_status() */
-	int print_status() override;
-
-	void start();
-
-private:
-	void Run() override;
-	void publishMagBiasEstimate();
-
-	static constexpr int MAX_SENSOR_COUNT = 4;
-
-	FieldSensorBiasEstimator _bias_estimator[MAX_SENSOR_COUNT];
-	hrt_abstime _timestamp_last_update[MAX_SENSOR_COUNT] {};
-
-	uORB::SubscriptionMultiArray<sensor_mag_s, MAX_SENSOR_COUNT> _sensor_mag_subs{ORB_ID::sensor_mag};
-	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
-	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-
-	uORB::Publication<magnetometer_bias_estimate_s> _magnetometer_bias_estimate_pub{ORB_ID(magnetometer_bias_estimate)};
-
-	calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];
-
-	hrt_abstime _time_valid[MAX_SENSOR_COUNT] {};
-
-	bool _reset_field_estimator[MAX_SENSOR_COUNT] {};
-	bool _valid[MAX_SENSOR_COUNT] {};
-
-	uint8_t _arming_state{0};
-	bool _system_calibrating{false};
-
-	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::MBE_LEARN_GAIN>) _param_mbe_learn_gain
-	)
-};
-
-} // namespace mag_bias_estimator
@@ -1,58 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * Enable online mag bias calibration
- *
- * This enables continuous calibration of the magnetometers
- * before takeoff using gyro data.
- *
- * @boolean
- * @reboot_required true
- * @group Magnetometer Bias Estimator
- */
-PARAM_DEFINE_INT32(MBE_ENABLE, 1);
-
-/**
- * Mag bias estimator learning gain
- *
- * Increase to make the estimator more responsive
- * Decrease to make the estimator more robust to noise
- *
- * @min 0.1
- * @max 100
- * @increment 0.1
- * @decimal 1
- * @group Magnetometer Bias Estimator
- */
-PARAM_DEFINE_FLOAT(MBE_LEARN_GAIN, 18.f);
@@ -147,7 +147,6 @@ bool VehicleMagnetometer::ParametersUpdate(bool force)

 			// update mag priority (CAL_MAGx_PRIO)
 			for (int mag = 0; mag < MAX_SENSOR_COUNT; mag++) {
-				bool clear_online_mag_cal = false;

 				const auto calibration_count = _calibration[mag].calibration_count();
 				const int32_t priority_old = _calibration[mag].priority();
@@ -168,24 +167,10 @@ bool VehicleMagnetometer::ParametersUpdate(bool force)

 					if (calibration_count != _calibration[mag].calibration_count()) {
 						calibration_updated = true;
-						clear_online_mag_cal = true;
 					}

 				} else {
 					_priority[mag] = 0;
-					clear_online_mag_cal = true;
-				}
-
-				if (clear_online_mag_cal) {
-					// clear any mag bias estimate
-					_calibration_estimator_bias[mag].zero();
-
-					for (auto &cal : _mag_cal) {
-						// clear any in flight mag calibration
-						if (cal.device_id == _calibration[mag].device_id()) {
-							cal = {};
-						}
-					}
 				}
 			}

@@ -200,60 +185,6 @@ bool VehicleMagnetometer::ParametersUpdate(bool force)
 	return false;
 }

-void VehicleMagnetometer::UpdateMagBiasEstimate()
-{
-	if (_magnetometer_bias_estimate_sub.updated()) {
-		// Continuous mag calibration is running when not armed
-		magnetometer_bias_estimate_s mag_bias_est;
-
-		if (_magnetometer_bias_estimate_sub.copy(&mag_bias_est)) {
-			bool parameters_notify = false;
-
-			for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
-				if (mag_bias_est.valid[mag_index] && (mag_bias_est.timestamp > _last_calibration_update)) {
-
-					const Vector3f bias{mag_bias_est.bias_x[mag_index],
-							    mag_bias_est.bias_y[mag_index],
-							    mag_bias_est.bias_z[mag_index]};
-
-					_calibration_estimator_bias[mag_index] = bias;
-
-					// set initial mag calibration if disarmed, mag uncalibrated, and valid estimated bias available
-					if (_param_sens_mag_autocal.get() && !_armed && mag_bias_est.stable[mag_index]
-					    && (_calibration[mag_index].device_id() != 0) && !_calibration[mag_index].calibrated()) {
-
-						const Vector3f old_offset = _calibration[mag_index].offset();
-
-						// set initial mag calibration
-						const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]);
-
-						if (_calibration[mag_index].set_offset(offset)) {
-							// save parameters with preferred calibration slot to current sensor index
-							_calibration[mag_index].ParametersSave(mag_index);
-
-							PX4_INFO("mag %d (%" PRIu32 ") setting offsets [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f]",
-								 mag_index, _calibration[mag_index].device_id(),
-								 (double)old_offset(0), (double)old_offset(1), (double)old_offset(2),
-								 (double)_calibration[mag_index].offset()(0),
-								 (double)_calibration[mag_index].offset()(1),
-								 (double)_calibration[mag_index].offset()(2));
-
-							_calibration_estimator_bias[mag_index].zero();
-
-							parameters_notify = true;
-						}
-					}
-				}
-			}
-
-			if (parameters_notify) {
-				param_notify_changes();
-				_last_calibration_update = hrt_absolute_time();
-			}
-		}
-	}
-}
-
 void VehicleMagnetometer::UpdateMagCalibration()
 {
 	// State variance assumed for magnetometer bias storage.
@@ -263,63 +194,66 @@ void VehicleMagnetometer::UpdateMagCalibration()
 	static constexpr float min_var_allowed = magb_vref * 0.01f;
 	static constexpr float max_var_allowed = magb_vref * 500.f;

-	if (_armed) {
-		static constexpr uint8_t mag_cal_size = sizeof(_mag_cal) / sizeof(_mag_cal[0]);
+	static constexpr uint8_t mag_cal_size = sizeof(_mag_cal) / sizeof(_mag_cal[0]);

-		for (int i = 0; i < math::min(_estimator_sensor_bias_subs.size(), mag_cal_size); i++) {
-			estimator_sensor_bias_s estimator_sensor_bias;
+	for (int i = 0; i < math::min(_estimator_sensor_bias_subs.size(), mag_cal_size); i++) {
+		estimator_sensor_bias_s estimator_sensor_bias;

-			if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias)) {
+		if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias)) {

-				const Vector3f bias{estimator_sensor_bias.mag_bias};
-				const Vector3f bias_variance{estimator_sensor_bias.mag_bias_variance};
+			const Vector3f bias{estimator_sensor_bias.mag_bias};
+			const Vector3f bias_variance{estimator_sensor_bias.mag_bias_variance};

-				const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)
-						   && (estimator_sensor_bias.mag_device_id != 0)
-						   && estimator_sensor_bias.mag_bias_valid
-						   && estimator_sensor_bias.mag_bias_stable
-						   && (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed);
+			const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)
+					   && (estimator_sensor_bias.mag_device_id != 0)
+					   && estimator_sensor_bias.mag_bias_valid
+					   && estimator_sensor_bias.mag_bias_stable
+					   && (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed);

-				if (valid) {
-					// find corresponding mag calibration
-					for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
-						if (_calibration[mag_index].device_id() == estimator_sensor_bias.mag_device_id) {
+			if (valid) {
+				// find corresponding mag calibration
+				for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
+					if (_calibration[mag_index].device_id() == estimator_sensor_bias.mag_device_id) {

-							_mag_cal[i].device_id = estimator_sensor_bias.mag_device_id;
+						_mag_cal[i].device_id = estimator_sensor_bias.mag_device_id;

-							// readd estimated bias that was removed before publishing vehicle_magnetometer (_calibration_estimator_bias)
-							const Vector3f mag_cal_offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias +
-											_calibration_estimator_bias[mag_index]);
+						const Vector3f mag_cal_offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias);

-							if ((mag_cal_offset - _mag_cal[i].offset).longerThan(0.001f)) {
-								const Vector3f mag_cal_orig{_calibration[mag_index].offset()};
+						if ((mag_cal_offset - _mag_cal[i].offset).longerThan(0.001f)) {
+							const Vector3f mag_cal_orig{_calibration[mag_index].offset()};

-								PX4_DEBUG("%d (%" PRIu32 ") EST:%d offset: [%.2f, %.2f, %.2f]->[%.2f, %.2f, %.2f] (full [%.3f, %.3f, %.3f])",
-									  mag_index, _calibration[mag_index].device_id(), i,
-									  (double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2),
-									  (double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2),
-									  (double)_mag_cal[i].offset(0), (double)_mag_cal[i].offset(1), (double)_mag_cal[i].offset(2));
-							}
-
-							_mag_cal[i].offset = mag_cal_offset;
-							_mag_cal[i].variance = bias_variance;
-
-							_in_flight_mag_cal_available = true;
-							break;
+							PX4_DEBUG("%d (%" PRIu32 ") EST:%d offset: [%.2f, %.2f, %.2f]->[%.2f, %.2f, %.2f] (full [%.3f, %.3f, %.3f])",
+								  mag_index, _calibration[mag_index].device_id(), i,
+								  (double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2),
+								  (double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2),
+								  (double)_mag_cal[i].offset(0), (double)_mag_cal[i].offset(1), (double)_mag_cal[i].offset(2));
 						}
+
+						_mag_cal[i].offset = mag_cal_offset;
+						_mag_cal[i].variance = bias_variance;
+
+						_mag_cal_available = true;
+						break;
 					}
 				}
 			}
 		}
+	}

-	} else if (_in_flight_mag_cal_available) {
-		// not armed and mag cal available
+	if (!_armed && _mag_cal_available) {
 		bool calibration_param_save_needed = false;
-		// iterate through available bias estimates and fuse them sequentially using a Kalman Filter scheme
-		Vector3f state_variance{magb_vref, magb_vref, magb_vref};

 		for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
-			// apply all valid saved offsets
+			const bool do_post_flight_mag_correction = _was_armed;
+			const bool do_auto_calibration = _param_sens_mag_autocal.get() && !_calibration[mag_index].calibrated();
+
+			if (!do_post_flight_mag_correction && !do_auto_calibration) {
+				continue;
+			}
+
+			// iterate through available bias estimates and fuse them sequentially using a Kalman Filter scheme
+			Vector3f state_variance{magb_vref, magb_vref, magb_vref};
+
 			for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
 				if ((_calibration[mag_index].device_id() != 0) && (_mag_cal[i].device_id == _calibration[mag_index].device_id())) {
 					const Vector3f mag_cal_orig{_calibration[mag_index].offset()};
@@ -328,15 +262,23 @@ void VehicleMagnetometer::UpdateMagCalibration()
 					const Vector3f &observation{_mag_cal[i].offset};
 					const Vector3f &obs_variance{_mag_cal[i].variance};

-					const Vector3f innovation{mag_cal_orig - observation};
-					const Vector3f innovation_variance{state_variance + obs_variance};
-					const Vector3f kalman_gain{state_variance.edivide(innovation_variance)};
+					Vector3f mag_cal_offset;

-					// new offset
-					const Vector3f mag_cal_offset{mag_cal_orig - innovation.emult(kalman_gain)};
+					if (!_calibration[mag_index].calibrated()) {
+						mag_cal_offset = observation;
+						state_variance = obs_variance;

-					for (int axis_index = 0; axis_index < 3; axis_index++) {
-						state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain(axis_index)), 0.f);
+					} else {
+						const Vector3f innovation{mag_cal_orig - observation};
+						const Vector3f innovation_variance{state_variance + obs_variance};
+						const Vector3f kalman_gain{state_variance.edivide(innovation_variance)};
+
+						// new offset
+						mag_cal_offset = mag_cal_orig - innovation.emult(kalman_gain);
+
+						for (int axis_index = 0; axis_index < 3; axis_index++) {
+							state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain(axis_index)), 0.f);
+						}
 					}

 					if (_calibration[mag_index].set_offset(mag_cal_offset)) {
@@ -349,8 +291,6 @@ void VehicleMagnetometer::UpdateMagCalibration()

 						_calibration[mag_index].ParametersSave();

-						_calibration_estimator_bias[mag_index].zero();
-
 						calibration_param_save_needed = true;

 					} else {
@@ -375,7 +315,7 @@ void VehicleMagnetometer::UpdateMagCalibration()
 			_mag_cal[i] = {};
 		}

-		_in_flight_mag_cal_available = false;
+		_mag_cal_available = false;
 	}
 }

@@ -428,6 +368,7 @@ void VehicleMagnetometer::Run()
 		vehicle_control_mode_s vehicle_control_mode;

 		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
+			_was_armed = _armed;
 			_armed = vehicle_control_mode.flag_armed;
 		}
 	}
@@ -438,8 +379,6 @@ void VehicleMagnetometer::Run()

 	UpdatePowerCompensation();

-	UpdateMagBiasEstimate();
-
 	bool updated[MAX_SENSOR_COUNT] {};

 	for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
@@ -489,7 +428,7 @@ void VehicleMagnetometer::Run()
 						ParametersUpdate(true);
 					}

-					const Vector3f vect{_calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z}) - _calibration_estimator_bias[uorb_index]};
+					const Vector3f vect{_calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z})};

 					float mag_array[3] {vect(0), vect(1), vect(2)};
 					_voter.put(uorb_index, report.timestamp, mag_array, report.error_count, _priority[uorb_index]);
@@ -52,7 +52,6 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/estimator_sensor_bias.h>
-#include <uORB/topics/magnetometer_bias_estimate.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/sensor_preflight_mag.h>
@@ -91,7 +90,6 @@ private:
 	 */
 	void calcMagInconsistency();

-	void UpdateMagBiasEstimate();
 	void UpdateMagCalibration();
 	void UpdatePowerCompensation();

@@ -112,13 +110,12 @@ private:

 	uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint), 0};
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0};
-	uORB::Subscription _magnetometer_bias_estimate_sub{ORB_ID(magnetometer_bias_estimate)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};

 	// Used to check, save and use learned magnetometer biases
 	uORB::SubscriptionMultiArray<estimator_sensor_bias_s> _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias};

-	bool _in_flight_mag_cal_available{false}; ///< from navigation filter
+	bool _mag_cal_available{false}; ///< from navigation filter

 	struct MagCal {
 		uint32_t device_id{0};
@@ -135,8 +132,6 @@ private:

 	hrt_abstime _last_calibration_update{0};

-	matrix::Vector3f _calibration_estimator_bias[MAX_SENSOR_COUNT] {};
-
 	calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];

 	// Magnetometer interference compensation
@@ -174,6 +169,7 @@ private:
 	int8_t _selected_sensor_sub_index{-1};

 	bool _armed{false};
+	bool _was_armed{false};

 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_comp_typ,
@@ -393,10 +393,6 @@ menu "Zenoh publishers/subscribers"
 		bool "mag_worker_data"
 		default n

-	config ZENOH_PUBSUB_MAGNETOMETER_BIAS_ESTIMATE
-		bool "magnetometer_bias_estimate"
-		default n
-
 	config ZENOH_PUBSUB_MANUAL_CONTROL_SETPOINT
 		bool "manual_control_setpoint"
 		default n
@@ -968,7 +964,6 @@ config ZENOH_PUBSUB_ALL_SELECTION
 	select ZENOH_PUBSUB_LOG_MESSAGE
 	select ZENOH_PUBSUB_LOGGER_STATUS
 	select ZENOH_PUBSUB_MAG_WORKER_DATA
-	select ZENOH_PUBSUB_MAGNETOMETER_BIAS_ESTIMATE
 	select ZENOH_PUBSUB_MANUAL_CONTROL_SETPOINT
 	select ZENOH_PUBSUB_MANUAL_CONTROL_SWITCHES
 	select ZENOH_PUBSUB_MAVLINK_LOG
Author	SHA1	Message	Date
bresch	992c110d82	Perform autocal with EKF mag bias estimate Remove separate mag bias estimator	2024-09-17 13:48:40 +02:00
bresch	6648856d44	fix	2024-09-16 15:26:58 +02:00
Daniel Agar	b49c0ebf86	ekf2: check mag field with mag bias removed - this is to allow the estimated mag bias to clear mag_field_disturbed and unlock mag_3D/mag_hdg	2024-09-16 15:26:58 +02:00