Removed mpu6000 and hmc5833 from px4 imu

conf/airframes/TUDELFT/tudelft_iris_indi.xml

@@ -26,7 +26,11 @@
     <subsystem name="stabilization" type="indi" />
     <subsystem name="ahrs" type="int_cmpl_quat" >
       <define name="AHRS_ICQ_IMU_ID" value="IMU_PX4_ID" />         <!-- Meaning the lsm303 and l3g -->
-      <define name="AHRS_ICQ_MAG_ID" value="IMU_MPU6000_HMC_ID" /> <!-- Meaning the external magnetometer -->
+      <define name="AHRS_ICQ_MAG_ID" value="MAG_HMC58XX_SENDER_ID" />         <!-- Meaning the external hmc-->
+
+      <configure name="USE_MAGNETOMETER" value="TRUE"/>
+      <define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
+
     </subsystem>
     <subsystem name="ins" type="extended" />
     <subsystem name="current_sensor">
@@ -80,23 +84,33 @@
     <load name="air_data.xml" />
     <load name="send_imu_mag_current.xml" />
     <load name="gps_ubx_ucenter.xml" />
+    <load name="mag_hmc58xx.xml">
+      <configure name="MAG_HMC58XX_I2C_DEV" value="i2c1"/>
+      <define name="MODULE_HMC58XX_UPDATE_AHRS" value="TRUE"/>
+    </load>
     <!--  <load name="spektrum_soft_bind.xml"/>-->
   </modules>
+
   <section name="MISC">
     <define name="MilliAmpereOfAdc(adc)" value="((float)adc) * (3.3f / 4096.0f) * (90.0f / 5.0f)" />
     <!-- 100Amp = 2Volt -> 2482,42 tick/100Amp"(0.0402832*adc)" -->
   </section>
   <section name="IMU" prefix="IMU_">
     <!-- replace this with your own calibration -->
-    <define name="MAG_X_NEUTRAL" value="14" />
+    <define name="ACCEL_X_NEUTRAL" value="274"/>
-    <define name="MAG_Y_NEUTRAL" value="116" />
+    <define name="ACCEL_Y_NEUTRAL" value="-43"/>
-    <define name="MAG_Z_NEUTRAL" value="119" />
+    <define name="ACCEL_Z_NEUTRAL" value="1112"/>
-    <define name="MAG_X_SENS" value="5.09245681612" integer="16" />
+    <define name="ACCEL_X_SENS" value="0.625515268183" integer="16"/>
-    <define name="MAG_Y_SENS" value="5.29702744632" integer="16" />
+    <define name="ACCEL_Y_SENS" value="0.609477051286" integer="16"/>
-    <define name="MAG_Z_SENS" value="5.65287938992" integer="16" />
+    <define name="ACCEL_Z_SENS" value="0.62150684522" integer="16"/>
-    <define name="BODY_TO_IMU_PHI" value="180." unit="deg" />
+
-    <define name="BODY_TO_IMU_THETA" value="180." unit="deg" />
+    <define name="MAG_X_NEUTRAL" value="-151"/>
-    <define name="BODY_TO_IMU_PSI" value="90." unit="deg" />
+    <define name="MAG_Y_NEUTRAL" value="-288"/>
+    <define name="MAG_Z_NEUTRAL" value="-3949"/>
+    <define name="MAG_X_SENS" value="0.420984584445" integer="16"/>
+    <define name="MAG_Y_SENS" value="0.447747800597" integer="16"/>
+    <define name="MAG_Z_SENS" value="0.431245648647" integer="16"/>
+
   </section>
   <commands>
     <axis name="PITCH" failsafe_value="0" />

conf/firmwares/subsystems/shared/imu_px4fmu_v2.4.makefile

@@ -2,7 +2,7 @@
 #
 # PX4 Pixhawk IMUconsists of two (internal) IMU's and one internal magneto. Also an optional external magneto
 #
-#MPU6000 +  L3GD20 +  LSM303D + HMC5883
+# L3GD20 +  LSM303D
 
 include $(CFG_SHARED)/spi_master.makefile
 
@@ -11,19 +11,12 @@ IMU_CFLAGS  = -DIMU_TYPE_H=\"imu/imu_px4fmu_v2.4.h\"
 IMU_SRCS    = $(SRC_SUBSYSTEMS)/imu.c
 IMU_SRCS   += $(SRC_SUBSYSTEMS)/imu/imu_px4fmu_v2.4.c
 
-# MPU
-IMU_SRCS   += peripherals/mpu60x0.c
-IMU_SRCS   += peripherals/mpu60x0_spi.c
-
 #L3GD20 gyro
 IMU_SRCS   += peripherals/l3gd20_spi.c
 
 #LSM303D accelero + magneto
 IMU_SRCS   += peripherals/lsm303dlhc_spi.c
 
-# Magnetometer
-IMU_SRCS   += peripherals/hmc58xx.c
-
 # for fixedwing firmware and ap only
 ifeq ($(TARGET), ap)
   IMU_CFLAGS  += -DUSE_IMU
@@ -37,11 +30,6 @@ IMU_SPI_DEV_LOWER=$(shell echo $(IMU_SPI_DEV) | tr A-Z a-z)
 IMU_CFLAGS += -DIMU_SPI_DEV=$(IMU_SPI_DEV_LOWER)
 IMU_CFLAGS += -DUSE_$(IMU_SPI_DEV_UPPER)
 
-#********** MPU6000 ***********
-IMU_MPU_SPI_SLAVE_IDX ?= SPI_SLAVE2
-IMU_CFLAGS += -DIMU_MPU_SPI_SLAVE_IDX=$(IMU_MPU_SPI_SLAVE_IDX)
-IMU_CFLAGS += -DUSE_$(IMU_MPU_SPI_SLAVE_IDX)
-
 #********** L3GD20 ***********
 IMU_L3G_SPI_SLAVE_IDX ?= SPI_SLAVE0
 IMU_CFLAGS += -DIMU_L3G_SPI_SLAVE_IDX=$(IMU_L3G_SPI_SLAVE_IDX)
@@ -52,15 +40,6 @@ IMU_LSM_SPI_SLAVE_IDX ?= SPI_SLAVE1
 IMU_CFLAGS += -DIMU_LSM_SPI_SLAVE_IDX=$(IMU_LSM_SPI_SLAVE_IDX)
 IMU_CFLAGS += -DUSE_$(IMU_LSM_SPI_SLAVE_IDX)
 
-#********** HMC5883 ***********
-IMU_HMC_I2C_DEV ?= i2c1
-
-# convert i2cx to upper/lower case
-IMU_HMC_I2C_DEV_UPPER=$(shell echo $(IMU_HMC_I2C_DEV) | tr a-z A-Z)
-IMU_HMC_I2C_DEV_LOWER=$(shell echo $(IMU_HMC_I2C_DEV) | tr A-Z a-z)
-IMU_CFLAGS += -DIMU_HMC_I2C_DEV=$(IMU_HMC_I2C_DEV_LOWER)
-IMU_CFLAGS += -DUSE_$(IMU_HMC_I2C_DEV_UPPER)
-
 # add it for all targets except sim, fbw and nps
 ifeq (,$(findstring $(TARGET),sim fbw nps))
 $(TARGET).CFLAGS += $(IMU_CFLAGS)

sw/airborne/peripherals/l3gd20_regs.h

@@ -80,7 +80,7 @@ enum L3gd20FullScale {
   L3GD20_FS_250dps = 0,
   L3GD20_FS_500dps = 1,
   L3GD20_FS_2000dps = 2,
-  L3GD20_FS_2000dps2 = 3,
+  L3GD20_FS_2000dps2 = 3, //yep, the same as L3GD20_FS_2000dps
 };
 
 

sw/airborne/peripherals/lsm303dlhc_spi.c

@@ -35,7 +35,7 @@
 #endif
 
 #ifndef LSM303DLHC_DEFAULT_AFS
-#define LSM303DLHC_DEFAULT_AFS 0x00 // acc +- 2G
+#define LSM303DLHC_DEFAULT_AFS (0x04 <<3) // acc +- 16G
 #endif
 
 #ifndef LSM303DLHC_DEFAULT_MODR

sw/airborne/subsystems/imu/imu_px4fmu_v2.4.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2013-2015 Felix Ruess <felix.ruess@gmail.com>
+ * Copyright (C) 2013-2016 the paparazzi team
  *
  * This file is part of paparazzi.
  *
@@ -19,74 +19,29 @@
  * Boston, MA 02111-1307, USA.
  */
 
+
 /**
  * @file subsystems/imu/imu_px4fmu_v2.4.h
  * Driver for pixhawk IMU's.
- * On with spi: L3GD20H + LSM303D and the MPU6000.
+ * L3GD20H + LSM303D (both on spi)
- * On i2c: external HMC5883L (through 3dr gps).
  */
 
 #include "subsystems/imu.h"
 #include "subsystems/abi.h"
 #include "mcu_periph/spi.h"
-#include "peripherals/hmc58xx_regs.h"
 #include "peripherals/l3gd20_regs.h"
 #include "peripherals/lsm303dlhc_regs.h"
 #include "peripherals/lsm303dlhc_spi.h"
 
-/************MPU6000*****************/
 /* SPI defaults set in subsystem makefile, can be configured from airframe file */
-PRINT_CONFIG_VAR(IMU_MPU_SPI_SLAVE_IDX)
+PRINT_CONFIG_VAR(IMU_LSM_SPI_SLAVE_IDX)
+PRINT_CONFIG_VAR(IMU_L3G_SPI_SLAVE_IDX)
 PRINT_CONFIG_VAR(IMU_SPI_DEV)
 
-/* MPU60x0 gyro/accel internal lowpass frequency */
-#if !defined IMU_MPU_LOWPASS_FILTER && !defined  IMU_MPU_SMPLRT_DIV
-#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120)
-/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms
- * Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz
- */
-#define IMU_MPU_LOWPASS_FILTER MPU60X0_DLPF_42HZ
-#define IMU_MPU_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
- */
-#define IMU_MPU_LOWPASS_FILTER MPU60X0_DLPF_256HZ
-#define IMU_MPU_SMPLRT_DIV 3
-PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
-#else
-#error Non-default PERIODIC_FREQUENCY: please define MPU_HMC_LOWPASS_FILTER and MPU_HMC_SMPLRT_DIV.
-#endif
-#endif
-PRINT_CONFIG_VAR(IMU_MPU_LOWPASS_FILTER)
-PRINT_CONFIG_VAR(IMU_MPU_SMPLRT_DIV)
-
-#ifndef IMU_MPU_GYRO_RANGE
-#define IMU_MPU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
-#endif
-PRINT_CONFIG_VAR(IMU_MPU_GYRO_RANGE)
-
-#ifndef IMU_MPU_ACCEL_RANGE
-#define IMU_MPU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
-#endif
-PRINT_CONFIG_VAR(IMU_MPU_ACCEL_RANGE)
-
-/************HMC58XX*****************/
-PRINT_CONFIG_VAR(IMU_HMC_I2C_DEV)
-
-
 struct ImuPX4 imu_px4;
 
 void imu_impl_init(void)
 {
-  /* MPU6000 init */
-  mpu60x0_spi_init(&imu_px4.mpu, &IMU_SPI_DEV, IMU_MPU_SPI_SLAVE_IDX);
-  // change the default configuration
-  imu_px4.mpu.config.smplrt_div = IMU_MPU_SMPLRT_DIV;
-  imu_px4.mpu.config.dlpf_cfg = IMU_MPU_LOWPASS_FILTER;
-  imu_px4.mpu.config.gyro_range = IMU_MPU_GYRO_RANGE;
-  imu_px4.mpu.config.accel_range = IMU_MPU_ACCEL_RANGE;
 
   /* L3GD20 gyro init */
   /* initialize gyro and set default options */
@@ -94,25 +49,23 @@ void imu_impl_init(void)
 
   /* LSM303dlhc acc + magneto init */
   lsm303dlhc_spi_init(&imu_px4.lsm_acc, &IMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_ACC);
+#if MODULE_HMC58XX_UPDATE_AHRS
   lsm303dlhc_spi_init(&imu_px4.lsm_mag, &IMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM_TARGET_MAG);
-
+#endif
-  /* HMC58XX magneto init */
-  /* initialize mag and set default options */
-  hmc58xx_init(&imu_px4.hmc, &IMU_HMC_I2C_DEV, HMC58XX_ADDR);
 
 }
 
 void imu_periodic(void)
 {
-  mpu60x0_spi_periodic(&imu_px4.mpu);
   l3gd20_spi_periodic(&imu_px4.l3g);
   lsm303dlhc_spi_periodic(&imu_px4.lsm_acc);
 
+#if MODULE_HMC58XX_UPDATE_AHRS
   /* Read magneto's every 10 times of main freq
    * at ~50Hz (main loop for rotorcraft: 512Hz)
    */
-  RunOnceEvery(10, hmc58xx_periodic(&imu_px4.hmc));
   RunOnceEvery(10, lsm303dlhc_spi_periodic(&imu_px4.lsm_mag));
+#endif
 }
 
 void imu_px4_event(void)
@@ -120,22 +73,13 @@ void imu_px4_event(void)
 
   uint32_t now_ts = get_sys_time_usec();
 
-  /* MPU6000 event task */
-  mpu60x0_spi_event(&imu_px4.mpu);
-  if (imu_px4.mpu.data_available) {
-    RATES_COPY(imu.gyro_unscaled, imu_px4.mpu.data_rates.rates);
-    VECT3_COPY(imu.accel_unscaled, imu_px4.mpu.data_accel.vect);
-    imu_px4.mpu.data_available = FALSE;
-    imu_scale_gyro(&imu);
-    imu_scale_accel(&imu);
-    AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.gyro);
-    AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.accel);
-  }
-
   /* L3GD20 event task */
   l3gd20_spi_event(&imu_px4.l3g);
   if (imu_px4.l3g.data_available) {
-    RATES_COPY(imu.gyro_unscaled, imu_px4.l3g.data_rates.rates);
+    //the p and q seem to be swapped on the Pixhawk board compared to the acc
+    imu.gyro_unscaled.p = imu_px4.l3g.data_rates.rates.q;
+    imu.gyro_unscaled.q = -imu_px4.l3g.data_rates.rates.p;
+    imu.gyro_unscaled.r = imu_px4.l3g.data_rates.rates.r;
     imu_px4.l3g.data_available = FALSE;
     imu_scale_gyro(&imu);
     AbiSendMsgIMU_GYRO_INT32(IMU_PX4_ID, now_ts, &imu.gyro);
@@ -149,22 +93,14 @@ void imu_px4_event(void)
     imu_scale_accel(&imu);
     AbiSendMsgIMU_ACCEL_INT32(IMU_PX4_ID, now_ts, &imu.accel);
   }
+#if MODULE_HMC58XX_UPDATE_AHRS
   lsm303dlhc_spi_event(&imu_px4.lsm_mag);
   if (imu_px4.lsm_mag.data_available_mag) {
     VECT3_COPY(imu.mag_unscaled, imu_px4.lsm_mag.data_mag.vect);
     imu_px4.lsm_mag.data_available_mag = FALSE;
     imu_scale_mag(&imu);
-    AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag);
+    AbiSendMsgIMU_MAG_INT32(IMU_PX4_ID, now_ts, &imu.mag);
-  }
-
-  /* HMC58XX event task */
-  hmc58xx_event(&imu_px4.hmc);
-  if (imu_px4.hmc.data_available) {
-    /* mag rotated by 90deg around z axis relative to MPU */
-    VECT3_COPY(imu.mag_unscaled, imu_px4.hmc.data.vect);
-    imu_px4.hmc.data_available = FALSE;
-    imu_scale_mag(&imu);
-    AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag);
   }
+#endif
 
 }

sw/airborne/subsystems/imu/imu_px4fmu_v2.4.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
+ * Copyright (C) 2013-2016 the paparazzi team
  *
  * This file is part of paparazzi.
  *
@@ -20,10 +20,9 @@
  */
 
 /**
- * @file subsystems/imu/imu_mpu6000.h
+ * @file subsystems/imu/imu_px4fmu_v2.4.h
  * Driver for pixhawk IMU's.
- * On with spi: L3GD20H + LSM303D and the MPU6000.
+ * L3GD20H + LSM303D (both on spi)
- * On i2c: external HMC5883L (through 3dr gps).
  */
 
 #ifndef IMU_PX4FMUV24_H
@@ -33,16 +32,12 @@
 #include "generated/airframe.h"
 #include "subsystems/imu.h"
 
-#include "subsystems/imu/imu_mpu60x0_defaults.h"
+#include "subsystems/imu/imu_px4_defaults.h"
-#include "peripherals/mpu60x0_spi.h"
-#include "peripherals/hmc58xx.h"
 #include "peripherals/l3gd20_spi.h"
 #include "peripherals/lsm303dlhc_spi.h"
 
 
 struct ImuPX4 {
-  struct Mpu60x0_Spi mpu;
-  struct Hmc58xx hmc;
   struct L3gd20_Spi l3g;
   struct Lsm303dlhc_Spi lsm_acc;
   struct Lsm303dlhc_Spi lsm_mag;