altos: Use stdbool true/false instead of TRUE/FALSE

[fw/altos] / src / drivers / ao_mpu6000.c

diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index f8ce7346c7149d690228c57445607382ce35c05b..8c85ab01fd82c00041128a9147e3f6a26f2caa39 100644 (file)
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -3,7 +3,8 @@
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
  *
  * This program is distributed in the hope that it will be useful, but
  * WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -21,7 +22,6 @@
 
 #if HAS_MPU6000
 
-static uint8_t ao_mpu6000_wake;
 static uint8_t ao_mpu6000_configured;
 
 #ifndef AO_MPU6000_I2C_INDEX
@@ -55,7 +55,7 @@ _ao_mpu6000_reg_write(uint8_t addr, uint8_t value)
 #else
        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_send(d, 2, AO_MPU6000_I2C_INDEX, true);
        ao_i2c_put(AO_MPU6000_I2C_INDEX);
 #endif
 }
@@ -72,9 +72,9 @@ _ao_mpu6000_read(uint8_t addr, void *data, uint8_t len)
 #else
        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(&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_recv(data, len, AO_MPU6000_I2C_INDEX, true);
        ao_i2c_put(AO_MPU6000_I2C_INDEX);
 #endif
 }
@@ -92,9 +92,9 @@ _ao_mpu6000_reg_read(uint8_t addr)
 #else
        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(&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_recv(&value, 1, AO_MPU6000_I2C_INDEX, true);
        ao_i2c_put(AO_MPU6000_I2C_INDEX);
 #endif
        return value;
@@ -133,7 +133,7 @@ ao_mpu6000_gyro(int16_t v)
 #endif
 
 static uint8_t
-ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which)
+ao_mpu6000_accel_check(int16_t normal, int16_t test)
 {
        int16_t diff = test - normal;
 
@@ -147,7 +147,7 @@ ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which)
 }
 
 static uint8_t
-ao_mpu6000_gyro_check(int16_t normal, int16_t test, char *which)
+ao_mpu6000_gyro_check(int16_t normal, int16_t test)
 {
        int16_t diff = test - normal;
 
@@ -192,7 +192,7 @@ _ao_mpu6000_setup(void)
        _ao_mpu6000_wait_alive();
 
        /* Reset the whole chip */
-       
+
        _ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1,
                              (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET));
 
@@ -292,14 +292,14 @@ _ao_mpu6000_setup(void)
 
                ao_delay(AO_MS_TO_TICKS(200));
                _ao_mpu6000_sample(&normal_mode);
-       
-               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");
-
-               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");
+
+               errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x);
+               errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y);
+               errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z);
+
+               errors += ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x);
+               errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y);
+               errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z);
                if (!errors)
                        break;
        }
@@ -315,7 +315,7 @@ _ao_mpu6000_setup(void)
        /* 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;
 }
@@ -325,6 +325,7 @@ struct ao_mpu6000_sample    ao_mpu6000_current;
 static void
 ao_mpu6000(void)
 {
+       struct ao_mpu6000_sample        sample;
        /* ao_mpu6000_init already grabbed the SPI bus and mutex */
        _ao_mpu6000_setup();
 #if AO_MPU6000_SPI
@@ -335,14 +336,15 @@ ao_mpu6000(void)
 #if AO_MPU6000_SPI
                ao_mpu6000_spi_get();
 #endif
-               _ao_mpu6000_sample(&ao_mpu6000_current);
+               _ao_mpu6000_sample(&sample);
 #if AO_MPU6000_SPI
                ao_mpu6000_spi_put();
-#endif 
-               ao_arch_critical(
-                       AO_DATA_PRESENT(AO_DATA_MPU6000);
-                       AO_DATA_WAIT();
-                       );
+#endif
+               ao_arch_block_interrupts();
+               ao_mpu6000_current = sample;
+               AO_DATA_PRESENT(AO_DATA_MPU6000);
+               AO_DATA_WAIT();
+               ao_arch_release_interrupts();
        }
 }
 
@@ -351,16 +353,13 @@ static struct ao_task ao_mpu6000_task;
 static void
 ao_mpu6000_show(void)
 {
-       struct ao_data  sample;
-
-       ao_data_get(&sample);
        printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d\n",
-               sample.mpu6000.accel_x,
-               sample.mpu6000.accel_y,
-               sample.mpu6000.accel_z,
-               sample.mpu6000.gyro_x,
-               sample.mpu6000.gyro_y,
-               sample.mpu6000.gyro_z);
+               ao_mpu6000_current.accel_x,
+               ao_mpu6000_current.accel_y,
+               ao_mpu6000_current.accel_z,
+               ao_mpu6000_current.gyro_x,
+               ao_mpu6000_current.gyro_y,
+               ao_mpu6000_current.gyro_z);
 }
 
 static const struct ao_cmds ao_mpu6000_cmds[] = {
@@ -374,7 +373,7 @@ ao_mpu6000_init(void)
        ao_mpu6000_configured = 0;
 
        ao_add_task(&ao_mpu6000_task, ao_mpu6000, "mpu6000");
-       
+
 #if AO_MPU6000_SPI
        ao_spi_init_cs(AO_MPU6000_SPI_CS_PORT, (1 << AO_MPU6000_SPI_CS_PIN));
 
@@ -386,7 +385,7 @@ ao_mpu6000_init(void)
        ao_cur_task = &ao_mpu6000_task;
        ao_spi_get(AO_MPU6000_SPI_BUS, AO_SPI_SPEED_1MHz);
        ao_cur_task = NULL;
-#endif 
+#endif
 
        ao_cmd_register(&ao_mpu6000_cmds[0]);
 }