X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_mpu6000.c;h=c65aecbcc1d363f9db603ea439f7c27b756df47d;hp=d27c42b04c83187bab6c0d5e0c8a1c8f3d3d5f4b;hb=fefc021045089ffd00d03e4c4e6cf42a13692828;hpb=318b564486aa9965bbad54c71e51fcb32b414162

diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index d27c42b0..c65aecbc 100644
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -22,47 +22,71 @@
 static uint8_t	ao_mpu6000_wake;
 static uint8_t	ao_mpu6000_configured;
 
-static void
-ao_mpu6000_write(uint8_t addr, uint8_t *data, uint8_t len)
-{
-	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);
-}
+#define ao_mpu6000_spi_get() 	ao_spi_get_bit(AO_MPU6000_SPI_CS_PORT,	\
+					       AO_MPU6000_SPI_CS_PIN,	\
+					       AO_MPU6000_SPI_CS,	\
+					       AO_MPU6000_SPI_BUS,	\
+					       AO_SPI_SPEED_1MHz)
+
+#define ao_mpu6000_spi_put() 	ao_spi_put_bit(AO_MPU6000_SPI_CS_PORT,	\
+					       AO_MPU6000_SPI_CS_PIN,	\
+					       AO_MPU6000_SPI_CS,	\
+					       AO_MPU6000_SPI_BUS)
+
 
 static void
 ao_mpu6000_reg_write(uint8_t addr, uint8_t value)
 {
 	uint8_t	d[2] = { addr, value };
+#ifdef AO_MPU6000_I2C_INDEX
 	ao_i2c_get(AO_MPU6000_I2C_INDEX);
 	ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE);
 	ao_i2c_send(d, 2, AO_MPU6000_I2C_INDEX, TRUE);
 	ao_i2c_put(AO_MPU6000_I2C_INDEX);
+#else
+	ao_mpu6000_spi_get();
+	ao_spi_send(d, 2, AO_MPU6000_SPI_BUS);
+	ao_mpu6000_spi_put();
+#endif
 }
 
 static void
 ao_mpu6000_read(uint8_t addr, void *data, uint8_t len)
 {
+#ifdef AO_MPU6000_I2C_INDEX
 	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(data, len, AO_MPU6000_I2C_INDEX, TRUE);
 	ao_i2c_put(AO_MPU6000_I2C_INDEX);
+#else
+	addr |= 0x80;
+	ao_mpu6000_spi_get();
+	ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
+	ao_spi_recv(data, len, AO_MPU6000_SPI_BUS);
+	ao_mpu6000_spi_put();
+#endif
 }
 
 static uint8_t
 ao_mpu6000_reg_read(uint8_t addr)
 {
 	uint8_t	value;
+#ifdef AO_MPU6000_I2C_INDEX
 	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);
+#else
+	addr |= 0x80;
+	ao_mpu6000_spi_get();
+	ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
+	ao_spi_recv(&value, 1, AO_MPU6000_SPI_BUS);
+	ao_mpu6000_spi_put();
+#endif
 	return value;
 }
 
@@ -73,11 +97,13 @@ ao_mpu6000_sample(struct ao_mpu6000_sample *sample)
 	int		i = sizeof (*sample) / 2;
 
 	ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, sample, sizeof (*sample));
-	/* byte swap (sigh) */
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+	/* byte swap */
 	while (i--) {
 		uint16_t	t = *d;
 		*d++ = (t >> 8) | (t << 8);
 	}
+#endif
 }
 
 #define G	981	/* in cm/s² */
@@ -100,16 +126,12 @@ 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;
+		return 1;
 	}
 	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 1;
 	}
-	return TRUE;
+	return 0;
 }
 
 static uint8_t
@@ -120,23 +142,19 @@ ao_mpu6000_gyro_check(int16_t normal, int16_t test, char *which)
 	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;
+		return 1;
 	}
 	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 1;
 	}
-	return TRUE;
+	return 0;
 }
 
 static void
 ao_mpu6000_setup(void)
 {
 	struct ao_mpu6000_sample	normal_mode, test_mode;
-	int				t;
+	int				errors =0;
 
 	if (ao_mpu6000_configured)
 		return;
@@ -145,9 +163,9 @@ ao_mpu6000_setup(void)
 	
 	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();
+
+	/* Wait for it to reset. If we talk too quickly, it appears to get confused */
+	ao_delay(AO_MS_TO_TICKS(100));
 
 	/* Reset signal conditioning */
 	ao_mpu6000_reg_write(MPU6000_USER_CONTROL,
@@ -205,6 +223,24 @@ ao_mpu6000_setup(void)
 	ao_delay(AO_MS_TO_TICKS(200));
 	ao_mpu6000_sample(&test_mode);
 
+#if TRIDGE
+	// read the product ID rev c has 1/2 the sensitivity of rev d
+    _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID);
+    //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
+
+    if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) ||
+        (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) {
+        // Accel scale 8g (4096 LSB/g)
+        // Rev C has different scaling than rev D
+        register_write(MPUREG_ACCEL_CONFIG,1<<3);
+    } else {
+        // Accel scale 8g (4096 LSB/g)
+        register_write(MPUREG_ACCEL_CONFIG,2<<3);
+    }
+    hal.scheduler->delay(1);
+
+#endif
+
 	/* Configure accelerometer to +/-16G */
 	ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG,
 			     (0 << MPU600_ACCEL_CONFIG_XA_ST) |
@@ -222,13 +258,16 @@ ao_mpu6000_setup(void)
 	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");
+	errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x");
+	errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y");
+	errors += 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");
+	errors += ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x");
+	errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y");
+	errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z");
+
+	if (errors)
+		ao_panic(AO_PANIC_SELF_TEST_MPU6000);
 
 	/* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */
 	ao_mpu6000_reg_write(MPU6000_CONFIG,
@@ -243,21 +282,37 @@ ao_mpu6000_setup(void)
 	ao_mpu6000_configured = 1;
 }
 
+struct ao_mpu6000_sample	ao_mpu6000_current;
+
+static void
+ao_mpu6000(void)
+{
+	ao_mpu6000_setup();
+	for (;;)
+	{
+		ao_mpu6000_sample(&ao_mpu6000_current);
+		ao_arch_critical(
+			AO_DATA_PRESENT(AO_DATA_MPU6000);
+			AO_DATA_WAIT();
+			);
+	}
+}
+
+static struct ao_task ao_mpu6000_task;
 
 static void
 ao_mpu6000_show(void)
 {
-	struct ao_mpu6000_sample	sample;
+	struct ao_data	sample;
 
-	ao_mpu6000_setup();
-	ao_mpu6000_sample(&sample);
+	ao_data_get(&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));
+		sample.mpu6000.accel_x,
+		sample.mpu6000.accel_y,
+		sample.mpu6000.accel_z,
+		sample.mpu6000.gyro_x,
+		sample.mpu6000.gyro_y,
+		sample.mpu6000.gyro_z);
 }
 
 static const struct ao_cmds ao_mpu6000_cmds[] = {
@@ -270,5 +325,6 @@ ao_mpu6000_init(void)
 {
 	ao_mpu6000_configured = 0;
 
+	ao_add_task(&ao_mpu6000_task, ao_mpu6000, "mpu6000");
 	ao_cmd_register(&ao_mpu6000_cmds[0]);
 }