Added mag support to the DriverFramework mpu9250 driver. Shortened parameter names for legacy drivers. Added temporary ifdef's in the calibration code for Snapdragon Flight builds.

Signed-off-by: jwilson <jwilson@qti.qualcomm.com>

cmake/configs/posix_eagle_legacy_driver_default.cmake

@@ -1,6 +1,6 @@
 include(posix/px4_impl_posix)
 
-set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-linux-gnueabihf.cmake)
 
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexagon")
 
@@ -12,7 +12,7 @@ set(CONFIG_SHMEM "1")
 # or if it is for the Snapdragon.
 add_definitions(
 	-D__PX4_POSIX_EAGLE
-   -D__USING_SNAPDRAGON_LEGACY_DRIVER
+    -D__USING_SNAPDRAGON_LEGACY_DRIVER
 	)
 
 set(config_module_list
@@ -48,6 +48,8 @@ set(config_module_list
 	modules/logger
 	modules/simulator
 	modules/commander
+	modules/navigator
+	modules/load_mon
 
 	lib/controllib
 	lib/mathlib

cmake/configs/qurt_eagle_legacy_driver_default.cmake

@@ -16,6 +16,8 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/cmake_hexa
 
 add_definitions(
    -D__USING_SNAPDRAGON_LEGACY_DRIVER
+   -D__PX4_QURT
+   -D__PX4_QURT_EAGLE
    )
 
 set(config_module_list
@@ -26,8 +28,6 @@ set(config_module_list
 	modules/sensors
 	platforms/posix/drivers/df_mpu9250_wrapper
 	platforms/posix/drivers/df_bmp280_wrapper
-	platforms/posix/drivers/df_hmc5883_wrapper
-	platforms/posix/drivers/df_trone_wrapper
 
 	#
 	# System commands
@@ -37,7 +37,6 @@ set(config_module_list
 	#
 	# Estimation modules (EKF/ SO3 / other filters)
 	#
-	#modules/attitude_estimator_ekf
 	modules/ekf_att_pos_estimator
 	modules/attitude_estimator_q
 	modules/position_estimator_inav
@@ -72,7 +71,6 @@ set(config_module_list
 	#
 	platforms/qurt/fc_addon/rc_receiver
 	platforms/qurt/fc_addon/uart_esc
-	#platforms/qurt/fc_addon/mpu_spi
 
 	#
 	# Libraries
@@ -105,6 +103,4 @@ set(config_module_list
 set(config_df_driver_list
 	mpu9250
 	bmp280
-	hmc5883
-	trone
 	)

posix-configs/eagle/200qx/px4-calib.config

@@ -59,9 +59,9 @@ param set MC_ROLLRATE_D 0.0001
 param set ATT_W_MAG 0.00
 param set PE_MAG_NOISE 1.0f
 param set SENS_BOARD_ROT 0
-param set MPU_GYRO_LPF_ENM 4
+param set MPU_GYR_LPF_ENUM 4
-param set MPU_ACC_LPF_ENM 4
+param set MPU_ACC_LPF_ENUM 4
-param set MPU_SAMPLE_R_ENM 2
+param set MPU_SMPRATE_ENUM 2
 sleep 1
 mpu9x50 start -D /dev/spi-1
 uart_esc start -D /dev/tty-2

posix-configs/eagle/200qx/px4-flight-v1-board.config

@@ -51,15 +51,15 @@ param set RC6_REV 1
 param set ATT_W_MAG 0.00
 param set PE_MAG_NOISE 1.0f
 param set SENS_BOARD_ROT 6
-param set MPU_GYRO_LPF_ENM 4
+param set MPU_GYR_LPF_ENUM 4
-param set MPU_ACC_LPF_ENM 4
+param set MPU_ACC_LPF_ENUM 4
-param set MPU_SAMPLE_R_ENM 2
+param set MPU_SMPRATE_ENUM 2
 param set UART_ESC_MODEL 0
-param set UART_ESC_BAUD 250000
+param set UART_ESC_BAUDRAT 250000
-param set UART_ESC_MOTOR1 2
+param set UART_ESC_PX4MOT1 2
-param set UART_ESC_MOTOR2 4
+param set UART_ESC_PX4MOT2 4
-param set UART_ESC_MOTOR3 1
+param set UART_ESC_PX4MOT3 1
-param set UART_ESC_MOTOR4 3
+param set UART_ESC_PX4MOT4 3
 sleep 1
 commander start
 rc_receiver start -D /dev/tty-1

posix-configs/eagle/200qx/px4-flight-v2-board.config

@@ -51,15 +51,15 @@ param set RC6_REV 1
 param set ATT_W_MAG 0.00
 param set PE_MAG_NOISE 1.0f
 param set SENS_BOARD_ROT 0
-param set MPU_GYRO_LPF_ENM 4
+param set MPU_GYR_LPF_ENUM 4
-param set MPU_ACC_LPF_ENM 4
+param set MPU_ACC_LPF_ENUM 4
-param set MPU_SAMPLE_R_ENM 2
+param set MPU_SMPRATE_ENUM 2
 param set UART_ESC_MODEL 0
-param set UART_ESC_BAUD 250000
+param set UART_ESC_BAUDRAT 250000
-param set UART_ESC_MOTOR1 2
+param set UART_ESC_PX4MOT1 2
-param set UART_ESC_MOTOR2 4
+param set UART_ESC_PX4MOT2 4
-param set UART_ESC_MOTOR3 1
+param set UART_ESC_PX4MOT3 1
-param set UART_ESC_MOTOR4 3
+param set UART_ESC_PX4MOT4 3
 sleep 1
 commander start
 rc_receiver start -D /dev/tty-1

posix-configs/eagle/210qc/mainapp-calib.config

@@ -1,8 +0,0 @@
-uorb start
-muorb start
-mavlink start -u 14556
-sleep 1
-mavlink stream -u 14556 -s HIGHRES_IMU -r 50
-mavlink stream -u 14556 -s ATTITUDE -r 50
-mavlink boot_complete
-commander start

posix-configs/eagle/210qc/mainapp-flight.config

@@ -1,7 +0,0 @@
-uorb start
-muorb start
-mavlink start -u 14556
-sleep 1
-mavlink stream -u 14556 -s HIGHRES_IMU -r 50
-mavlink stream -u 14556 -s ATTITUDE -r 50
-mavlink boot_complete

posix-configs/eagle/210qc/px4-calib.config

@@ -1,72 +0,0 @@
-uorb start
-param set CAL_GYRO0_ID 2293760
-param set CAL_ACC0_ID 1310720
-param set CAL_ACC1_ID 1376256
-param set CAL_MAG0_ID 196608
-param set RC_RECEIVER_TYPE 1
-rc_receiver start -D /dev/tty-1
-attitude_estimator_q start
-position_estimator_inav start
-mc_pos_control start
-mc_att_control start
-sleep 1
-param set SYS_AUTOSTART 4001
-param set SYS_AUTOCONFIG 1
-param set MAV_TYPE 2​
-param set RC_MAP_THROTTLE 1
-param set RC_MAP_ROLL 2
-param set RC_MAP_PITCH 3
-param set RC_MAP_YAW 4
-param set RC_MAP_MODE_SW 5
-param set RC_MAP_POSCTL_SW 6
-param set RC1_MAX 1900
-param set RC1_MIN 1099
-param set RC1_TRIM 1099
-param set RC1_REV 1
-param set RC2_MAX 1900
-param set RC2_MIN 1099
-param set RC2_TRIM 1500
-param set RC2_REV -1
-param set RC3_MAX 1900
-param set RC3_MIN 1099
-param set RC3_TRIM 1500
-param set RC3_REV 1
-param set RC4_MAX 1900
-param set RC4_MIN 1099
-param set RC4_TRIM 1500
-param set RC4_REV -1
-param set RC5_MAX 1900
-param set RC5_MIN 1099
-param set RC5_TRIM 1500
-param set RC5_REV 1
-param set RC6_MAX 1900
-param set RC6_MIN 1099
-param set RC6_TRIM 1099
-param set RC6_REV 1
-sensors start
-param set MC_YAW_P 7.0
-param set MC_YAWRATE_P 0.08
-param set MC_YAWRATE_I 0.0
-param set MC_YAWRATE_D 0
-param set MC_PITCH_P 7.0
-param set MC_PITCHRATE_P 0.08
-param set MC_PITCHRATE_I 0.0
-param set MC_PITCHRATE_D 0.0001
-param set MC_ROLL_P 7.0
-param set MC_ROLLRATE_P 0.08
-param set MC_ROLLRATE_I 0.0
-param set MC_ROLLRATE_D 0.0001
-param set ATT_W_MAG 0.00
-param set PE_MAG_NOISE 1.0f
-param set SENS_BOARD_ROT 0
-param set MPU_GYRO_LPF_ENM 4
-param set MPU_ACC_LPF_ENM 4
-param set MPU_SAMPLE_R_ENM 2
-sleep 1
-mpu9x50 start -D /dev/spi-1
-uart_esc start -D /dev/tty-2
-csr_gps start -D /dev/tty-3
-list_devices
-list_files
-list_tasks
-list_topics

posix-configs/eagle/210qc/px4-flight.config

@@ -1,97 +0,0 @@
-uorb start
-param set CAL_GYRO0_ID 2293760
-param set CAL_ACC0_ID 1310720
-param set CAL_ACC1_ID 1376256
-param set CAL_MAG0_ID 196608
-param set MAV_TYPE 2
-param set MC_YAW_P 12
-param set MC_YAWRATE_P 0.08
-param set MC_YAWRATE_I 0.0
-param set MC_YAWRATE_D 0
-param set MC_PITCH_P 7.0
-param set MC_PITCHRATE_P 0.08
-param set MC_PITCHRATE_I 0.0
-param set MC_PITCHRATE_D 0.001
-param set MC_ROLL_P 7.0
-param set MC_ROLLRATE_P 0.08
-param set MC_ROLLRATE_I 0.0
-param set MC_ROLLRATE_D 0.001
-param set RC_MAP_THROTTLE 1
-param set RC_MAP_ROLL 2
-param set RC_MAP_PITCH 3
-param set RC_MAP_YAW 4
-param set RC_MAP_MODE_SW 5
-param set RC_MAP_POSCTL_SW 6
-param set RC1_MAX 1900
-param set RC1_MIN 1099
-param set RC1_TRIM 1099
-param set RC1_REV 1
-param set RC2_MAX 1900
-param set RC2_MIN 1099
-param set RC2_TRIM 1500
-param set RC2_REV -1
-param set RC3_MAX 1900
-param set RC3_MIN 1099
-param set RC3_TRIM 1500
-param set RC3_REV 1
-param set RC4_MAX 1900
-param set RC4_MIN 1099
-param set RC4_TRIM 1500
-param set RC4_REV -1
-param set RC5_MAX 1900
-param set RC5_MIN 1099
-param set RC5_TRIM 1500
-param set RC5_REV 1
-param set RC6_MAX 1900
-param set RC6_MIN 1099
-param set RC6_TRIM 1099
-param set RC6_REV 1
-param set ATT_W_MAG 0.00
-param set PE_MAG_NOISE 1.0f
-param set SENS_BOARD_ROT 0
-param set MPU_GYRO_LPF_ENM 4
-param set MPU_ACC_LPF_ENM 4
-param set MPU_SAMPLE_R_ENM 2
-param set GYRO0_XOFF -0.0028
-param set GYRO0_YOFF -0.0047
-param set GYRO0_ZOFF -0.0034
-param set GYRO0_XSCALE 1.0000
-param set GYRO0_YSCALE 1.0000
-param set GYRO0_ZSCALE 1.0000
-param set MAG0_XOFF 0.0000
-param set MAG0_YOFF 0.0000
-param set MAG0_ZOFF 0.0000
-param set MAG0_XSCALE 1.0000
-param set MAG0_YSCALE 1.0000
-param set MAG0_ZSCALE 1.0000
-param set ACC0_XOFF -0.0941
-param set ACC0_YOFF 0.1168
-param set ACC0_ZOFF -0.0398
-param set ACC0_XSCALE 1.0004
-param set ACC0_YSCALE 0.9974
-param set ACC0_ZSCALE 0.9951
-param set RC_RECEIVER_TYPE 1
-param set UART_ESC_MODEL 2
-param set UART_ESC_BAUD 250000
-param set UART_ESC_MOTOR1 4
-param set UART_ESC_MOTOR2 2
-param set UART_ESC_MOTOR3 1
-param set UART_ESC_MOTOR4 3
-sleep 1
-commander start
-rc_receiver start -D /dev/tty-1
-sleep 1
-mpu9x50 start -D /dev/spi-1
-df_bmp280_wrapper start
-sensors start
-attitude_estimator_q start
-position_estimator_inav start
-mc_pos_control start
-mc_att_control start
-land_detector start multicopter
-sleep 1
-uart_esc start -D /dev/tty-2
-list_devices
-list_files
-list_tasks
-list_topics

src/modules/commander/accelerometer_calibration.cpp

@@ -175,7 +175,7 @@ typedef struct  {
 
 int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 {
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
 	int fd;
 #endif
 
@@ -195,7 +195,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 
 	/* reset all sensors */
 	for (unsigned s = 0; s < max_accel_sens; s++) {
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
 		/* reset all offsets to zero and all scales to one */
 		fd = px4_open(str, 0);
@@ -329,7 +329,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 			return ERROR;
 		}
 
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
 		fd = px4_open(str, 0);
 
@@ -419,7 +419,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
 			break;
 		}
 
-#ifdef __PX4_QURT
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE)
 		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
 		struct accel_report	accel_report;
 		orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report);

src/modules/commander/gyro_calibration.cpp

@@ -186,7 +186,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 
 		// Reset all offsets to 0 and scales to 1
 		(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero));
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
 		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 		int fd = px4_open(str, 0);
 		if (fd >= 0) {
@@ -239,9 +239,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 	for (unsigned s = 0; s < gyro_count; s++) {
 
 		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
-#ifdef __PX4_QURT
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE)
 		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
-		struct gyro_report	gyro_report;
+		struct gyro_report gyro_report;
 		orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &gyro_report);
 		worker_data.device_id[s] = gyro_report.device_id;
 #endif
@@ -337,7 +337,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 				(void)sprintf(str, "CAL_GYRO%u_ID", s);
 				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
 
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
 				/* apply new scaling and offsets */
 				(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 				int fd = px4_open(str, 0);

src/modules/sensors/sensors.cpp

@@ -46,9 +46,6 @@
  * @author Anton Babushkin <anton@px4.io>
  */
 
-// TODO-JYW: TESTING-TESTING
-#define DEBUG_BUILD 1
-
 #include <board_config.h>
 
 #include <px4_config.h>

src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp

@@ -58,6 +58,7 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
 #include <drivers/device/integrator.h>
 
 #include <uORB/topics/parameter_update.h>
@@ -77,7 +78,7 @@ using namespace DriverFramework;
 class DfMpu9250Wrapper : public MPU9250
 {
 public:
-	DfMpu9250Wrapper(/*enum Rotation rotation*/);
+	DfMpu9250Wrapper(bool mag_enabled);
 	~DfMpu9250Wrapper();
 
 
@@ -105,11 +106,13 @@ private:
 
 	void _update_accel_calibration();
 	void _update_gyro_calibration();
+	void _update_mag_calibration();
 
 	//enum Rotation		_rotation;
 
 	orb_advert_t		    _accel_topic;
 	orb_advert_t		    _gyro_topic;
+	orb_advert_t		    _mag_topic;
 
 	orb_advert_t		    _mavlink_log_pub;
 
@@ -133,11 +136,22 @@ private:
 		float z_scale;
 	} _gyro_calibration;
 
+	struct mag_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _mag_calibration;
+
 	int			    _accel_orb_class_instance;
 	int			    _gyro_orb_class_instance;
+	int			    _mag_orb_class_instance;
 
 	Integrator		    _accel_int;
 	Integrator		    _gyro_int;
+	Integrator		    _mag_int;
 
 	unsigned		    _publish_count;
 
@@ -147,24 +161,31 @@ private:
 	perf_counter_t		    _fifo_corruption_counter;
 	perf_counter_t		    _gyro_range_hit_counter;
 	perf_counter_t		    _accel_range_hit_counter;
+	perf_counter_t		    _mag_range_hit_counter;
 	perf_counter_t		    _publish_perf;
 
 	hrt_abstime		    _last_accel_range_hit_time;
 	uint64_t		    _last_accel_range_hit_count;
+
+	bool _mag_enabled;
 };
 
-DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
+DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled) :
-	MPU9250(IMU_DEVICE_PATH),
+	MPU9250(IMU_DEVICE_PATH, mag_enabled),
 	_accel_topic(nullptr),
 	_gyro_topic(nullptr),
+	_mag_topic(nullptr),
 	_mavlink_log_pub(nullptr),
 	_param_update_sub(-1),
 	_accel_calibration{},
 	_gyro_calibration{},
+	_mag_calibration{},
 	_accel_orb_class_instance(-1),
 	_gyro_orb_class_instance(-1),
+	_mag_orb_class_instance(-1),
 	_accel_int(MPU9250_NEVER_AUTOPUBLISH_US, false),
 	_gyro_int(MPU9250_NEVER_AUTOPUBLISH_US, true),
+	_mag_int(MPU9250_NEVER_AUTOPUBLISH_US, true),
 	/*_rotation(rotation)*/
 	_publish_count(0),
 	_read_counter(perf_alloc(PC_COUNT, "mpu9250_reads")),
@@ -173,9 +194,11 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
 	_fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_corruptions")),
 	_gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_gyro_range_hits")),
 	_accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_accel_range_hits")),
+	_mag_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_mag_range_hits")),
 	_publish_perf(perf_alloc(PC_ELAPSED, "mpu9250_publish")),
 	_last_accel_range_hit_time(0),
-	_last_accel_range_hit_count(0)
+	_last_accel_range_hit_count(0),
+	_mag_enabled(mag_enabled)
 {
 	// Set sane default calibration values
 	_accel_calibration.x_scale = 1.0f;
@@ -191,6 +214,15 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
 	_gyro_calibration.x_offset = 0.0f;
 	_gyro_calibration.y_offset = 0.0f;
 	_gyro_calibration.z_offset = 0.0f;
+
+	if (_mag_enabled == true) {
+		_mag_calibration.x_scale = 1.0f;
+		_mag_calibration.y_scale = 1.0f;
+		_mag_calibration.z_scale = 1.0f;
+		_mag_calibration.x_offset = 0.0f;
+		_mag_calibration.y_offset = 0.0f;
+		_mag_calibration.z_offset = 0.0f;
+	}
 }
 
 DfMpu9250Wrapper::~DfMpu9250Wrapper()
@@ -201,6 +233,9 @@ DfMpu9250Wrapper::~DfMpu9250Wrapper()
 	perf_free(_fifo_corruption_counter);
 	perf_free(_gyro_range_hit_counter);
 	perf_free(_accel_range_hit_counter);
+	if (_mag_enabled == true) {
+		perf_free(_mag_range_hit_counter);
+	}
 	perf_free(_publish_perf);
 }
 
@@ -226,6 +261,18 @@ int DfMpu9250Wrapper::start()
 		return -1;
 	}
 
+	if (_mag_enabled == true) {
+		// TODO: don't publish garbage here
+		mag_report mag_report = {};
+		_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
+						  &_mag_orb_class_instance, ORB_PRIO_DEFAULT);
+
+		if (_mag_topic == nullptr) {
+			PX4_ERR("sensor_mag advert fail");
+			return -1;
+		}
+	}
+
 	/* Subscribe to param update topic. */
 	if (_param_update_sub < 0) {
 		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -251,6 +298,7 @@ int DfMpu9250Wrapper::start()
 	/* Force getting the calibration values. */
 	_update_accel_calibration();
 	_update_gyro_calibration();
+	_update_mag_calibration();
 
 	return 0;
 }
@@ -276,6 +324,9 @@ void DfMpu9250Wrapper::info()
 	perf_print_counter(_fifo_corruption_counter);
 	perf_print_counter(_gyro_range_hit_counter);
 	perf_print_counter(_accel_range_hit_counter);
+	if (_mag_enabled == true) {
+		perf_print_counter(_mag_range_hit_counter);
+	}
 	perf_print_counter(_publish_perf);
 }
 
@@ -430,6 +481,83 @@ void DfMpu9250Wrapper::_update_accel_calibration()
 	_accel_calibration.z_offset = 0.0f;
 }
 
+void DfMpu9250Wrapper::_update_mag_calibration()
+{
+	if (_mag_enabled == false) {
+		return;
+	}
+
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_MAG%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+	}
+
+	// Set sane default calibration values
+	_mag_calibration.x_scale = 1.0f;
+	_mag_calibration.y_scale = 1.0f;
+	_mag_calibration.z_scale = 1.0f;
+	_mag_calibration.x_offset = 0.0f;
+	_mag_calibration.y_offset = 0.0f;
+	_mag_calibration.z_offset = 0.0f;
+}
+
 int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 {
 	/* Check if calibration values are still up-to-date. */
@@ -442,6 +570,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 
 		_update_accel_calibration();
 		_update_gyro_calibration();
+		_update_mag_calibration();
 	}
 
 	math::Vector<3> vec_integrated_unused;
@@ -491,13 +620,20 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
 	perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
 	perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
+	if (_mag_enabled == true) {
+		perf_set_count(_mag_range_hit_counter, data.mag_range_hit_counter);
+	}
 
 	perf_begin(_publish_perf);
 
 	accel_report accel_report = {};
 	gyro_report gyro_report = {};
+	mag_report mag_report = {};
 
 	accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
+	if (_mag_enabled == true) {
+		mag_report.timestamp = accel_report.timestamp;
+	}
 
 	// TODO: get these right
 	gyro_report.scaling = -1.0f;
@@ -508,6 +644,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.range_m_s2 = -1.0f;
 	accel_report.device_id = m_id.dev_id;
 
+	if (_mag_enabled == true) {
+		mag_report.scaling = -1.0f;
+		mag_report.range_ga = -1.0f;
+		mag_report.device_id = m_id.dev_id;
+	}
+
 	// TODO: remove these (or get the values)
 	gyro_report.x_raw = NAN;
 	gyro_report.y_raw = NAN;
@@ -517,6 +659,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.y_raw = NAN;
 	accel_report.z_raw = NAN;
 
+	if (_mag_enabled == true) {
+		mag_report.x_raw = NAN;
+		mag_report.y_raw = NAN;
+		mag_report.z_raw = NAN;
+	}
+
 	math::Vector<3> gyro_val_filt;
 	math::Vector<3> accel_val_filt;
 
@@ -533,6 +681,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.y = accel_val_filt(1);
 	accel_report.z = accel_val_filt(2);
 
+	if (_mag_enabled == true) {
+		mag_report.x = (data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale;
+		mag_report.y = (data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale;
+		mag_report.z = (data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale;
+	}
+
 	gyro_report.x_integral = gyro_val_integ(0);
 	gyro_report.y_integral = gyro_val_integ(1);
 	gyro_report.z_integral = gyro_val_integ(2);
@@ -552,6 +706,10 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
 		}
 
+		if ((_mag_topic != nullptr) && (_mag_enabled == true)) {
+			orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+		}
+
 		/* Notify anyone waiting for data. */
 		DevMgr::updateNotify(*this);
 
@@ -588,9 +746,9 @@ int stop();
 int info();
 void usage();
 
-int start(/*enum Rotation rotation*/)
+int start(bool mag_enabled)
 {
-	g_dev = new DfMpu9250Wrapper(/*rotation*/);
+	g_dev = new DfMpu9250Wrapper(mag_enabled);
 
 	if (g_dev == nullptr) {
 		PX4_ERR("failed instantiating DfMpu9250Wrapper object");
@@ -695,9 +853,12 @@ df_mpu9250_wrapper_main(int argc, char *argv[])
 
 	const char *verb = argv[myoptind];
 
+	if (!strcmp(verb, "start_with_mag")) {
+		ret = df_mpu9250_wrapper::start(true);
+	}
 
-	if (!strcmp(verb, "start")) {
+	else if (!strcmp(verb, "start")) {
-		ret = df_mpu9250_wrapper::start(/*rotation*/);
+		ret = df_mpu9250_wrapper::start(false);
 	}
 
 	else if (!strcmp(verb, "stop")) {