Fixed axis assignment and raw value outputs. Scaling and offsets to be done

3 files changed, 45 insertions, 27 deletions

diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index d1025bcc6..cd198b6e6 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -658,9 +658,18 @@ HMC5883::collect()
 	    (abs(report.z) > 2048))
 		goto out;
 
+	/* raw outputs */
+	/* to align the sensor axes with the board, x and y need to be flipped */
+	_reports[_next_report].x_raw = report.y;
+	_reports[_next_report].y_raw = report.x;
+	/* z remains z */
+	_reports[_next_report].z_raw = report.z;
+
 	/* scale values for output */
-	_reports[_next_report].x = report.x * _scale.x_scale + _scale.x_offset;
-	_reports[_next_report].y = report.y * _scale.y_scale + _scale.y_offset;
+	/* to align the sensor axes with the board, x and y need to be flipped */
+	_reports[_next_report].x = report.y * _scale.x_scale + _scale.x_offset;
+	_reports[_next_report].y = report.x * _scale.y_scale + _scale.y_offset;
+	/* z remains z */
 	_reports[_next_report].z = report.z * _scale.z_scale + _scale.z_offset;
 
 	/* publish it */
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index 3afb0ddda..77b5aaa98 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -790,22 +790,29 @@ MPU6000::measure()
 	 */
 	_gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time();
 
-	_accel_report.x_raw = report.accel_x;
+	/* to align the sensor axes with the board, x and y need to be flipped */
+	_accel_report.x_raw = report.accel_y;
 	_accel_report.y_raw = report.accel_x;
-	_accel_report.z_raw = report.accel_x;
+	/* z remains z and has the right sign */
+	_accel_report.z_raw = report.accel_z;
 
-	_accel_report.x = report.accel_x * _accel_range_scale;
-	_accel_report.y = report.accel_y * _accel_range_scale;
+	/* to align the sensor axes with the board, x and y need to be flipped */
+	_accel_report.x = report.accel_y * _accel_range_scale;
+	_accel_report.y = report.accel_x * _accel_range_scale;
+	/* z remains z and has the right sign */
 	_accel_report.z = report.accel_z * _accel_range_scale;
 	_accel_report.scaling = _accel_range_scale;
 	_accel_report.range_m_s2 = _accel_range_m_s2;
 
-	_gyro_report.x_raw = report.gyro_x;
-	_gyro_report.y_raw = report.gyro_y;
+	/* to align the sensor axes with the board, x and y need to be flipped */
+	_gyro_report.x_raw = report.gyro_y;
+	_gyro_report.y_raw = report.gyro_x;
+	/* z remains z and has the right sign */
 	_gyro_report.z_raw = report.gyro_z;
 
-	_gyro_report.x = report.gyro_x * _gyro_range_scale;
-	_gyro_report.y = report.gyro_y * _gyro_range_scale;
+	_gyro_report.x = report.gyro_y * _gyro_range_scale;
+	_gyro_report.y = report.gyro_x * _gyro_range_scale;
+	/* z remains z and has the right sign */
 	_gyro_report.z = report.gyro_z * _gyro_range_scale;
 	_gyro_report.scaling = _gyro_range_scale;
 	_gyro_report.range_rad_s = _gyro_range_rad_s;
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 56a9d8995..f75834ddf 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -2,8 +2,6 @@
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
  *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -604,14 +602,16 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 		read(_fd_bma180, buf, sizeof(buf));
 
 		accel_report.timestamp = hrt_absolute_time();
-		accel_report.x_raw = buf[0];
-		accel_report.y_raw = buf[1];
-		accel_report.z_raw = buf[2];
 
-		/* XXX scale raw values to readings */
-		accel_report.x = 0;
-		accel_report.y = 0;
-		accel_report.z = 0;
+		accel_report.x_raw = (buf[1] == -32768) ? 32767 : -buf[1];
+		accel_report.y_raw = (buf[0] == -32768) ? -32767 : buf[0];
+		accel_report.z_raw = (buf[2] == -32768) ? -32767 : buf[2];
+
+		const float range_g = 4.0f;
+		/* scale from 14 bit to m/s2 */
+		accel_report.x = (((accel_report.x_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
+		accel_report.y = (((accel_report.y_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
+		accel_report.z = (((accel_report.z_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
 
 	} else {
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
@@ -640,14 +640,16 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 		read(_fd_gyro_l3gd20, buf, sizeof(buf));
 
 		gyro_report.timestamp = hrt_absolute_time();
-		gyro_report.x_raw = buf[0];
-		gyro_report.y_raw = buf[1];
-		gyro_report.z_raw = buf[2];
-
-		/* XXX scale raw values to readings */
-		gyro_report.x = 0;
-		gyro_report.y = 0;
-		gyro_report.z = 0;
+
+		gyro_report.x_raw = ((buf[1] == -32768) ? -32768 : buf[1]);
+		gyro_report.y_raw = ((buf[0] == -32768) ? 32767 : -buf[0]);
+		gyro_report.z_raw = ((buf[2] == -32768) ? -32768 : buf[2]);
+
+		/* scaling calculated as: raw * (1/(32768*(500/180*PI))) */
+		gyro_report.x = (gyro_report.x_raw - _parameters.gyro_offset[0]) * 0.000266316109f;
+		gyro_report.y = (gyro_report.y_raw - _parameters.gyro_offset[1]) * 0.000266316109f;
+		gyro_report.z = (gyro_report.z_raw - _parameters.gyro_offset[2]) * 0.000266316109f;
+
 	} else {
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);