altos: Initialize MPU6000 CS pin for SPI mode

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

diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c
index 6d47482cc0c31bac633cf66014e5d9dd57bdf16e..fc768cc9ae1cdc166f4add87866d4e505df101f9 100644 (file)
--- a/src/drivers/ao_mpu6000.c
+++ b/src/drivers/ao_mpu6000.c
@@ -19,50 +19,76 @@
 #include <ao_mpu6000.h>
 #include <ao_exti.h>
 
+#if HAS_MPU6000
+
 static uint8_t ao_mpu6000_wake;
 static uint8_t ao_mpu6000_configured;
 
-static void
-ao_mpu6000_write(uint8_t addr, uint8_t *data, uint8_t len)
-{
-       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(data, len, AO_MPU6000_I2C_INDEX, TRUE);
-       ao_i2c_put(AO_MPU6000_I2C_INDEX);
-}
+#define ao_mpu6000_spi_get()   ao_spi_get_bit(AO_MPU6000_SPI_CS_PORT,  \
+                                              AO_MPU6000_SPI_CS_PIN,   \
+                                              AO_MPU6000_SPI_CS,       \
+                                              AO_MPU6000_SPI_BUS,      \
+                                              AO_SPI_SPEED_1MHz)
+
+#define ao_mpu6000_spi_put()   ao_spi_put_bit(AO_MPU6000_SPI_CS_PORT,  \
+                                              AO_MPU6000_SPI_CS_PIN,   \
+                                              AO_MPU6000_SPI_CS,       \
+                                              AO_MPU6000_SPI_BUS)
+
 
 static void
 ao_mpu6000_reg_write(uint8_t addr, uint8_t value)
 {
        uint8_t d[2] = { addr, value };
+#ifdef AO_MPU6000_I2C_INDEX
        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_put(AO_MPU6000_I2C_INDEX);
+#else
+       ao_mpu6000_spi_get();
+       ao_spi_send(d, 2, AO_MPU6000_SPI_BUS);
+       ao_mpu6000_spi_put();
+#endif
 }
 
 static void
 ao_mpu6000_read(uint8_t addr, void *data, uint8_t len)
 {
+#ifdef AO_MPU6000_I2C_INDEX
        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_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_READ);
        ao_i2c_recv(data, len, AO_MPU6000_I2C_INDEX, TRUE);
        ao_i2c_put(AO_MPU6000_I2C_INDEX);
+#else
+       addr |= 0x80;
+       ao_mpu6000_spi_get();
+       ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
+       ao_spi_recv(data, len, AO_MPU6000_SPI_BUS);
+       ao_mpu6000_spi_put();
+#endif
 }
 
 static uint8_t
 ao_mpu6000_reg_read(uint8_t addr)
 {
        uint8_t value;
+#ifdef AO_MPU6000_I2C_INDEX
        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_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_READ);
        ao_i2c_recv(&value, 1, AO_MPU6000_I2C_INDEX, TRUE);
        ao_i2c_put(AO_MPU6000_I2C_INDEX);
+#else
+       addr |= 0x80;
+       ao_mpu6000_spi_get();
+       ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS);
+       ao_spi_recv(&value, 1, AO_MPU6000_SPI_BUS);
+       ao_mpu6000_spi_put();
+#endif
        return value;
 }
 
@@ -302,5 +328,9 @@ ao_mpu6000_init(void)
        ao_mpu6000_configured = 0;
 
        ao_add_task(&ao_mpu6000_task, ao_mpu6000, "mpu6000");
+#ifndef AO_MPU6000_I2C_INDEX
+       ao_spi_init_cs(AO_MPU6000_SPI_CS_PORT, (1 << AO_MPU6000_SPI_CS_PIN));
+#endif 
        ao_cmd_register(&ao_mpu6000_cmds[0]);
 }
+#endif