From d5a7aaa09df7dea7920eba55a51841773c4b0f5f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 31 May 2016 18:04:42 +0100
Subject: [PATCH] df_hmc5883_wrapper: rotation on driver level

---
 .../df_hmc5883_wrapper/df_hmc5883_wrapper.cpp | 54 +++++++++++--------
 1 file changed, 33 insertions(+), 21 deletions(-)

diff --git a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
index 22e410ea8d..3fc6a33850 100644
--- a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
@@ -59,8 +59,8 @@
 #include <uORB/topics/parameter_update.h>
 
 #include <board_config.h>
-//#include <mathlib/math/filter/LowPassFilter2p.hpp>
-//#include <lib/conversion/rotation.h>
+
+#include <lib/conversion/rotation.h>
 
 #include <hmc5883/HMC5883.hpp>
 #include <DevMgr.hpp>
@@ -74,7 +74,7 @@ using namespace DriverFramework;
 class DfHmc9250Wrapper : public HMC5883
 {
 public:
-	DfHmc9250Wrapper(/*enum Rotation rotation*/);
+	DfHmc9250Wrapper(enum Rotation rotation);
 	~DfHmc9250Wrapper();
 
 
@@ -97,8 +97,6 @@ private:
 
 	void _update_mag_calibration();
 
-	//enum Rotation		_rotation;
-
 	orb_advert_t		_mag_topic;
 
 	int			_param_update_sub;
@@ -112,20 +110,21 @@ private:
 		float z_scale;
 	} _mag_calibration;
 
+	math::Matrix<3, 3>      _rotation_matrix;
+
 	int			_mag_orb_class_instance;
 
 	perf_counter_t		_mag_sample_perf;
 
 };
 
-DfHmc9250Wrapper::DfHmc9250Wrapper(/*enum Rotation rotation*/) :
+DfHmc9250Wrapper::DfHmc9250Wrapper(enum Rotation rotation) :
 	HMC5883(MAG_DEVICE_PATH),
 	_mag_topic(nullptr),
 	_param_update_sub(-1),
 	_mag_calibration{},
 	_mag_orb_class_instance(-1),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read"))
-	/*_rotation(rotation)*/
 {
 	// Set sane default calibration values
 	_mag_calibration.x_scale = 1.0f;
@@ -134,6 +133,9 @@ DfHmc9250Wrapper::DfHmc9250Wrapper(/*enum Rotation rotation*/) :
 	_mag_calibration.x_offset = 0.0f;
 	_mag_calibration.y_offset = 0.0f;
 	_mag_calibration.z_offset = 0.0f;
+
+	// Get sensor rotation matrix
+	get_rot_matrix(rotation, &_rotation_matrix);
 }
 
 DfHmc9250Wrapper::~DfHmc9250Wrapper()
@@ -278,9 +280,19 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
 	mag_report.x_raw = NAN;
 	mag_report.y_raw = NAN;
 	mag_report.z_raw = NAN;
-	mag_report.x = (data.field_x_ga - _mag_calibration.x_offset) * _mag_calibration.x_scale;
-	mag_report.y = (data.field_y_ga - _mag_calibration.y_offset) * _mag_calibration.y_scale;
-	mag_report.z = (data.field_z_ga - _mag_calibration.z_offset) * _mag_calibration.z_scale;
+
+
+	math::Vector<3> mag_val((data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale,
+				(data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale,
+				(data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale);
+
+	// apply sensor rotation on the accel measurement
+	mag_val = _rotation_matrix * mag_val;
+
+	mag_report.x = mag_val(0);
+	mag_report.y = mag_val(1);
+	mag_report.z = mag_val(2);
+
 
 	// TODO: get these right
 	//mag_report.scaling = -1.0f;
@@ -315,14 +327,14 @@ namespace df_hmc5883_wrapper
 
 DfHmc9250Wrapper *g_dev = nullptr;
 
-int start(/* enum Rotation rotation */);
+int start(enum Rotation rotation);
 int stop();
 int info();
 void usage();
 
-int start(/*enum Rotation rotation*/)
+int start(enum Rotation rotation)
 {
-	g_dev = new DfHmc9250Wrapper(/*rotation*/);
+	g_dev = new DfHmc9250Wrapper(rotation);
 
 	if (g_dev == nullptr) {
 		PX4_ERR("failed instantiating DfHmc9250Wrapper object");
@@ -389,9 +401,9 @@ info()
 void
 usage()
 {
-	PX4_WARN("Usage: df_hmc5883_wrapper 'start', 'info', 'stop'");
-	PX4_WARN("options:");
-	//PX4_WARN("    -R rotation");
+	PX4_INFO("Usage: df_hmc5883_wrapper 'start', 'info', 'stop'");
+	PX4_INFO("options:");
+	PX4_INFO("    -R rotation");
 }
 
 } // namespace df_hmc5883_wrapper
@@ -401,7 +413,7 @@ int
 df_hmc5883_wrapper_main(int argc, char *argv[])
 {
 	int ch;
-	// enum Rotation rotation = ROTATION_NONE;
+	enum Rotation rotation = ROTATION_NONE;
 	int ret = 0;
 	int myoptind = 1;
 	const char *myoptarg = NULL;
@@ -409,9 +421,9 @@ df_hmc5883_wrapper_main(int argc, char *argv[])
 	/* jump over start/off/etc and look at options first */
 	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
-		//case 'R':
-		//	rotation = (enum Rotation)atoi(myoptarg);
-		//	break;
+		case 'R':
+			rotation = (enum Rotation)atoi(myoptarg);
+			break;
 
 		default:
 			df_hmc5883_wrapper::usage();
@@ -428,7 +440,7 @@ df_hmc5883_wrapper_main(int argc, char *argv[])
 
 
 	if (!strcmp(verb, "start")) {
-		ret = df_hmc5883_wrapper::start(/*rotation*/);
+		ret = df_hmc5883_wrapper::start(rotation);
 	}
 
 	else if (!strcmp(verb, "stop")) {