From ae8fdf4c04e9a345c8d71a7faa0b85be4207a016 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 9 Nov 2021 11:33:10 -0500
Subject: [PATCH] drivers/magnetometer/hmc5883: fix Z direction

---
 .../airmind/mindpx-v2/init/rc.board_sensors   |  2 +-
 boards/px4/fmu-v2/init/rc.board_sensors       |  5 +--
 boards/px4/fmu-v3/init/rc.board_sensors       |  5 +--
 boards/px4/fmu-v4/init/rc.board_sensors       |  6 ++--
 src/drivers/magnetometer/hmc5883/HMC5883.cpp  | 31 +++++--------------
 5 files changed, 13 insertions(+), 36 deletions(-)

diff --git a/boards/airmind/mindpx-v2/init/rc.board_sensors b/boards/airmind/mindpx-v2/init/rc.board_sensors
index 30fca7a7ed..2867d34b56 100644
--- a/boards/airmind/mindpx-v2/init/rc.board_sensors
+++ b/boards/airmind/mindpx-v2/init/rc.board_sensors
@@ -6,7 +6,7 @@
 board_adc start
 
 # Internal I2C bus
-hmc5883 -T -I -R 12 start
+hmc5883 -T -I -R 2 start
 qmc5883l -I -R 6 start
 
 mpu6000 -s -R 2 start
diff --git a/boards/px4/fmu-v2/init/rc.board_sensors b/boards/px4/fmu-v2/init/rc.board_sensors
index 8aecc96c34..ce1a7e9fd5 100644
--- a/boards/px4/fmu-v2/init/rc.board_sensors
+++ b/boards/px4/fmu-v2/init/rc.board_sensors
@@ -8,9 +8,6 @@ rgbled_ncp5623c start -I
 
 board_adc start
 
-# Internal I2C bus
-hmc5883 -T -I -R 4 start
-
 # Internal SPI (auto detect ms5611 or ms5607)
 if ! ms5611 -T 5607 -s -b 1 start
 then
@@ -80,7 +77,7 @@ then
 		mpu6000 -s -b 1 start
 
 		# v2.0 Has internal hmc5883 on SPI1
-		hmc5883 -T -s -b 1 -R 8 start
+		hmc5883 -T -s -b 1 start
 	fi
 
 	if [ $BOARD_FMUV3 = 21 ]
diff --git a/boards/px4/fmu-v3/init/rc.board_sensors b/boards/px4/fmu-v3/init/rc.board_sensors
index 33e5698d76..6c0f1d0358 100644
--- a/boards/px4/fmu-v3/init/rc.board_sensors
+++ b/boards/px4/fmu-v3/init/rc.board_sensors
@@ -8,9 +8,6 @@ rgbled_ncp5623c start -I
 
 board_adc start
 
-# Internal I2C bus
-hmc5883 -T -I -R 4 start
-
 # Internal SPI (auto detect ms5611 or ms5607)
 if ! ms5611 -T 5607 -s -b 1 start
 then
@@ -80,7 +77,7 @@ then
 		mpu6000 -s -b 1 start
 
 		# v2.0 Has internal hmc5883 on SPI1
-		hmc5883 -T -s -b 1 -R 8 start
+		hmc5883 -T -s -b 1 start
 	fi
 
 	if [ $BOARD_FMUV3 = 21 ]
diff --git a/boards/px4/fmu-v4/init/rc.board_sensors b/boards/px4/fmu-v4/init/rc.board_sensors
index b6f2662f17..6cb78339e9 100644
--- a/boards/px4/fmu-v4/init/rc.board_sensors
+++ b/boards/px4/fmu-v4/init/rc.board_sensors
@@ -16,10 +16,10 @@ pwm_out sensor_reset 50
 # Internal SPI
 ms5611 -s start
 
-# hmc5883 internal SPI bus is rotated 90 deg yaw
-if ! hmc5883 -T -s -R 2 start
+# hmc5883 internal SPI bus
+if ! hmc5883 -T -s -R 12 start
 then
-	# lis3mdl internal SPI bus is rotated 90 deg yaw
+	# lis3mdl internal SPI bus
 	lis3mdl -s start
 fi
 
diff --git a/src/drivers/magnetometer/hmc5883/HMC5883.cpp b/src/drivers/magnetometer/hmc5883/HMC5883.cpp
index fdf0a9353e..d97057796b 100644
--- a/src/drivers/magnetometer/hmc5883/HMC5883.cpp
+++ b/src/drivers/magnetometer/hmc5883/HMC5883.cpp
@@ -281,10 +281,6 @@ int HMC5883::collect()
 
 	uint8_t check_counter;
 
-	float xraw_f;
-	float yraw_f;
-	float zraw_f;
-
 	_px4_mag.set_error_count(perf_event_count(_comms_errors));
 
 	perf_begin(_sample_perf);
@@ -361,28 +357,15 @@ int HMC5883::collect()
 		}
 	}
 
-	/*
-	 * RAW outputs
-	 *
-	 * to align the sensor axes with the board, x and y need to be flipped
-	 * and y needs to be negated
-	 */
-	if (!_px4_mag.external()) {
-		// convert onboard so it matches offboard for the
-		// scaling below
-		report.y = -report.y;
-		report.x = -report.x;
+	if (_px4_mag.external()) {
+		// legacy handling of the popular 3DR external compass
+		//  x pointing to the right, y pointing backwards, and z down
+		_px4_mag.update(timestamp_sample, math::negate(report.y), math::negate(report.x), math::negate(report.z));
+
+	} else {
+		_px4_mag.update(timestamp_sample, math::negate(report.x), report.y, math::negate(report.z));
 	}
 
-	/* the standard external mag by 3DR has x pointing to the
-	 * right, y pointing backwards, and z down, therefore switch x
-	 * and y and invert y */
-	xraw_f = -report.y;
-	yraw_f = report.x;
-	zraw_f = report.z;
-
-	_px4_mag.update(timestamp_sample, xraw_f, yraw_f, zraw_f);
-
 	/*
 	  periodically check the range register and configuration
 	  registers. With a bad I2C cable it is possible for the