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

diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index d27c42b0..065ed221 100644
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -73,11 +73,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² */
@@ -145,9 +147,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,
@@ -243,14 +245,33 @@ ao_mpu6000_setup(void)
 	ao_mpu6000_configured = 1;
 }
 
+struct ao_mpu6000_sample ao_mpu6000_current;
+uint8_t ao_mpu6000_valid;
+
+static void
+ao_mpu6000(void)
+{
+	ao_mpu6000_setup();
+	for (;;)
+	{
+		struct ao_mpu6000_sample ao_mpu6000_next;
+		ao_mpu6000_sample(&ao_mpu6000_next);
+		ao_arch_critical(
+			ao_mpu6000_current = ao_mpu6000_next;
+			ao_mpu6000_valid = 1;
+			);
+		ao_delay(0);
+	}
+}
+
+static struct ao_task ao_mpu6000_task;
 
 static void
 ao_mpu6000_show(void)
 {
 	struct ao_mpu6000_sample	sample;
 
-	ao_mpu6000_setup();
-	ao_mpu6000_sample(&sample);
+	sample = ao_mpu6000_current;
 	printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d\n",
 		ao_mpu6000_accel(sample.accel_x),
 		ao_mpu6000_accel(sample.accel_y),
@@ -269,6 +290,8 @@ void
 ao_mpu6000_init(void)
 {
 	ao_mpu6000_configured = 0;
+	ao_mpu6000_valid = 0;
 
+	ao_add_task(&ao_mpu6000_task, ao_mpu6000, "mpu6000");
 	ao_cmd_register(&ao_mpu6000_cmds[0]);
 }