X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fdrivers%2Fao_mpu6000.c;h=a1c32d4d836db80808389009a8b38d0e6d2565ee;hb=aab7b31b71aa7c87c5a5003084e4b7773c30835f;hp=df400fcb096fa1be1130ca67e71d04191a0bde4e;hpb=0239e4dfe587528524b6380bbf6d9583047e52d6;p=fw%2Faltos

diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index df400fcb..a1c32d4d 100644
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -102,16 +102,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
@@ -122,23 +118,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;
@@ -224,13 +216,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);
 
 	/* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */
 	ao_mpu6000_reg_write(MPU6000_CONFIG,
@@ -246,6 +241,7 @@ ao_mpu6000_setup(void)
 }
 
 struct ao_mpu6000_sample ao_mpu6000_current;
+uint8_t ao_mpu6000_valid;
 
 static void
 ao_mpu6000(void)
@@ -257,6 +253,7 @@ ao_mpu6000(void)
 		ao_mpu6000_sample(&ao_mpu6000_next);
 		ao_arch_critical(
 			ao_mpu6000_current = ao_mpu6000_next;
+			ao_mpu6000_valid = 1;
 			);
 		ao_delay(0);
 	}
@@ -269,14 +266,16 @@ ao_mpu6000_show(void)
 {
 	struct ao_mpu6000_sample	sample;
 
-	sample = ao_mpu6000_current;
+	ao_arch_critical(
+		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),
-		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.accel_x,
+		sample.accel_y,
+		sample.accel_z,
+		sample.gyro_x,
+		sample.gyro_y,
+		sample.gyro_z);
 }
 
 static const struct ao_cmds ao_mpu6000_cmds[] = {
@@ -288,6 +287,7 @@ 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]);