Add mag bias estimator to CAN GPS units with IMUs

- run `sensors` hub to process sensor_mag and publish vehicle_magnetometer
 - update uavcannode to use vehicle_magnetometer

ROMFS/cannode/init.d/rcS

@@ -96,6 +96,17 @@ 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
 
 uavcannode start

boards/ark/can-gps/default.px4board

@@ -14,10 +14,15 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=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
+# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y

boards/ark/can-gps/init/rc.board_defaults

@@ -4,6 +4,7 @@
 #------------------------------------------------------------------------------
 
 param set-default CBRK_IO_SAFETY 0
+param set-default MBE_ENABLE 1
 
 safety_button start
 tone_alarm start

boards/ark/can-rtk-gps/default.px4board

@@ -14,10 +14,15 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=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
+# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y

boards/ark/can-rtk-gps/init/rc.board_defaults

@@ -5,6 +5,7 @@
 
 param set-default CBRK_IO_SAFETY 0
 param set-default CANNODE_GPS_RTCM 1
+param set-default MBE_ENABLE 1
 
 safety_button start
 tone_alarm start

boards/cuav/can-gps-v1/default.px4board

@@ -1,6 +1,8 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
+CONFIG_BOARD_CONSTRAINED_FLASH=y
+CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_BOARD_COMPILE_DEFINITIONS="-DUSE_S_RGB_LED_DMA"
 CONFIG_DRIVERS_BAROMETER_MS5611=y
@@ -12,5 +14,16 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_MODULES_SENSORS=y
+# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
+# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
+# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y

boards/freefly/can-rtk-gps/default.px4board

@@ -1,16 +1,22 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m7"
 CONFIG_BOARD_ROMFSROOT="cannode"
+CONFIG_BOARD_CONSTRAINED_FLASH=y
+CONFIG_BOARD_NO_HELP=y
+CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
-CONFIG_DRIVERS_IMU_ST=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
-CONFIG_MODULES_LOAD_MON=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
+# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
 CONFIG_SYSTEMCMDS_MFT=y
 CONFIG_SYSTEMCMDS_MTD=y
@@ -19,5 +25,7 @@ CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y

boards/holybro/can-gps-v1/default.px4board

@@ -2,6 +2,7 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
+CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
@@ -12,6 +13,18 @@ CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=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
+# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y

boards/matek/gnss-m9n-f4/default.px4board

@@ -1,6 +1,8 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
+CONFIG_BOARD_CONSTRAINED_FLASH=y
+CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
 CONFIG_DRIVERS_BAROMETER_DPS310=y
@@ -10,13 +12,20 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_MAGNETOMETER_RM3100=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=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
+# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_SERIAL_PASSTHRU=y
 CONFIG_SERIAL_PASSTHRU_UBLOX=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y

src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp

@@ -38,7 +38,7 @@
 #include <uavcan/equipment/ahrs/MagneticFieldStrength2.hpp>
 
 #include <uORB/SubscriptionCallback.hpp>
-#include <uORB/topics/sensor_mag.h>
+#include <uORB/topics/vehicle_magnetometer.h>
 
 namespace uavcannode
 {
@@ -51,7 +51,7 @@ class MagneticFieldStrength2 :
 public:
 	MagneticFieldStrength2(px4::WorkItem *work_item, uavcan::INode &node) :
 		UavcanPublisherBase(uavcan::equipment::ahrs::MagneticFieldStrength2::DefaultDataTypeID),
-		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_mag)),
+		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(vehicle_magnetometer)),
 		uavcan::Publisher<uavcan::equipment::ahrs::MagneticFieldStrength2>(node)
 	{
 		this->setPriority(uavcan::TransferPriority::Default);
@@ -69,15 +69,15 @@ public:
 
 	void BroadcastAnyUpdates() override
 	{
-		// sensor_mag -> uavcan::equipment::ahrs::MagneticFieldStrength2
-		sensor_mag_s mag;
+		// vehicle_magnetometer -> uavcan::equipment::ahrs::MagneticFieldStrength2
+		vehicle_magnetometer_s vehicle_magnetometer;
 
-		if (uORB::SubscriptionCallbackWorkItem::update(&mag)) {
+		if (uORB::SubscriptionCallbackWorkItem::update(&vehicle_magnetometer)) {
 			uavcan::equipment::ahrs::MagneticFieldStrength2 magnetic_field{};
-			magnetic_field.sensor_id = mag.device_id;
-			magnetic_field.magnetic_field_ga[0] = mag.x;
-			magnetic_field.magnetic_field_ga[1] = mag.y;
-			magnetic_field.magnetic_field_ga[2] = mag.z;
+			magnetic_field.sensor_id = uORB::SubscriptionCallbackWorkItem::get_instance();
+			magnetic_field.magnetic_field_ga[0] = vehicle_magnetometer.magnetometer_ga[0];
+			magnetic_field.magnetic_field_ga[1] = vehicle_magnetometer.magnetometer_ga[1];
+			magnetic_field.magnetic_field_ga[2] = vehicle_magnetometer.magnetometer_ga[2];
 			uavcan::Publisher<uavcan::equipment::ahrs::MagneticFieldStrength2>::broadcast(magnetic_field);
 
 			// ensure callback is registered

src/modules/sensors/sensor_params_mag.c

@@ -116,4 +116,4 @@ PARAM_DEFINE_INT32(SENS_MAG_MODE, 1);
  * @category system
  * @group Sensors
  */
-PARAM_DEFINE_INT32(SENS_MAG_AUTOCAL, 0);
+PARAM_DEFINE_INT32(SENS_MAG_AUTOCAL, 1);

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -205,6 +205,8 @@ void VehicleMagnetometer::UpdateMagBiasEstimate()
 					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]);
 
@@ -212,6 +214,13 @@ void VehicleMagnetometer::UpdateMagBiasEstimate()
 							// 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;