projects
/
fw
/
altos
/ blobdiff
commit
grep
author
committer
pickaxe
?
search:
re
summary
|
shortlog
|
log
|
commit
|
commitdiff
|
tree
raw
|
inline
| side by side
altos: Fix up quadrature driver
[fw/altos]
/
src
/
drivers
/
ao_mpu6000.c
diff --git
a/src/drivers/ao_mpu6000.c
b/src/drivers/ao_mpu6000.c
index eb4044be225ebd257aa058eaaf7615fe193f560a..a1c32d4d836db80808389009a8b38d0e6d2565ee 100644
(file)
--- 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) {
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) {
}
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
}
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) {
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) {
}
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;
}
static void
ao_mpu6000_setup(void)
{
struct ao_mpu6000_sample normal_mode, test_mode;
- int
t
;
+ int
errors =0
;
if (ao_mpu6000_configured)
return;
if (ao_mpu6000_configured)
return;
@@
-147,9
+139,9
@@
ao_mpu6000_setup(void)
ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
(1 << MPU6000_PWR_MGMT_1_DEVICE_RESET));
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,
/* Reset signal conditioning */
ao_mpu6000_reg_write(MPU6000_USER_CONTROL,
@@
-224,13
+216,16
@@
ao_mpu6000_setup(void)
ao_delay(AO_MS_TO_TICKS(10));
ao_mpu6000_sample(&normal_mode);
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,
/* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */
ao_mpu6000_reg_write(MPU6000_CONFIG,
@@
-245,21
+240,42
@@
ao_mpu6000_setup(void)
ao_mpu6000_configured = 1;
}
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;
static void
ao_mpu6000_show(void)
{
struct ao_mpu6000_sample sample;
- ao_mpu6000_setup();
- ao_mpu6000_sample(&sample);
+ ao_arch_critical(
+ sample = ao_mpu6000_current;
+ );
printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d\n",
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[] = {
}
static const struct ao_cmds ao_mpu6000_cmds[] = {
@@
-271,6
+287,8
@@
void
ao_mpu6000_init(void)
{
ao_mpu6000_configured = 0;
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]);
}
ao_cmd_register(&ao_mpu6000_cmds[0]);
}