[peripheral] update mpu9250 driver, imu and module as well

paparazzi · Nov 17, 2014 · 18cbec7 · 18cbec7
1 parent 0d3e40a
commit 18cbec7
Show file tree

Hide file tree

Showing 10 changed files with 106 additions and 67 deletions.
diff --git a/...es/subsystems/shared/imu_mpu9250.makefile → ...ubsystems/shared/imu_mpu9250_i2c.makefile b/...es/subsystems/shared/imu_mpu9250.makefile → ...ubsystems/shared/imu_mpu9250_i2c.makefile
@@ -8,14 +8,14 @@ ifeq ($(TARGET), ap)
   IMU_MPU9250_CFLAGS  = -DUSE_IMU
 endif
 
-IMU_MPU9250_CFLAGS += -DIMU_TYPE_H=\"imu/imu_mpu9250.h\"
+IMU_MPU9250_CFLAGS += -DIMU_TYPE_H=\"imu/imu_mpu9250_i2c.h\"
 IMU_MPU9250_SRCS    = $(SRC_SUBSYSTEMS)/imu.c
-IMU_MPU9250_SRCS   += $(SRC_SUBSYSTEMS)/imu/imu_mpu9250.c
+IMU_MPU9250_SRCS   += $(SRC_SUBSYSTEMS)/imu/imu_mpu9250_i2c.c
 IMU_MPU9250_SRCS   += peripherals/mpu9250.c
 IMU_MPU9250_SRCS   += peripherals/mpu9250_i2c.c
 
 # Magnetometer
-#IMU_MPU9250_SRCS   += peripherals/ak8963.c
+IMU_MPU9250_SRCS   += peripherals/ak8963.c
 
 
 # set default i2c bus

diff --git a/conf/modules/imu_mpu9250.xml b/conf/modules/imu_mpu9250.xml
@@ -20,6 +20,7 @@
     <file name="imu_mpu9250.c"/>
     <file name="mpu9250.c" dir="peripherals"/>
     <file name="mpu9250_i2c.c" dir="peripherals"/>
+    <file name="ak8963.c" dir="peripherals"/>
     <define name="USE_I2C"/>
     <define name="IMU_MPU9250_I2C_DEV" value="$(IMU_MPU9250_I2C_DEV)"/>
   </makefile>

diff --git a/sw/airborne/modules/sensors/imu_mpu9250.c b/sw/airborne/modules/sensors/imu_mpu9250.c
@@ -67,5 +67,12 @@ void imu_mpu9250_report(void)
     (int32_t)(mpu9250.data_rates.rates.r)
   };
   DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &rates.p, &rates.q, &rates.r);
+
+  struct Int32Vect3 mag = {
+    (int32_t)(mpu9250.akm.data.vect.x),
+    (int32_t)(mpu9250.akm.data.vect.y),
+    (int32_t)(mpu9250.akm.data.vect.z)
+  };
+  DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z);
 }
 
diff --git a/sw/airborne/peripherals/mpu9250.c b/sw/airborne/peripherals/mpu9250.c
@@ -81,7 +81,7 @@ void mpu9250_send_config(Mpu9250ConfigSet mpu_set, void* mpu, struct Mpu9250Conf
       break;
     case MPU9250_CONF_DLPF_ACCEL:
       /* configure digital low pass filter fir accelerometer */
-      mpu_set(mpu, MPU9250_REG_CONFIG, config->dlpf_accel_cfg);
+      mpu_set(mpu, MPU9250_REG_ACCEL_CONFIG_2, config->dlpf_accel_cfg);
       config->init_status++;
       break;
     case MPU9250_CONF_GYRO:

diff --git a/sw/airborne/peripherals/mpu9250.h b/sw/airborne/peripherals/mpu9250.h
@@ -35,9 +35,9 @@
 /// Default sample rate divider
 #define MPU9250_DEFAULT_SMPLRT_DIV 0
 /// Default gyro full scale range +- 2000°/s
-#define MPU9250_DEFAULT_FS_SEL MPU9250_GYRO_RANGE_2000
+#define MPU9250_DEFAULT_FS_SEL MPU9250_GYRO_RANGE_1000
 /// Default accel full scale range +- 16g
-#define MPU9250_DEFAULT_AFS_SEL MPU9250_ACCEL_RANGE_16G
+#define MPU9250_DEFAULT_AFS_SEL MPU9250_ACCEL_RANGE_8G
 /// Default internal sampling (1kHz, 42Hz LP Bandwidth)
 #define MPU9250_DEFAULT_DLPF_ACCEL_CFG MPU9250_DLPF_ACCEL_41HZ
 /// Default internal sampling (1kHz, 42Hz LP Bandwidth)

diff --git a/sw/airborne/peripherals/mpu9250_i2c.c b/sw/airborne/peripherals/mpu9250_i2c.c
@@ -27,6 +27,8 @@
 
 #include "peripherals/mpu9250_i2c.h"
 
+bool_t imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set __attribute__ ((unused)), void* mpu __attribute__ ((unused)));
+
 void mpu9250_i2c_init(struct Mpu9250_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr)
 {
   /* set i2c_peripheral */
@@ -44,13 +46,16 @@ void mpu9250_i2c_init(struct Mpu9250_I2c *mpu, struct i2c_periph *i2c_p, uint8_t
   mpu->config.initialized = FALSE;
   mpu->config.init_status = MPU9250_CONF_UNINIT;
 
+  /* "internal" ak8963 magnetometer */
+  ak8963_init(&mpu->akm, i2c_p, MPU9250_MAG_ADDR);
   /* mag is declared as slave to call the configure function,
    * regardless if it is an actual MPU slave or passthrough
    */
-  //mpu->config.nb_slaves = 1;
+  mpu->config.nb_slaves = 1;
   /* set callback function to configure mag */
-  //mpu->config.slaves[0].configure = &imu_mpu9250_configure_mag_slave;
-
+  mpu->config.slaves[0].configure = &imu_mpu9250_configure_mag_slave;
+  /* read the mag directly for now */
+  mpu->config.i2c_bypass = TRUE;
 
   mpu->slave_init_status = MPU9250_I2C_CONF_UNINIT;
 }
@@ -80,6 +85,12 @@ void mpu9250_i2c_read(struct Mpu9250_I2c *mpu)
     /* set read bit and multiple byte bit, then address */
     mpu->i2c_trans.buf[0] = MPU9250_REG_INT_STATUS;
     i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, mpu->config.nb_bytes);
+    /* read mag */
+#ifdef MPU9250_MAG_PRESCALER
+    RunOnceEvery(MPU9250_MAG_PRESCALER, ak8963_read(&mpu->akm));
+#else
+    ak8963_read(&mpu->akm);
+#endif
   }
 }
 
@@ -125,19 +136,23 @@ void mpu9250_i2c_event(struct Mpu9250_I2c *mpu)
         break;
     }
   }
+  // Ak8963 event function
+  ak8963_event(&mpu->akm);
 }
 
 /** callback function to configure ak8963 mag
  * @return TRUE if mag configuration finished
  */
-//bool_t imu_mpu9250_configure_mag_slave(Mpu60x0ConfigSet mpu_set __attribute__ ((unused)), void* mpu __attribute__ ((unused)))
-//{
-//  ak8963_start_configure(&imu_mpu9250.akm);
-//  if (imu_mpu9250.akm.initialized)
-//    return TRUE;
-//  else
-//    return FALSE;
-//}
+bool_t imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set __attribute__ ((unused)), void* mpu)
+{
+  struct Mpu9250_I2c* mpu_i2c = (struct Mpu9250_I2c*)(mpu);
+
+  ak8963_configure(&mpu_i2c->akm);
+  if (mpu_i2c->akm.initialized)
+    return TRUE;
+  else
+    return FALSE;
+}
 
 /** @todo: only one slave so far. */
 bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void* mpu)

diff --git a/sw/airborne/peripherals/mpu9250_i2c.h b/sw/airborne/peripherals/mpu9250_i2c.h
@@ -34,6 +34,7 @@
 
 /* Include common MPU9250 options and definitions */
 #include "peripherals/mpu9250.h"
+#include "peripherals/ak8963.h"
 
 
 #define MPU9250_BUFFER_EXT_LEN 16
@@ -66,6 +67,7 @@ struct Mpu9250_I2c {
   uint8_t data_ext[MPU9250_BUFFER_EXT_LEN];
   struct Mpu9250Config config;
   enum Mpu9250I2cSlaveInitStatus slave_init_status;
+  struct Ak8963 akm;                  ///< "internal" magnetometer
 };
 
 // Functions

diff --git a/sw/airborne/peripherals/mpu9250_regs.h b/sw/airborne/peripherals/mpu9250_regs.h
@@ -32,6 +32,8 @@
 #define MPU9250_ADDR                0xD0
 #define MPU9250_ADDR_ALT            0xD2
 
+#define MPU9250_MAG_ADDR            0x18
+
 #define MPU9250_SPI_READ            0x80
 
 // Power and Interface
@@ -43,47 +45,47 @@
 // FIFO
 #define MPU9250_REG_FIFO_EN         0x23
 #define MPU9250_REG_FIFO_COUNT_H    0x72
-#define MPU9250_REG_FIFO_COUNT_L	0x73
-#define MPU9250_REG_FIFO_R_W		0x74
+#define MPU9250_REG_FIFO_COUNT_L	  0x73
+#define MPU9250_REG_FIFO_R_W		    0x74
 
 // Measurement Settings
-#define MPU9250_REG_SMPLRT_DIV        0x19
-#define MPU9250_REG_CONFIG            0x1A
-#define MPU9250_REG_GYRO_CONFIG       0x1B
-#define MPU9250_REG_ACCEL_CONFIG      0x1C
-#define MPU9250_REG_ACCEL_CONFIG_2    0x1D
+#define MPU9250_REG_SMPLRT_DIV      0x19
+#define MPU9250_REG_CONFIG          0x1A
+#define MPU9250_REG_GYRO_CONFIG     0x1B
+#define MPU9250_REG_ACCEL_CONFIG    0x1C
+#define MPU9250_REG_ACCEL_CONFIG_2  0x1D
 
 
 // I2C Slave settings
 #define MPU9250_REG_I2C_MST_CTRL    0x24
 #define MPU9250_REG_I2C_MST_STATUS  0x36
 #define MPU9250_REG_I2C_MST_DELAY   0x67
 // Slave 0
-#define MPU9250_REG_I2C_SLV0_ADDR	0X25	// i2c addr
-#define MPU9250_REG_I2C_SLV0_REG	0X26	// slave reg
-#define MPU9250_REG_I2C_SLV0_CTRL	0X27	// set-bits
-#define MPU9250_REG_I2C_SLV0_DO		0X63	// DO
+#define MPU9250_REG_I2C_SLV0_ADDR	  0X25	// i2c addr
+#define MPU9250_REG_I2C_SLV0_REG	  0X26	// slave reg
+#define MPU9250_REG_I2C_SLV0_CTRL	  0X27	// set-bits
+#define MPU9250_REG_I2C_SLV0_DO		  0X63	// DO
 // Slave 1
-#define MPU9250_REG_I2C_SLV1_ADDR	0X28	// i2c addr
-#define MPU9250_REG_I2C_SLV1_REG	0X29	// slave reg
-#define MPU9250_REG_I2C_SLV1_CTRL	0X2A	// set-bits
-#define MPU9250_REG_I2C_SLV1_DO		0X64	// DO
+#define MPU9250_REG_I2C_SLV1_ADDR	  0X28	// i2c addr
+#define MPU9250_REG_I2C_SLV1_REG	  0X29	// slave reg
+#define MPU9250_REG_I2C_SLV1_CTRL	  0X2A	// set-bits
+#define MPU9250_REG_I2C_SLV1_DO		  0X64	// DO
 // Slave 2
-#define MPU9250_REG_I2C_SLV2_ADDR	0X2B	// i2c addr
-#define MPU9250_REG_I2C_SLV2_REG	0X2C	// slave reg
-#define MPU9250_REG_I2C_SLV2_CTRL	0X2D	// set-bits
-#define MPU9250_REG_I2C_SLV2_DO		0X65	// DO
+#define MPU9250_REG_I2C_SLV2_ADDR	  0X2B	// i2c addr
+#define MPU9250_REG_I2C_SLV2_REG	  0X2C	// slave reg
+#define MPU9250_REG_I2C_SLV2_CTRL	  0X2D	// set-bits
+#define MPU9250_REG_I2C_SLV2_DO		  0X65	// DO
 // Slave 3
-#define MPU9250_REG_I2C_SLV3_ADDR	0X2E	// i2c addr
-#define MPU9250_REG_I2C_SLV3_REG	0X2F	// slave reg
-#define MPU9250_REG_I2C_SLV3_CTRL	0X30	// set-bits
-#define MPU9250_REG_I2C_SLV3_DO		0X66	// DO
+#define MPU9250_REG_I2C_SLV3_ADDR	  0X2E	// i2c addr
+#define MPU9250_REG_I2C_SLV3_REG	  0X2F	// slave reg
+#define MPU9250_REG_I2C_SLV3_CTRL	  0X30	// set-bits
+#define MPU9250_REG_I2C_SLV3_DO		  0X66	// DO
 // Slave 4 - special
-#define MPU9250_REG_I2C_SLV4_ADDR	0X31	// i2c addr
-#define MPU9250_REG_I2C_SLV4_REG	0X32	// slave reg
-#define MPU9250_REG_I2C_SLV4_DO		0X33	// DO
-#define MPU9250_REG_I2C_SLV4_CTRL	0X34	// set-bits
-#define MPU9250_REG_I2C_SLV4_DI		0X35	// DI
+#define MPU9250_REG_I2C_SLV4_ADDR	  0X31	// i2c addr
+#define MPU9250_REG_I2C_SLV4_REG	  0X32	// slave reg
+#define MPU9250_REG_I2C_SLV4_DO		  0X33	// DO
+#define MPU9250_REG_I2C_SLV4_CTRL	  0X34	// set-bits
+#define MPU9250_REG_I2C_SLV4_DI		  0X35	// DI
 
 // Interrupt
 #define MPU9250_REG_INT_PIN_CFG     0x37
@@ -116,7 +118,7 @@
 
 
 #define MPU9250_REG_WHO_AM_I        0x75
-#define MPU9250_WHOAMI_REPLY	    0x68
+#define MPU9250_WHOAMI_REPLY	      0x68
 
 // Bit positions
 #define MPU9250_I2C_BYPASS_EN       1
@@ -126,7 +128,7 @@
 #define MPU9250_I2C_MST_RESET       1
 #define MPU9250_FIFO_RESET          2
 #define MPU9250_I2C_IF_DIS          4
-#define MPU9250_I2C_MST_EN		    5
+#define MPU9250_I2C_MST_EN		      5
 #define MPU9250_FIFO_EN             6
 
 // in MPU9250_REG_I2C_MST_STATUS
@@ -135,7 +137,7 @@
 /** Digital Low Pass Filter Options
  */
 enum Mpu9250DLPFGyro {
-  MPU9250_DLPF__GYRO_250HZ = 0x0,  // internal sampling rate 8kHz
+  MPU9250_DLPF_GYRO_250HZ = 0x0,  // internal sampling rate 8kHz
   MPU9250_DLPF_GYRO_184HZ = 0x1,  // internal sampling rate 1kHz
   MPU9250_DLPF_GYRO_92HZ  = 0x2,
   MPU9250_DLPF_GYRO_41HZ  = 0x3,
@@ -144,7 +146,6 @@ enum Mpu9250DLPFGyro {
   MPU9250_DLPF_GYRO_05HZ  = 0x6
 };
 
-
 enum Mpu9250DLPFAccel {
   MPU9250_DLPF_ACCEL_460HZ = 0x0,  // internal sampling rate 8kHz
   MPU9250_DLPF_ACCEL_184HZ = 0x1,  // internal sampling rate 1kHz
@@ -154,6 +155,7 @@ enum Mpu9250DLPFAccel {
   MPU9250_DLPF_ACCEL_10HZ  = 0x5,
   MPU9250_DLPF_ACCEL_05HZ  = 0x6
 };
+
 /**
  * Selectable gyro range
  */

diff --git a/sw/airborne/subsystems/imu/imu_mpu9250.c → sw/airborne/subsystems/imu/imu_mpu9250_i2c.c b/sw/airborne/subsystems/imu/imu_mpu9250.c → sw/airborne/subsystems/imu/imu_mpu9250_i2c.c
@@ -19,7 +19,7 @@
  */
 
 /**
- * @file subsystems/imu/imu_mpu9250.c
+ * @file subsystems/imu/imu_mpu9250_i2c.c
  *
  * IMU driver for the MPU9250 using I2C
  *
@@ -31,21 +31,29 @@
 
 #if !defined IMU_MPU9250_GYRO_LOWPASS_FILTER && !defined IMU_MPU9250_ACCEL_LOWPASS_FILTER && !defined  IMU_MPU9250_SMPLRT_DIV
 #if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120)
-/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms
- * Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz
+/* Accelerometer: Bandwidth 41Hz, Delay 5.9ms
+ * Gyroscope: Bandwidth 41Hz, Delay 5.9ms sampling 1kHz
+ * Output rate: 100Hz
  */
 #define IMU_MPU9250_GYRO_LOWPASS_FILTER MPU9250_DLPF_GYRO_41HZ
 #define IMU_MPU9250_ACCEL_LOWPASS_FILTER MPU9250_DLPF_ACCEL_41HZ
 #define IMU_MPU9250_SMPLRT_DIV 9
 PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
 #elif PERIODIC_FREQUENCY == 512
-/* Accelerometer: Bandwidth 260Hz, Delay 0ms
- * Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz
+/* Accelerometer: Bandwidth 184Hz, Delay 5.8ms
+ * Gyroscope: Bandwidth 250Hz, Delay 0.97ms sampling 8kHz
+ * Output rate: 2kHz
  */
-#define IMU_MPU9250_GYRO_LOWPASS_FILTER MPU9250_DLPF_GYRO_41HZ
-#define IMU_MPU9250_ACCEL_LOWPASS_FILTER MPU9250_DLPF_ACCEL_41HZ
+#define IMU_MPU9250_GYRO_LOWPASS_FILTER MPU9250_DLPF_GYRO_250HZ
+#define IMU_MPU9250_ACCEL_LOWPASS_FILTER MPU9250_DLPF_ACCEL_184HZ
 #define IMU_MPU9250_SMPLRT_DIV 3
 PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
+#else
+/* By default, don't go too fast */
+#define IMU_MPU9250_SMPLRT_DIV 9
+#define IMU_MPU9250_GYRO_LOWPASS_FILTER MPU9250_DLPF_GYRO_41HZ
+#define IMU_MPU9250_ACCEL_LOWPASS_FILTER MPU9250_DLPF_ACCEL_41HZ
+PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
 #endif
 #endif
 PRINT_CONFIG_VAR(IMU_MPU9250_SMPLRT_DIV)
@@ -122,20 +130,24 @@ void imu_mpu9250_event(void)
       (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]),
       (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z])
     };
-    //struct Int32Vect3 mag = {
-    //  (int32_t)(imu_mpu9250.mpu.data_mag.value[IMU_MPU9250_CHAN_X]),
-    //  (int32_t)(imu_mpu9250.mpu.data_mag.value[IMU_MPU9250_CHAN_Y]),
-    //  (int32_t)(imu_mpu9250.mpu.data_mag.value[IMU_MPU9250_CHAN_Z])
-    //};
     // unscaled vector
     VECT3_COPY(imu.accel_unscaled, accel);
     RATES_COPY(imu.gyro_unscaled, rates);
-    //VECT3_COPY(imu.mag_unscaled, mag);
 
     imu_mpu9250.mpu.data_available = FALSE;
     imu_mpu9250.gyro_valid = TRUE;
     imu_mpu9250.accel_valid = TRUE;
-    //imu_mpu9250.mag_valid = TRUE;
+  }
+  // Test if mag data are updated
+  if (imu_mpu9250.mpu.akm.data_available) {
+    struct Int32Vect3 mag = {
+      (int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_X]),
+      (int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Y]),
+      (int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Z])
+    };
+    VECT3_COPY(imu.mag_unscaled, mag);
+    imu_mpu9250.mag_valid = TRUE;
+    imu_mpu9250.mpu.akm.data_available = FALSE;
   }
 
 }

diff --git a/sw/airborne/subsystems/imu/imu_mpu9250.h → sw/airborne/subsystems/imu/imu_mpu9250_i2c.h b/sw/airborne/subsystems/imu/imu_mpu9250.h → sw/airborne/subsystems/imu/imu_mpu9250_i2c.h
@@ -19,14 +19,14 @@
  */
 
 /**
- * @file subsystems/imu/imu_mpu9250.h
+ * @file subsystems/imu/imu_mpu9250_i2c.h
  *
  * IMU driver for the MPU9250 using I2C
  *
  */
 
-#ifndef IMU_MPU9250_H
-#define IMU_MPU9250_H
+#ifndef IMU_MPU9250_I2C_H
+#define IMU_MPU9250_I2C_H
 
 #include "std.h"
 #include "generated/airframe.h"
@@ -98,4 +98,4 @@ static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler
   }
 }
 
-#endif /* IMU_MPU9250_H */
+#endif /* IMU_MPU9250_I2C_H */