altos: Get mpu6000 working

This initializes the device appropraitely, and provides a command to
dump the current values in converted form.

Signed-off-by: Keith Packard <keithp@keithp.com>

2 files changed, 334 insertions, 24 deletions

diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index d7f67d6e..d27c42b0 100644
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -23,25 +23,27 @@ static uint8_t	ao_mpu6000_wake;
 static uint8_t	ao_mpu6000_configured;
 
 static void
-ao_mpu6000_isr(void)
+ao_mpu6000_write(uint8_t addr, uint8_t *data, uint8_t len)
 {
-	ao_exti_disable(&AO_MPU6000_INT_PORT, AO_MPU6000_INT_PIN);
-	ao_mpu6000_wake = 1;
-	ao_wakeup(&ao_mpu6000_wake);
+	ao_i2c_get(AO_MPU6000_I2C_INDEX);
+	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
+	ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE);
+	ao_i2c_send(data, len, AO_MPU6000_I2C_INDEX, TRUE);
+	ao_i2c_put(AO_MPU6000_I2C_INDEX);
 }
 
 static void
-ao_mpu6000_write(uint8_t addr, uint8_t *data, uint8_t len)
+ao_mpu6000_reg_write(uint8_t addr, uint8_t value)
 {
+	uint8_t	d[2] = { addr, value };
 	ao_i2c_get(AO_MPU6000_I2C_INDEX);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
-	ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE);
-	ao_i2c_send(data, len, AO_MPU6000_I2C_INDEX, TRUE);
+	ao_i2c_send(d, 2, AO_MPU6000_I2C_INDEX, TRUE);
 	ao_i2c_put(AO_MPU6000_I2C_INDEX);
 }
 
 static void
-ao_mpu6000_read(uint8_t addr, uint8_t *data, uint8_t len)
+ao_mpu6000_read(uint8_t addr, void *data, uint8_t len)
 {
 	ao_i2c_get(AO_MPU6000_I2C_INDEX);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
@@ -51,34 +53,211 @@ ao_mpu6000_read(uint8_t addr, uint8_t *data, uint8_t len)
 	ao_i2c_put(AO_MPU6000_I2C_INDEX);
 }
 
+static uint8_t
+ao_mpu6000_reg_read(uint8_t addr)
+{
+	uint8_t	value;
+	ao_i2c_get(AO_MPU6000_I2C_INDEX);
+	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
+	ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE);
+	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_READ);
+	ao_i2c_recv(&value, 1, AO_MPU6000_I2C_INDEX, TRUE);
+	ao_i2c_put(AO_MPU6000_I2C_INDEX);
+	return value;
+}
+
+static void
+ao_mpu6000_sample(struct ao_mpu6000_sample *sample)
+{
+	uint16_t	*d = (uint16_t *) sample;
+	int		i = sizeof (*sample) / 2;
+
+	ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, sample, sizeof (*sample));
+	/* byte swap (sigh) */
+	while (i--) {
+		uint16_t	t = *d;
+		*d++ = (t >> 8) | (t << 8);
+	}
+}
+
+#define G	981	/* in cm/s² */
+
+static int16_t /* cm/s² */
+ao_mpu6000_accel(int16_t v)
+{
+	return (int16_t) ((v * (int32_t) (16.0 * 980.665 + 0.5)) / 32767);
+}
+
+static int16_t	/* deg*10/s */
+ao_mpu6000_gyro(int16_t v)
+{
+	return (int16_t) ((v * (int32_t) 20000) / 32767);
+}
+
+static uint8_t
+ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which)
+{
+	int16_t	diff = test - normal;
+
+	if (diff < MPU6000_ST_ACCEL(16) / 2) {
+		printf ("%s accel self test value too small (normal %d, test %d)\n",
+			which, normal, test);
+		return FALSE;
+	}
+	if (diff > MPU6000_ST_ACCEL(16) * 2) {
+		printf ("%s accel self test value too large (normal %d, test %d)\n",
+			which, normal, test);
+		return FALSE;
+	}
+	return TRUE;
+}
+
+static uint8_t
+ao_mpu6000_gyro_check(int16_t normal, int16_t test, char *which)
+{
+	int16_t	diff = test - normal;
+
+	if (diff < 0)
+		diff = -diff;
+	if (diff < MPU6000_ST_GYRO(2000) / 2) {
+		printf ("%s gyro self test value too small (normal %d, test %d)\n",
+			which, normal, test);
+		return FALSE;
+	}
+	if (diff > MPU6000_ST_GYRO(2000) * 2) {
+		printf ("%s gyro self test value too large (normal %d, test %d)\n",
+			which, normal, test);
+		return FALSE;
+	}
+	return TRUE;
+}
+
 static void
 ao_mpu6000_setup(void)
 {
+	struct ao_mpu6000_sample	normal_mode, test_mode;
+	int				t;
+
 	if (ao_mpu6000_configured)
 		return;
 
-	/* Enable the EXTI interrupt for the appropriate pin */
-	ao_enable_port(AO_MPU6000_INT_PORT);
-	ao_exti_setup(&AO_MPU6000_INT_PORT, AO_MPU6000_INT_PIN,
-		      AO_EXTI_MODE_FALLING, ao_mpu6000_isr);
+	/* Reset the whole chip */
+	
+	ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
+			     (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET));
+	while (ao_mpu6000_reg_read(MPU6000_PWR_MGMT_1) &
+	       (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET))
+		ao_yield();
+
+	/* Reset signal conditioning */
+	ao_mpu6000_reg_write(MPU6000_USER_CONTROL,
+			     (0 << MPU6000_USER_CONTROL_FIFO_EN) |
+			     (0 << MPU6000_USER_CONTROL_I2C_MST_EN) |
+			     (0 << MPU6000_USER_CONTROL_I2C_IF_DIS) |
+			     (0 << MPU6000_USER_CONTROL_FIFO_RESET) |
+			     (0 << MPU6000_USER_CONTROL_I2C_MST_RESET) |
+			     (1 << MPU6000_USER_CONTROL_SIG_COND_RESET));
+
+	while (ao_mpu6000_reg_read(MPU6000_USER_CONTROL) & (1 << MPU6000_USER_CONTROL_SIG_COND_RESET))
+		ao_yield();
+
+	/* Reset signal paths */
+	ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
+			     (1 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
+			     (1 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
+			     (1 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
+
+	ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET,
+			     (0 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) |
+			     (0 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) |
+			     (0 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET));
 
+	/* Select clocks, disable sleep */
+	ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
+			     (0 << MPU6000_PWR_MGMT_1_DEVICE_RESET) |
+			     (0 << MPU6000_PWR_MGMT_1_SLEEP) |
+			     (0 << MPU6000_PWR_MGMT_1_CYCLE) |
+			     (0 << MPU6000_PWR_MGMT_1_TEMP_DIS) |
+			     (MPU6000_PWR_MGMT_1_CLKSEL_PLL_X_AXIS << MPU6000_PWR_MGMT_1_CLKSEL));
+
+	/* Set sample rate divider to sample at full speed
+	ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, 0);
+
+	/* Disable filtering */
+	ao_mpu6000_reg_write(MPU6000_CONFIG,
+			     (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
+			     (MPU6000_CONFIG_DLPF_CFG_260_256 << MPU6000_CONFIG_DLPF_CFG));
+
+	/* Configure accelerometer to +/-16G in self-test mode */
+	ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
+			     (1 << MPU600_ACCEL_CONFIG_XA_ST) |
+			     (1 << MPU600_ACCEL_CONFIG_YA_ST) |
+			     (1 << MPU600_ACCEL_CONFIG_ZA_ST) |
+			     (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
+
+	/* Configure gyro to +/- 2000°/s in self-test mode */
+	ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
+			     (1 << MPU600_GYRO_CONFIG_XG_ST) |
+			     (1 << MPU600_GYRO_CONFIG_YG_ST) |
+			     (1 << MPU600_GYRO_CONFIG_ZG_ST) |
+			     (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
+
+	ao_delay(AO_MS_TO_TICKS(200));
+	ao_mpu6000_sample(&test_mode);
+
+	/* Configure accelerometer to +/-16G */
+	ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
+			     (0 << MPU600_ACCEL_CONFIG_XA_ST) |
+			     (0 << MPU600_ACCEL_CONFIG_YA_ST) |
+			     (0 << MPU600_ACCEL_CONFIG_ZA_ST) |
+			     (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL));
+
+	/* Configure gyro to +/- 2000°/s */
+	ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG,
+			     (0 << MPU600_GYRO_CONFIG_XG_ST) |
+			     (0 << MPU600_GYRO_CONFIG_YG_ST) |
+			     (0 << MPU600_GYRO_CONFIG_ZG_ST) |
+			     (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL));
+
+	ao_delay(AO_MS_TO_TICKS(10));
+	ao_mpu6000_sample(&normal_mode);
+	
+	ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x");
+	ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y");
+	ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z");
+
+	ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x");
+	ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y");
+	ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z");
+
+	/* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */
+	ao_mpu6000_reg_write(MPU6000_CONFIG,
+			     (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) |
+			     (MPU6000_CONFIG_DLPF_CFG_94_98 << MPU6000_CONFIG_DLPF_CFG));
+
+	/* Set sample rate divider to sample at 200Hz (v = gyro/rate - 1) */
+	ao_mpu6000_reg_write(MPU6000_SMPRT_DIV,
+			     1000 / 200 - 1);
+	
+	ao_delay(AO_MS_TO_TICKS(100));
 	ao_mpu6000_configured = 1;
 }
 
+
 static void
 ao_mpu6000_show(void)
 {
-	uint8_t	addr;
-	uint8_t	data[14];
-	uint8_t i;
-
-	ao_mpu6000_read(MPU6000_WHO_AM_I, data, 1);
-	printf ("mpu6000 WHO_AM_I: %02x\n", data[0]);
-#if 0
-	ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, data, 14);
-	for (i = 0; i < 14; i++)
-		printf ("reg %02x: %02x\n", i + MPU6000_ACCEL_XOUT_H, data[i]);
-#endif
+	struct ao_mpu6000_sample	sample;
+
+	ao_mpu6000_setup();
+	ao_mpu6000_sample(&sample);
+	printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d\n",
+		ao_mpu6000_accel(sample.accel_x),
+		ao_mpu6000_accel(sample.accel_y),
+		ao_mpu6000_accel(sample.accel_z),
+		ao_mpu6000_gyro(sample.gyro_x),
+		ao_mpu6000_gyro(sample.gyro_y),
+		ao_mpu6000_gyro(sample.gyro_z));
 }
 
 static const struct ao_cmds ao_mpu6000_cmds[] = {
diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h
index d436a3e0..ca76b081 100644
--- a/src/drivers/ao_mpu6000.h
+++ b/src/drivers/ao_mpu6000.h
@@ -21,9 +21,140 @@
 #define MPU6000_ADDR_WRITE	0xd0
 #define MPU6000_ADDR_READ	0xd1
 
-#define MPU6000_ACCEL_XOUT_H	0x3b
+#define MPU6000_SMPRT_DIV	0x19
+
+#define MPU6000_CONFIG		0x1a
+
+#define  MPU6000_CONFIG_EXT_SYNC_SET	3
+#define  MPU6000_CONFIG_EXT_SYNC_SET_DISABLED		0
+#define  MPU6000_CONFIG_EXT_SYNC_SET_TEMP_OUT_L		1
+#define  MPU6000_CONFIG_EXT_SYNC_SET_GYRO_XOUT_L	2
+#define  MPU6000_CONFIG_EXT_SYNC_SET_GYRO_YOUT_L	3
+#define  MPU6000_CONFIG_EXT_SYNC_SET_GYRO_ZOUT_L	4
+#define  MPU6000_CONFIG_EXT_SYNC_SET_ACCEL_XOUT_L	5
+#define  MPU6000_CONFIG_EXT_SYNC_SET_ACCEL_YOUT_L	6
+#define  MPU6000_CONFIG_EXT_SYNC_SET_ACCEL_ZOUT_L	7
+#define  MPU6000_CONFIG_EXT_SYNC_SET_MASK		7
+
+#define  MPU6000_CONFIG_DLPF_CFG	0
+#define  MPU6000_CONFIG_DLPF_CFG_260_256		0
+#define  MPU6000_CONFIG_DLPF_CFG_184_188		1
+#define  MPU6000_CONFIG_DLPF_CFG_94_98			2
+#define  MPU6000_CONFIG_DLPF_CFG_44_42			3
+#define  MPU6000_CONFIG_DLPF_CFG_21_20			4
+#define  MPU6000_CONFIG_DLPF_CFG_10_10			5
+#define  MPU6000_CONFIG_DLPF_CFG_5_5			6
+#define  MPU6000_CONFIG_DLPF_CFG_MASK			7
+
+#define MPU6000_GYRO_CONFIG	0x1b
+# define MPU600_GYRO_CONFIG_XG_ST	7
+# define MPU600_GYRO_CONFIG_YG_ST	6
+# define MPU600_GYRO_CONFIG_ZG_ST	5
+# define MPU600_GYRO_CONFIG_FS_SEL	3
+# define MPU600_GYRO_CONFIG_FS_SEL_250		0
+# define MPU600_GYRO_CONFIG_FS_SEL_500		1
+# define MPU600_GYRO_CONFIG_FS_SEL_1000		2
+# define MPU600_GYRO_CONFIG_FS_SEL_2000		3
+# define MPU600_GYRO_CONFIG_FS_SEL_MASK		3
+
+#define MPU6000_ACCEL_CONFIG	0x1c
+# define MPU600_ACCEL_CONFIG_XA_ST	7
+# define MPU600_ACCEL_CONFIG_YA_ST	6
+# define MPU600_ACCEL_CONFIG_ZA_ST	5
+# define MPU600_ACCEL_CONFIG_AFS_SEL	3
+# define MPU600_ACCEL_CONFIG_AFS_SEL_2G		0
+# define MPU600_ACCEL_CONFIG_AFS_SEL_4G		1
+# define MPU600_ACCEL_CONFIG_AFS_SEL_8G		2
+# define MPU600_ACCEL_CONFIG_AFS_SEL_16G	3
+# define MPU600_ACCEL_CONFIG_AFS_SEL_MASK	3
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF	0
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF_RESET	0
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF_5Hz	1
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF_2_5Hz	2
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF_1_25Hz	3
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF_0_63Hz	4
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF_HOLD	7
+# define MPU600_ACCEL_CONFIG_ACCEL_HPF_MASK	7
+
+#define MPU6000_INT_ENABLE	0x38
+#define  MPU6000_INT_ENABLE_FF_EN		7
+#define  MPU6000_INT_ENABLE_MOT_EN		6
+#define  MPU6000_INT_ENABLE_ZMOT_EN		5
+#define  MPU6000_INT_ENABLE_FIFO_OFLOW_EN	4
+#define  MPU6000_INT_ENABLE_I2C_MST_INT_EN	3
+#define  MPU6000_INT_ENABLE_DATA_RDY_EN		0
+
+#define MPU6000_INT_STATUS	0x3a
+#define  MPU6000_INT_STATUS_FF_EN		7
+#define  MPU6000_INT_STATUS_MOT_EN		6
+#define  MPU6000_INT_STATUS_ZMOT_EN		5
+#define  MPU6000_INT_STATUS_FIFO_OFLOW_EN	4
+#define  MPU6000_INT_STATUS_I2C_MST_INT_EN	3
+#define  MPU6000_INT_STATUS_DATA_RDY_EN		0
+
+#define MPU6000_ACCEL_XOUT_H		0x3b
+#define MPU6000_ACCEL_XOUT_L		0x3c
+#define MPU6000_ACCEL_YOUT_H		0x3d
+#define MPU6000_ACCEL_YOUT_L		0x3e
+#define MPU6000_ACCEL_ZOUT_H		0x3f
+#define MPU6000_ACCEL_ZOUT_L		0x40
+#define MPU6000_TEMP_H			0x41
+#define MPU6000_TEMP_L			0x42
+#define MPU6000_GYRO_XOUT_H		0x43
+#define MPU6000_GYRO_XOUT_L		0x44
+#define MPU6000_GYRO_YOUT_H		0x45
+#define MPU6000_GYRO_YOUT_L		0x46
+#define MPU6000_GYRO_ZOUT_H		0x47
+#define MPU6000_GYRO_ZOUT_L		0x48
+
+#define MPU6000_SIGNAL_PATH_RESET	0x68
+#define MPU6000_SIGNAL_PATH_RESET_GYRO_RESET	2
+#define MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET	1
+#define MPU6000_SIGNAL_PATH_RESET_TEMP_RESET	0
+
+#define MPU6000_USER_CONTROL		0x6a
+#define MPU6000_USER_CONTROL_FIFO_EN		6
+#define MPU6000_USER_CONTROL_I2C_MST_EN		5
+#define MPU6000_USER_CONTROL_I2C_IF_DIS		4
+#define MPU6000_USER_CONTROL_FIFO_RESET		2
+#define MPU6000_USER_CONTROL_I2C_MST_RESET	1
+#define MPU6000_USER_CONTROL_SIG_COND_RESET	0
+
+#define MPU6000_PWR_MGMT_1	0x6b
+#define MPU6000_PWR_MGMT_1_DEVICE_RESET		7
+#define MPU6000_PWR_MGMT_1_SLEEP		6
+#define MPU6000_PWR_MGMT_1_CYCLE		5
+#define MPU6000_PWR_MGMT_1_TEMP_DIS		3
+#define MPU6000_PWR_MGMT_1_CLKSEL		0
+#define MPU6000_PWR_MGMT_1_CLKSEL_INTERNAL		0
+#define MPU6000_PWR_MGMT_1_CLKSEL_PLL_X_AXIS		1
+#define MPU6000_PWR_MGMT_1_CLKSEL_PLL_Y_AXIS		2
+#define MPU6000_PWR_MGMT_1_CLKSEL_PLL_Z_AXIS		3
+#define MPU6000_PWR_MGMT_1_CLKSEL_PLL_EXTERNAL_32K	4
+#define MPU6000_PWR_MGMT_1_CLKSEL_PLL_EXTERNAL_19M	5
+#define MPU6000_PWR_MGMT_1_CLKSEL_STOP			7
+#define MPU6000_PWR_MGMT_1_CLKSEL_MASK			7
+
+#define MPU6000_PWR_MGMT_2	0x6c
+
 #define MPU6000_WHO_AM_I	0x75
 
+/* Self test acceleration is approximately 0.5g */
+#define MPU6000_ST_ACCEL(full_scale)	(32767 / ((full_scale) * 2))
+
+/* Self test gyro is approximately 50°/s */
+#define MPU6000_ST_GYRO(full_scale)	((int16_t) (((int32_t) 32767 * (int32_t) 50) / (full_scale)))
+
+struct ao_mpu6000_sample {
+	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;
+};
+
 void
 ao_mpu6000_init(void);