Aspirin2 Driver Via DMA accelerated SPI

conf/airframes/CDW/LisaAspirin2.xml

@@ -230,7 +230,7 @@
       <configure name="PERIODIC_FREQUENCY" value="120"/>
       <configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
       <configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
-      <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" />
+<!--       <define name="AHRS_TRIGGERED_ATTITUDE_LOOP" /> -->
 
       <configure name="AHRS_ALIGNER_LED" value="1"/>
       <configure name="CPU_LED" value="1"/>

sw/airborne/arch/stm32/mcu_periph/spi_arch.c

@@ -184,6 +184,7 @@ void dma1_c4_irq_handler(void)
   DMA_Cmd(DMA1_Channel5, DISABLE);
 
   slave0->status = SPITransSuccess;
+  *(slave0->ready) = 1;
 }
 
 

sw/airborne/mcu_periph/spi.h

@@ -43,6 +43,7 @@ enum SPITransactionStatus {
 struct spi_transaction {
   volatile uint8_t* mosi_buf;
   volatile uint8_t* miso_buf;
+  volatile uint8_t* ready;
   uint8_t length;
   volatile enum SPITransactionStatus status;
 };

sw/airborne/peripherals/mpu60X0.h

@@ -5,6 +5,8 @@
 #define MPU60X0_ADDR            0xD0
 #define MPU60X0_ADDR_ALT        0xD2
 
+#define MPU60X0_SPI_READ	0x80
+
 // Power and Interface
 #define MPU60X0_REG_AUX_VDDIO		0x01	// Must be set to 0 on MPU6000
 #define MPU60X0_REG_USER_CTRL		0x6A

sw/airborne/subsystems/imu/imu_aspirin2.c

@@ -12,10 +12,11 @@ struct ImuAspirin2 imu_aspirin2;
 
 struct spi_transaction aspirin2_mpu60x0;
 
-/*
+
 
 // initialize peripherals
-static void configure_gyro(void);
+static void configure(void);
+/*
 static void configure_accel(void);
 //static void configure_mag(void);
 
@@ -28,10 +29,10 @@ void imu_impl_init(void) {
 
   aspirin2_mpu60x0.mosi_buf = imu_aspirin2.imu_tx_buf;
   aspirin2_mpu60x0.miso_buf = imu_aspirin2.imu_rx_buf;
+  aspirin2_mpu60x0.ready = &(imu_aspirin2.imu_available);
   aspirin2_mpu60x0.length = 2;
 
 //  imu_aspirin2_arch_init();
-//  spi_arch_int_enable();
 
 }
 
@@ -39,61 +40,128 @@ void imu_impl_init(void) {
 void imu_periodic(void) 
 {
 
-  imu_aspirin2.imu_tx_buf[0] = MPU60X0_REG_WHO_AM_I + 0x80;
+  if (imu_aspirin2.status == Aspirin2StatusUninit) 
-  imu_aspirin2.imu_tx_buf[1] = 0x00;
+  {
+    configure();
+    // imu_aspirin_arch_int_enable();
+    imu_aspirin2.status = Aspirin2StatusIdle;
 
-  spi_rw(&aspirin2_mpu60x0);
+    aspirin2_mpu60x0.length = 22;
+    aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ;
+    {
+      for (int i=1;i<aspirin2_mpu60x0.length;i++)
+        aspirin2_mpu60x0.mosi_buf[i] = 0;
+    }
+  }
+  else
+  {
+
+    // imu_aspirin2.imu_tx_buf[0] = MPU60X0_REG_WHO_AM_I + MPU60X0_SPI_READ;
+    // imu_aspirin2.imu_tx_buf[1] = 0x00;
+
+    spi_rw(&aspirin2_mpu60x0);
 
 /*
-  hmc5843_periodic();
-  if (imu_aspirin.status == AspirinStatusUninit) {
-    configure_gyro();
-    configure_accel();
-    imu_aspirin_arch_int_enable();
-    imu_aspirin.status = AspirinStatusIdle;
-  } else {
-    imu_aspirin.gyro_available_blaaa = TRUE;
     imu_aspirin.time_since_last_reading++;
     imu_aspirin.time_since_last_accel_reading++;
-    if (imu_aspirin.time_since_last_accel_reading > ASPIRIN_ACCEL_TIMEOUT) {
+    if (imu_aspirin.time_since_last_accel_reading > ASPIRIN_ACCEL_TIMEOUT) 
+    {
       configure_accel();
       imu_aspirin.time_since_last_accel_reading=0;
     }
+*/
   } 
+}
+
+static void configure(void) 
+{
+  aspirin2_mpu60x0.length = 2;
+
+  ///////////////////
+  // Configure power:
+
+  // MPU60X0_REG_AUX_VDDIO = 0 (good on startup)
+
+  // MPU60X0_REG_USER_CTRL:
+  // -Enable Aux I2C Master Mode
+  // -Enable SPI
+
+  // MPU60X0_REG_PWR_MGMT_1
+  // -switch to gyroX clock
+  aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_PWR_MGMT_1;
+  aspirin2_mpu60x0.mosi_buf[1] = 0x01;
+  spi_rw(&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status != SPITransSuccess);
+
+  // MPU60X0_REG_PWR_MGMT_2: Nothing should be in standby: default OK
+
+  /////////////////////////
+  // Measurement Settings
+
+  // MPU60X0_REG_CONFIG
+  // -ext sync on gyro X (bit 3->6)
+  // -digital low pass filter: 1kHz sampling of gyro/acc with 44Hz bandwidth: since reading is at 100Hz
+#if PERIODIC_FREQUENCY == 60
+#else
+#  if PERIODIC_FREQUENCY == 120
+#  else
+#  error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
+#  endif
+#endif
+  aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_CONFIG;
+  aspirin2_mpu60x0.mosi_buf[1] = (2 << 3) | (3 << 0);
+  spi_rw(&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status != SPITransSuccess);
+
+  // MPU60X0_REG_SMPLRT_DIV
+  // -100Hz output = 1kHz / (9 + 1)
+  aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_SMPLRT_DIV;
+  aspirin2_mpu60x0.mosi_buf[1] = 9;
+  spi_rw(&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status != SPITransSuccess);
+
+  // MPU60X0_REG_GYRO_CONFIG
+  // -2000deg/sec
+  aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_GYRO_CONFIG;
+  aspirin2_mpu60x0.mosi_buf[1] = (3<<3);
+  spi_rw(&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status != SPITransSuccess);
+
+  // MPU60X0_REG_ACCEL_CONFIG
+  // 16g, no HPFL
+  aspirin2_mpu60x0.mosi_buf[0] = MPU60X0_REG_ACCEL_CONFIG;
+  aspirin2_mpu60x0.mosi_buf[1] = (3<<3);
+  spi_rw(&aspirin2_mpu60x0);
+    while(aspirin2_mpu60x0.status != SPITransSuccess);
+
+/*
+  struct i2c_transaction t;
+  t.type = I2CTransTx;
+  t.slave_addr = ITG3200_ADDR;
+  // set gyro range to 2000deg/s and low pass at 256Hz
+  t.mosi_buf[0] = ITG3200_REG_DLPF_FS;
+  t.mosi_buf[1] = (0x03<<3);
+  t.len_w = 2;
+  send_i2c_msg_with_retry(&t);
+  // set sample rate to 533Hz 
+  t.mosi_buf[0] = ITG3200_REG_SMPLRT_DIV;
+  t.mosi_buf[1] = 0x0E;
+  send_i2c_msg_with_retry(&t);
+  // switch to gyroX clock 
+  t.mosi_buf[0] = ITG3200_REG_PWR_MGM;
+  t.mosi_buf[1] = 0x01;
+  send_i2c_msg_with_retry(&t);
+  // enable interrupt on data ready, idle high, latch until read any register
+  t.mosi_buf[0] = ITG3200_REG_INT_CFG;
+  t.mosi_buf[1] = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7);
+  send_i2c_msg_with_retry(&t);
 */
 }
 
 
 /*
-// sends a serie of I2C commands to configure the ITG3200 gyro 
+static void configure_accel(void) 
-static void configure_gyro(void) {
+{
-  struct i2c_transaction t;
-  t.type = I2CTransTx;
-  t.slave_addr = ITG3200_ADDR;
-  // set gyro range to 2000deg/s and low pass at 256Hz
-  t.buf[0] = ITG3200_REG_DLPF_FS;
-  t.buf[1] = (0x03<<3);
-  t.len_w = 2;
-  send_i2c_msg_with_retry(&t);
-  // set sample rate to 533Hz 
-  t.buf[0] = ITG3200_REG_SMPLRT_DIV;
-  t.buf[1] = 0x0E;
-  send_i2c_msg_with_retry(&t);
-  // switch to gyroX clock 
-  t.buf[0] = ITG3200_REG_PWR_MGM;
-  t.buf[1] = 0x01;
-  send_i2c_msg_with_retry(&t);
-  // enable interrupt on data ready, idle high, latch until read any register
-  t.buf[0] = ITG3200_REG_INT_CFG;
-  t.buf[1] = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7);
-  send_i2c_msg_with_retry(&t);
-
-}
-
-
-
-static void configure_accel(void) {
-
   // set data rate to 800Hz
   adxl345_write_to_reg(ADXL345_REG_BW_RATE, 0x0D);
   // switch to measurememnt mode 
@@ -108,7 +176,6 @@ static void configure_accel(void) {
   adxl345_start_reading_data();
 
 }
-
 */
 
 

sw/airborne/subsystems/imu/imu_aspirin2.h

@@ -26,11 +26,7 @@
 
 #include "generated/airframe.h"
 #include "subsystems/imu.h"
-
+#include "led.h"
-#include "mcu_periph/i2c.h"
-#include "peripherals/itg3200.h"
-#include "peripherals/hmc5843.h"
-#include "peripherals/adxl345.h"
 
 
 #ifdef IMU_ASPIRIN_VERSION_2_0
@@ -95,53 +91,69 @@ extern struct ImuAspirin2 imu_aspirin2;
 /* must be implemented by underlying architecture */
 extern void imu_aspirin2_arch_init(void);
 
-/*
+
-static inline void gyro_copy_i2c(void)
+static inline void imu_from_buff(void)
 {
-  int16_t gp = imu_aspirin.i2c_trans_gyro.buf[0]<<8 | imu_aspirin.i2c_trans_gyro.buf[1];
+  int32_t x, y, z;
-  int16_t gq = imu_aspirin.i2c_trans_gyro.buf[2]<<8 | imu_aspirin.i2c_trans_gyro.buf[3];
+
-  int16_t gr = imu_aspirin.i2c_trans_gyro.buf[4]<<8 | imu_aspirin.i2c_trans_gyro.buf[5];
+
-  RATES_ASSIGN(imu.gyro_unscaled, gp, gq, gr);
+  // If the itg3200 I2C transaction has succeeded: convert the data
+#define MPU_OFFSET_GYRO 10
+  x = (int16_t) ((imu_aspirin2.imu_rx_buf[0+MPU_OFFSET_GYRO] << 8) | imu_aspirin2.imu_rx_buf[1+MPU_OFFSET_GYRO]);
+  y = (int16_t) ((imu_aspirin2.imu_rx_buf[2+MPU_OFFSET_GYRO] << 8) | imu_aspirin2.imu_rx_buf[3+MPU_OFFSET_GYRO]);
+  z = (int16_t) ((imu_aspirin2.imu_rx_buf[4+MPU_OFFSET_GYRO] << 8) | imu_aspirin2.imu_rx_buf[5+MPU_OFFSET_GYRO]);
+
+  RATES_ASSIGN(imu.gyro_unscaled, x, y, z);
+
+#define MPU_OFFSET_ACC 2
+  x = (int16_t) ((imu_aspirin2.imu_rx_buf[0+MPU_OFFSET_ACC] << 8) | imu_aspirin2.imu_rx_buf[1+MPU_OFFSET_ACC]);
+  y = (int16_t) ((imu_aspirin2.imu_rx_buf[2+MPU_OFFSET_ACC] << 8) | imu_aspirin2.imu_rx_buf[3+MPU_OFFSET_ACC]);
+  z = (int16_t) ((imu_aspirin2.imu_rx_buf[4+MPU_OFFSET_ACC] << 8) | imu_aspirin2.imu_rx_buf[5+MPU_OFFSET_ACC]);
+
+  VECT3_ASSIGN(imu.accel_unscaled, x, y, z);
+
+  // Is this is new data
+#define MPU_OFFSET_STATUS 1
+  if (imu_aspirin2.imu_rx_buf[MPU_OFFSET_STATUS] & 0x01)
+  {
+    //gyr_valid = TRUE;
+    //acc_valid = TRUE;
+  }
+  else
+  {
+  }
 }
 
-static inline void accel_copy_spi(void)
-{
-  const int16_t ax = imu_aspirin.accel_rx_buf[1] | (imu_aspirin.accel_rx_buf[2]<<8);
-  const int16_t ay = imu_aspirin.accel_rx_buf[3] | (imu_aspirin.accel_rx_buf[4]<<8);
-  const int16_t az = imu_aspirin.accel_rx_buf[5] | (imu_aspirin.accel_rx_buf[6]<<8);
-  VECT3_ASSIGN(imu.accel_unscaled, ax, ay, az);
-}
-
-
-static inline void imu_gyro_event(void (* _gyro_handler)(void))
-{
-
-}
-*/
 
 static inline void imu_aspirin2_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
 {
+  LED_TOGGLE(4);
+
   if (imu_aspirin2.status == Aspirin2StatusUninit) return;
 
-  imu_aspirin2_arch_int_disable();
+  // imu_aspirin2_arch_int_disable();
-  if (imu_aspirin2.imu_available) {
+
+  if (imu_aspirin2.imu_available) 
+  {
     imu_aspirin2.time_since_last_reading = 0;
     imu_aspirin2.imu_available = FALSE;
-    //accel_copy_spi();
+    imu_from_buff();
+
+    _gyro_handler();
     _accel_handler();
   }
-  imu_aspirin2_arch_int_enable();
+  // imu_aspirin2_arch_int_enable();
 
   // Reset everything if we've been waiting too long
-  if (imu_aspirin2.time_since_last_reading > ASPIRIN2_TIMEOUT) {
+  //if (imu_aspirin2.time_since_last_reading > ASPIRIN2_TIMEOUT) {
-    imu_aspirin2.time_since_last_reading = 0;
+  //  imu_aspirin2.time_since_last_reading = 0;
-    return;
+  //  return;
-  }
+  //}
 
 }
 
-#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) do {		\
+#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
   imu_aspirin2_event(_gyro_handler, _accel_handler, _mag_handler); \
-} while(0);
+}
 
 #endif /* IMU_ASPIRIN_H */