diff --git a/msg/sensor_mag.msg b/msg/sensor_mag.msg
index 7e02b35ba0..63e1385e70 100644
--- a/msg/sensor_mag.msg
+++ b/msg/sensor_mag.msg
@@ -1,17 +1,14 @@
-uint64 timestamp	# time since system start (microseconds)
-uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+uint64 timestamp          # time since system start (microseconds)
+uint64 timestamp_sample
 
-uint64 error_count
+uint32 device_id          # unique device ID for the sensor that does not change between power cycles
 
-float32 x		# magnetic field in the NED X board axis in Gauss
-float32 y		# magnetic field in the NED Y board axis in Gauss
-float32 z		# magnetic field in the NED Z board axis in Gauss
+float32 x                 # magnetic field in the NED X board axis in Gauss
+float32 y                 # magnetic field in the NED Y board axis in Gauss
+float32 z                 # magnetic field in the NED Z board axis in Gauss
 
-float32 temperature	# temperature in degrees celsius
+float32 temperature       # temperature in degrees celsius
 
-float32 scaling		# scaling from raw to Gauss
-int16 x_raw
-int16 y_raw
-int16 z_raw
+uint32 error_count
 
 bool  is_external	# if true the mag is external (i.e. not built into the board)
diff --git a/src/drivers/imu/mpu9250/MPU9250_mag.cpp b/src/drivers/imu/mpu9250/MPU9250_mag.cpp
index f67240c22b..3c912efad5 100644
--- a/src/drivers/imu/mpu9250/MPU9250_mag.cpp
+++ b/src/drivers/imu/mpu9250/MPU9250_mag.cpp
@@ -148,7 +148,7 @@ bool MPU9250_mag::_measure(const hrt_abstime &timestamp_sample, const ak8963_reg
 	int16_t x = combine(data.HXH, data.HXL);
 	int16_t y = -combine(data.HYH, data.HYL);
 	int16_t z = -combine(data.HZH, data.HZL);
-	_px4_mag.update(timestamp_sample, x, y, z);
+	_px4_mag.update(timestamp_sample, x * _ak8963_ASA[0], y * _ak8963_ASA[1], z * _ak8963_ASA[2]);
 
 	return true;
 }
@@ -244,19 +244,17 @@ MPU9250_mag::ak8963_read_adjustments()
 
 	write_reg_through_mpu9250(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE);
 
-	float ak8963_ASA[3] {};
+
 
 	for (int i = 0; i < 3; i++) {
 		if (0 != response[i] && 0xff != response[i]) {
-			ak8963_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f;
+			_ak8963_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f;
 
 		} else {
 			return false;
 		}
 	}
 
-	_px4_mag.set_sensitivity(ak8963_ASA[0], ak8963_ASA[1], ak8963_ASA[2]);
-
 	return true;
 }
 
diff --git a/src/drivers/imu/mpu9250/MPU9250_mag.h b/src/drivers/imu/mpu9250/MPU9250_mag.h
index 5a504d4868..916a6fd82d 100644
--- a/src/drivers/imu/mpu9250/MPU9250_mag.h
+++ b/src/drivers/imu/mpu9250/MPU9250_mag.h
@@ -147,6 +147,8 @@ private:
 
 	MPU9250 *_parent;
 
+	float _ak8963_ASA[3] {1.f, 1.f, 1.f};
+
 	bool _mag_reading_data{false};
 
 	perf_counter_t _mag_overruns;
diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp
index 11196b7384..742b1e4265 100644
--- a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp
+++ b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp
@@ -39,15 +39,12 @@
 PX4Magnetometer::PX4Magnetometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
 	CDev(nullptr),
 	_sensor_mag_pub{ORB_ID(sensor_mag), priority},
-	_rotation{rotation}
+	_rotation{rotation},
+	_device_id{device_id}
 {
 	_class_device_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
 
 	_sensor_mag_pub.advertise();
-
-	_sensor_mag_pub.get().device_id = device_id;
-	_sensor_mag_pub.get().scaling = 1.0f;
-	_sensor_mag_pub.get().temperature = NAN;
 }
 
 PX4Magnetometer::~PX4Magnetometer()
@@ -88,10 +85,10 @@ int PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 		return 0;
 
 	case MAGIOCGEXTERNAL:
-		return _sensor_mag_pub.get().is_external;
+		return _external;
 
 	case DEVIOCGDEVICEID:
-		return _sensor_mag_pub.get().device_id;
+		return _device_id;
 
 	default:
 		return -ENOTTY;
@@ -102,19 +99,22 @@ void PX4Magnetometer::set_device_type(uint8_t devtype)
 {
 	// current DeviceStructure
 	union device::Device::DeviceId device_id;
-	device_id.devid = _sensor_mag_pub.get().device_id;
+	device_id.devid = _device_id;
 
 	// update to new device type
 	device_id.devid_s.devtype = devtype;
 
 	// copy back to report
-	_sensor_mag_pub.get().device_id = device_id.devid;
+	_device_id = device_id.devid;
 }
 
 void PX4Magnetometer::update(hrt_abstime timestamp_sample, float x, float y, float z)
 {
-	sensor_mag_s &report = _sensor_mag_pub.get();
-	report.timestamp = timestamp_sample;
+	sensor_mag_s report;
+	report.timestamp_sample = timestamp_sample;
+	report.device_id = _device_id;
+	report.temperature = _temperature;
+	report.error_count = _error_count;
 
 	// Apply rotation (before scaling)
 	rotate_3f(_rotation, x, y, z);
@@ -122,18 +122,16 @@ void PX4Magnetometer::update(hrt_abstime timestamp_sample, float x, float y, flo
 	const matrix::Vector3f raw_f{x, y, z};
 
 	// Apply range scale and the calibrating offset/scale
-	const matrix::Vector3f val_calibrated{(((raw_f.emult(_sensitivity) * report.scaling) - _calibration_offset).emult(_calibration_scale))};
-
-	// Raw values (ADC units 0 - 65535)
-	report.x_raw = x;
-	report.y_raw = y;
-	report.z_raw = z;
+	const matrix::Vector3f val_calibrated{(((raw_f * _scale) - _calibration_offset).emult(_calibration_scale))};
 
 	report.x = val_calibrated(0);
 	report.y = val_calibrated(1);
 	report.z = val_calibrated(2);
 
-	_sensor_mag_pub.update();
+	report.is_external = _external;
+
+	report.timestamp = hrt_absolute_time();
+	_sensor_mag_pub.publish(report);
 }
 
 void PX4Magnetometer::print_status()
diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp
index 563e972ca2..0a7519c695 100644
--- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp
+++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp
@@ -43,22 +43,20 @@
 
 class PX4Magnetometer : public cdev::CDev
 {
-
 public:
 	PX4Magnetometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
 	~PX4Magnetometer() override;
 
-	int	ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
+	int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
 
-	bool external() { return _sensor_mag_pub.get().is_external; }
+	bool external() { return _external; }
 
 	void set_device_type(uint8_t devtype);
-	void set_error_count(uint64_t error_count) { _sensor_mag_pub.get().error_count = error_count; }
-	void increase_error_count() { _sensor_mag_pub.get().error_count++; }
-	void set_scale(float scale) { _sensor_mag_pub.get().scaling = scale; }
-	void set_temperature(float temperature) { _sensor_mag_pub.get().temperature = temperature; }
-	void set_external(bool external) { _sensor_mag_pub.get().is_external = external; }
-	void set_sensitivity(float x, float y, float z) { _sensitivity = matrix::Vector3f{x, y, z}; }
+	void set_error_count(uint32_t error_count) { _error_count = error_count; }
+	void increase_error_count() { _error_count++; }
+	void set_scale(float scale) { _scale = scale; }
+	void set_temperature(float temperature) { _temperature = temperature; }
+	void set_external(bool external) { _external = external; }
 
 	void update(hrt_abstime timestamp_sample, float x, float y, float z);
 
@@ -67,16 +65,19 @@ public:
 	void print_status();
 
 private:
+	uORB::PublicationMulti<sensor_mag_s> _sensor_mag_pub;
 
-	uORB::PublicationMultiData<sensor_mag_s>	_sensor_mag_pub;
+	matrix::Vector3f _calibration_scale{1.f, 1.f, 1.f};
+	matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f};
 
-	const enum Rotation	_rotation;
+	const enum Rotation _rotation;
+	uint32_t _device_id{0};
+	float _temperature{NAN};
+	float _scale{1.f};
 
-	matrix::Vector3f	_calibration_scale{1.0f, 1.0f, 1.0f};
-	matrix::Vector3f	_calibration_offset{0.0f, 0.0f, 0.0f};
+	uint32_t _error_count{0};
 
-	matrix::Vector3f	_sensitivity{1.0f, 1.0f, 1.0f};
-
-	int			_class_device_instance{-1};
+	bool _external{false};
 
+	int _class_device_instance{-1};
 };