X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fdrivers%2Fao_mpu6000.c;h=c894239ec0e54ae0934781886c44a759e8bcc38b;hb=refs%2Fheads%2Fmaster;hp=650407ad8b679d8f98478e82e84da422b1c3bdce;hpb=1085ec5d57e0ed5d132f2bbdac1a0b6a32c0ab4a;p=fw%2Faltos diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c index 650407ad..e5594a66 100644 --- a/src/drivers/ao_mpu6000.c +++ b/src/drivers/ao_mpu6000.c @@ -32,7 +32,9 @@ static uint8_t ao_mpu6000_configured; #if AO_MPU6000_SPI -#define ao_mpu6000_spi_get() ao_spi_get(AO_MPU6000_SPI_BUS, AO_SPI_SPEED_1MHz) +#define AO_MPU6000_SPI_SPEED ao_spi_speed(AO_MPU6000_SPI_BUS, 1000000) /* 1Mhz for all register access */ + +#define ao_mpu6000_spi_get() ao_spi_get(AO_MPU6000_SPI_BUS, AO_MPU6000_SPI_SPEED) #define ao_mpu6000_spi_put() ao_spi_put(AO_MPU6000_SPI_BUS) #define ao_mpu6000_spi_start() ao_spi_set_cs(AO_MPU6000_SPI_CS_PORT, \ @@ -55,7 +57,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 +74,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 +94,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; @@ -111,7 +113,7 @@ _ao_mpu6000_sample(struct ao_mpu6000_sample *sample) /* byte swap */ while (i--) { uint16_t t = *d; - *d++ = (t >> 8) | (t << 8); + *d++ = (uint16_t) ((t >> 8) | (t << 8)); } #endif } @@ -162,6 +164,8 @@ ao_mpu6000_gyro_check(int16_t normal, int16_t test) return 0; } +static uint8_t mpu_id; + static void _ao_mpu6000_wait_alive(void) { @@ -170,11 +174,11 @@ _ao_mpu6000_wait_alive(void) /* Wait for the chip to wake up */ for (i = 0; i < 30; i++) { ao_delay(AO_MS_TO_TICKS(100)); - if (_ao_mpu6000_reg_read(MPU6000_WHO_AM_I) == 0x68) - break; + mpu_id = _ao_mpu6000_reg_read(MPU6000_WHO_AM_I); + if (mpu_id == 0x68) + return; } - if (i == 30) - ao_panic(AO_PANIC_SELF_TEST_MPU6000); + AO_SENSOR_ERROR(AO_DATA_MPU6000); } #define ST_TRIES 10 @@ -192,7 +196,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,7 +296,7 @@ _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); 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); @@ -305,7 +309,7 @@ _ao_mpu6000_setup(void) } if (st_tries == ST_TRIES) - ao_sensor_errors = 1; + AO_SENSOR_ERROR(AO_DATA_MPU6000); /* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */ _ao_mpu6000_reg_write(MPU6000_CONFIG, @@ -315,7 +319,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 +329,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 +340,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 +357,23 @@ static struct ao_task ao_mpu6000_task; static void ao_mpu6000_show(void) { - struct ao_data sample; - - ao_data_get(&sample); +#ifdef AO_LOG_NORMALIZED + printf ("MPU6000: %7d %7d %7d %7d %7d %7d\n", + ao_mpu6000_along(&ao_mpu6000_current), + ao_mpu6000_across(&ao_mpu6000_current), + ao_mpu6000_through(&ao_mpu6000_current), + ao_mpu6000_roll(&ao_mpu6000_current), + ao_mpu6000_pitch(&ao_mpu6000_current), + ao_mpu6000_yaw(&ao_mpu6000_current)); +#else 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); +#endif } static const struct ao_cmds ao_mpu6000_cmds[] = { @@ -374,7 +387,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)); @@ -384,9 +397,9 @@ ao_mpu6000_init(void) */ ao_cur_task = &ao_mpu6000_task; - ao_spi_get(AO_MPU6000_SPI_BUS, AO_SPI_SPEED_1MHz); + ao_mpu6000_spi_get(); ao_cur_task = NULL; -#endif +#endif ao_cmd_register(&ao_mpu6000_cmds[0]); }