Driver debugging (scaling, ranges, endianess) MPU-6000 needs more love

4 files changed, 127 insertions, 58 deletions

diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h
index 47d366bc4..977440518 100644
--- a/apps/drivers/drv_accel.h
+++ b/apps/drivers/drv_accel.h
@@ -57,9 +57,9 @@ struct accel_report {
 	float z;
 	float range_m_s2;
 	float scaling;
-	uint16_t x_raw;
-	uint16_t y_raw;
-	uint16_t z_raw;
+	int16_t x_raw;
+	int16_t y_raw;
+	int16_t z_raw;
 };
 
 /** accel scaling factors; Vout = (Vin * Vscale) + Voffset */
diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h
index 895d2d797..1f5bb998e 100644
--- a/apps/drivers/drv_mag.h
+++ b/apps/drivers/drv_mag.h
@@ -60,9 +60,9 @@ struct mag_report {
 	float range_ga;
 	float scaling;
 	
-	uint16_t x_raw;
-	uint16_t y_raw;
-	uint16_t z_raw;
+	int16_t x_raw;
+	int16_t y_raw;
+	int16_t z_raw;
 };
 
 /** mag scaling factors; Vout = (Vin * Vscale) + Voffset */
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index 3a194ee11..99deb5da8 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -649,9 +649,9 @@ HMC5883::collect()
 	}
 
 	/* swap the data we just received */
-	report.x = ((int16_t)hmc_report.x[0] << 8) + hmc_report.x[1];
-	report.y = ((int16_t)hmc_report.y[0] << 8) + hmc_report.y[1];
-	report.z = ((int16_t)hmc_report.z[0] << 8) + hmc_report.z[1];
+	report.x = (((int16_t)hmc_report.x[0]) << 8) + hmc_report.x[1];
+	report.y = (((int16_t)hmc_report.y[0]) << 8) + hmc_report.y[1];
+	report.z = (((int16_t)hmc_report.z[0]) << 8) + hmc_report.z[1];
 
 	/*
 	 * If any of the values are -4096, there was an internal math error in the sensor.
@@ -662,10 +662,14 @@ 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 */
+	/*
+	 * RAW outputs
+	 *
+	 * to align the sensor axes with the board, x and y need to be flipped
+	 * and y needs to be negated
+	 */
 	_reports[_next_report].x_raw = report.y;
-	_reports[_next_report].y_raw = report.x;
+	_reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x);
 	/* z remains z */
 	_reports[_next_report].z_raw = report.z;
 
@@ -688,7 +692,8 @@ HMC5883::collect()
 
 	/* to align the sensor axes with the board, x and y need to be flipped */
 	_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
-	_reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
+	/* flip axes and negate value for y */
+	_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
 	/* z remains z */
 	_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index 5a801a9b7..cb526d52a 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -386,7 +386,7 @@ MPU6000::init()
 	_gyro_scale.y_scale  = 1.0f;
 	_gyro_scale.z_offset = 0;
 	_gyro_scale.z_scale  = 1.0f;
-	_gyro_range_scale = 1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
+	_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
 	_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;
 
 	// product-specific scaling
@@ -421,7 +421,7 @@ MPU6000::init()
 	_accel_scale.y_scale  = 1.0f;
 	_accel_scale.z_offset = 0;
 	_accel_scale.z_scale  = 1.0f;
-	_accel_range_scale = 1.0f / (4096.0f / 9.81f);
+	_accel_range_scale = (9.81f / 4096.0f);
 	_accel_range_m_s2 = 8.0f * 9.81f;
 
 	usleep(1000);
@@ -595,6 +595,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case ACCELIOCSRANGE:
 	case ACCELIOCGRANGE:
 		/* XXX not implemented */
+		// XXX change these two values on set:
+		// _accel_range_scale = (9.81f / 4096.0f);
+		// _accel_range_rad_s = 8.0f * 9.81f;
 		return -EINVAL;
 
 	default:
@@ -634,6 +637,9 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 	case GYROIOCSRANGE:
 	case GYROIOCGRANGE:
 		/* XXX not implemented */
+		// XXX change these two values on set:
+		// _gyro_range_scale = xx
+		// _gyro_range_m_s2 = xx
 		return -EINVAL;
 
 	default:
@@ -755,6 +761,20 @@ MPU6000::measure_trampoline(void *arg)
 	dev->measure();
 }
 
+int16_t
+int16_t_from_bytes(uint8_t bytes[])
+{
+    union {
+        uint8_t    b[2];
+        int16_t    w;
+    } u;
+
+    u.b[1] = bytes[0];
+    u.b[0] = bytes[1];
+
+    return u.w;
+}
+
 void
 MPU6000::measure()
 {
@@ -763,40 +783,80 @@ MPU6000::measure()
 	 * Report conversation within the MPU6000, including command byte and
 	 * interrupt status.
 	 */
-	struct Report {
+	struct MPUReport {
 		uint8_t		cmd;
 		uint8_t		status;
-		uint16_t	accel_x;
-		uint16_t	accel_y;
-		uint16_t	accel_z;
-		uint16_t	temp;
-		uint16_t	gyro_x;
-		uint16_t	gyro_y;
-		uint16_t	gyro_z;
-	} report;
+		uint8_t		accel_x[2];
+		uint8_t		accel_y[2];
+		uint8_t		accel_z[2];
+		uint8_t		temp[2];
+		uint8_t		gyro_x[2];
+		uint8_t		gyro_y[2];
+		uint8_t		gyro_z[2];
+	} mpu_report;
 #pragma pack(pop)
 
+	struct Report {
+		int16_t		accel_x;
+		int16_t		accel_y;
+		int16_t		accel_z;
+		int16_t		temp;
+		int16_t		gyro_x;
+		int16_t		gyro_y;
+		int16_t		gyro_z;
+	} report;
+
 	/* start measuring */
 	perf_begin(_sample_perf);
 
 	/*
 	 * Fetch the full set of measurements from the MPU6000 in one pass.
 	 */
-	report.cmd = DIR_READ | MPUREG_INT_STATUS;
-	transfer((uint8_t *)&report, (uint8_t *)&report, sizeof(report));
+	mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
+	transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report));
 
 	/*
-	 * Adjust and scale results to mg.
+	 * Convert from big to little endian
 	 */
-	_gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time();
+	// report.accel_x = ((int16_t)(mpu_report.accel_x[0]) << 8);
+	// report.accel_x |= mpu_report.accel_x[1];
+	// report.accel_y = ((int16_t)(mpu_report.accel_y[0]) << 8);
+	// report.accel_y |= mpu_report.accel_y[1];
+	// report.accel_z = ((int16_t)(mpu_report.accel_z[0]) << 8);
+	// report.accel_z |= mpu_report.accel_z[1];
 
-	/* 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;
-	/* z remains z and has the right sign */
-	_accel_report.z_raw = report.accel_z;
+	report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
+	report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
+	report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
+
+	report.temp = (((int16_t)mpu_report.temp[0]) << 8) | mpu_report.temp[1];
+
+	report.gyro_x = ((int16_t)(mpu_report.gyro_x[0]) << 8) | mpu_report.gyro_x[1];
+	report.gyro_y = ((int16_t)(mpu_report.gyro_y[0]) << 8) | mpu_report.gyro_y[1];
+	report.gyro_z = ((int16_t)(mpu_report.gyro_z[0]) << 8) | mpu_report.gyro_z[1];
+
+	/*
+	 * Swap axes and negate y
+	 */
+	int16_t accel_xt = report.accel_y;
+	int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
+
+	int16_t gyro_xt = report.gyro_y;
+	int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
+
+	/*
+	 * Apply the swap
+	 */
+	report.accel_x = accel_xt;
+	report.accel_y = accel_yt;
+	report.gyro_x = gyro_xt;
+	report.gyro_y = gyro_yt;
+
+	/*
+	 * Adjust and scale results to m/s^2.
+	 */
+	_gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time();
 
-	/* to align the sensor axes with the board, x and y need to be flipped */
 
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.
@@ -812,22 +872,26 @@ MPU6000::measure()
 	 *	 	  the offset is 74 from the origin and subtracting
 	 *		  74 from all measurements centers them around zero.
 	 */
-	_accel_report.x = ((report.accel_y * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	_accel_report.y = ((report.accel_x * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	/* z remains z and has the right sign */
+
+
+	/* NOTE: Axes have been swapped to match the board a few lines above. */
+
+	_accel_report.x_raw = report.accel_x;
+	_accel_report.y_raw = report.accel_y;
+	_accel_report.z_raw = report.accel_z;
+
+	_accel_report.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
+	_accel_report.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
 	_accel_report.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
 	_accel_report.scaling = _accel_range_scale;
 	_accel_report.range_m_s2 = _accel_range_m_s2;
 
-	/* 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.x_raw = report.gyro_x;
+	_gyro_report.y_raw = report.gyro_y;
 	_gyro_report.z_raw = report.gyro_z;
 
-	_gyro_report.x = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
-	_gyro_report.y = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
-	/* z remains z and has the right sign */
+	_gyro_report.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
+	_gyro_report.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
 	_gyro_report.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
 	_gyro_report.scaling = _gyro_range_scale;
 	_gyro_report.range_rad_s = _gyro_range_rad_s;
@@ -963,12 +1027,12 @@ test()
 
 	warnx("single read");
 	warnx("time:     %lld", a_report.timestamp);
-	warnx("acc  x:  \t% 8.4f\tm/s^2", (double)a_report.x);
-	warnx("acc  y:  \t% 8.4f\tm/s^2", (double)a_report.y);
-	warnx("acc  z:  \t% 8.4f\tm/s^2", (double)a_report.z);
-	warnx("acc  x:  \t%d\traw", a_report.x_raw);
-	warnx("acc  y:  \t%d\traw", a_report.y_raw);
-	warnx("acc  z:  \t%d\traw", a_report.z_raw);
+	warnx("acc  x:  \t%8.4f\tm/s^2", (double)a_report.x);
+	warnx("acc  y:  \t%8.4f\tm/s^2", (double)a_report.y);
+	warnx("acc  z:  \t%8.4f\tm/s^2", (double)a_report.z);
+	warnx("acc  x:  \t%d\traw", (int)a_report.x_raw);
+	warnx("acc  y:  \t%d\traw", (int)a_report.y_raw);
+	warnx("acc  z:  \t%d\traw", (int)a_report.z_raw);
 	warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
 		(double)(a_report.range_m_s2 / 9.81f));
 
@@ -977,14 +1041,14 @@ test()
 	if (sz != sizeof(g_report))
 		err(1, "immediate gyro read failed");
 
-	warnx("gyro x: \t% 8.4f\trad/s", (double)g_report.x);
-	warnx("gyro y: \t% 8.4f\trad/s", (double)g_report.y);
-	warnx("gyro z: \t% 8.4f\trad/s", (double)g_report.z);
-	warnx("gyro x: \t%d\traw", g_report.x_raw);
-	warnx("gyro y: \t%d\traw", g_report.y_raw);
-	warnx("gyro z: \t%d\traw", g_report.z_raw);
-	warnx("gyro range: %8.4f rad/s (%8.2f deg/s)", (double)g_report.range_rad_s,
-		(double)((g_report.range_rad_s / M_PI_F) * 180.0f));
+	warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
+	warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
+	warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
+	warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
+	warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
+	warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
+	warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
+		(int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f));
 
 	/* XXX add poll-rate tests here too */