X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_mpu6000.c;h=8c85ab01fd82c00041128a9147e3f6a26f2caa39;hp=f8ce7346c7149d690228c57445607382ce35c05b;hb=684741765117611b7d666efbdfafd87c6199752c;hpb=ee4279613b4757453d0d8f8afc06037c61eeb520 diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c index f8ce7346..8c85ab01 100644 --- 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]); }