Merge pull request #953 from paparazzi/mpu9250

Driver for the MPU9250

conf/firmwares/subsystems/shared/imu_mpu9250_i2c.makefile

@@ -0,0 +1,54 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# MPU9250 IMU via I2C
+#
+
+# for fixedwing firmware and ap only
+ifeq ($(TARGET), ap)
+  IMU_MPU9250_CFLAGS  = -DUSE_IMU
+endif
+
+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_i2c.c
+IMU_MPU9250_SRCS   += peripherals/mpu9250.c
+IMU_MPU9250_SRCS   += peripherals/mpu9250_i2c.c
+
+# Magnetometer
+IMU_MPU9250_SRCS   += peripherals/ak8963.c
+
+
+# set default i2c bus
+ifeq ($(ARCH), lpc21)
+MPU9250_I2C_DEV ?= i2c0
+else ifeq ($(ARCH), stm32)
+MPU9250_I2C_DEV ?= i2c2
+endif
+
+ifeq ($(TARGET), ap)
+ifndef MPU9250_I2C_DEV
+$(error Error: MPU9250_I2C_DEV not configured!)
+endif
+endif
+
+# convert i2cx to upper/lower case
+MPU9250_I2C_DEV_UPPER=$(shell echo $(MPU9250_I2C_DEV) | tr a-z A-Z)
+MPU9250_I2C_DEV_LOWER=$(shell echo $(MPU9250_I2C_DEV) | tr A-Z a-z)
+
+IMU_MPU9250_CFLAGS += -DIMU_MPU9250_I2C_DEV=$(MPU9250_I2C_DEV_LOWER)
+IMU_MPU9250_CFLAGS += -DUSE_$(MPU9250_I2C_DEV_UPPER)
+
+
+# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
+
+ap.CFLAGS += $(IMU_MPU9250_CFLAGS)
+ap.srcs   += $(IMU_MPU9250_SRCS)
+
+test_imu.CFLAGS += $(IMU_MPU9250_CFLAGS)
+test_imu.srcs   += $(IMU_MPU9250_SRCS)
+
+
+#
+# NPS simulator
+#
+include $(CFG_SHARED)/imu_nps.makefile

conf/modules/imu_mpu9250.xml

@@ -0,0 +1,28 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="imu_mpu9250" dir="sensors">
+  <doc>
+    <description>
+      Test module for the mpu9250 with I2C
+      Report RAW values on telemetry
+    </description>
+    <configure name="IMU_MPU9250_I2C_DEV" value="i2c1" description="I2C device to use (e.g. i2c1)"/>
+    <define name="IMU_MPU9250_ADDR" value="MPU9250_ADDR|MPU9250_ADDR_ALT" description="Use regular or alternate I2C address"/>
+  </doc>
+  <header>
+    <file name="imu_mpu9250.h"/>
+  </header>
+  <init fun="imu_mpu9250_init()"/>
+  <periodic fun="imu_mpu9250_periodic()"/>
+  <periodic fun="imu_mpu9250_report()" freq="10" autorun="TRUE"/>
+  <event fun="imu_mpu9250_event()"/>
+  <makefile>
+    <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>
+</module>
+

sw/airborne/modules/sensors/imu_mpu9250.c

@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file "modules/sensors/imu_mpu9250.c"
+ * @author Gautier Hattenberger
+ *
+ * Test module for the mpu9250
+ */
+
+#include "modules/sensors/imu_mpu9250.h"
+
+// Default I2C address
+#ifndef IMU_MPU9250_ADDR
+#define IMU_MPU9250_ADDR MPU9250_ADDR
+#endif
+
+struct Mpu9250_I2c mpu9250;
+
+void imu_mpu9250_init(void)
+{
+  mpu9250_i2c_init(&mpu9250, &(IMU_MPU9250_I2C_DEV), IMU_MPU9250_ADDR);
+}
+
+void imu_mpu9250_periodic(void)
+{
+  mpu9250_i2c_periodic(&mpu9250);
+}
+
+void imu_mpu9250_event(void)
+{
+  mpu9250_i2c_event(&mpu9250);
+}
+
+#include "math/pprz_algebra_int.h"
+#include "subsystems/datalink/downlink.h"
+
+void imu_mpu9250_report(void)
+{
+  struct Int32Vect3 accel = {
+    (int32_t)(mpu9250.data_accel.vect.x),
+    (int32_t)(mpu9250.data_accel.vect.y),
+    (int32_t)(mpu9250.data_accel.vect.z)
+  };
+  DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &accel.x, &accel.y, &accel.z);
+
+  struct Int32Rates rates = {
+    (int32_t)(mpu9250.data_rates.rates.p),
+    (int32_t)(mpu9250.data_rates.rates.q),
+    (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);
+}
+

sw/airborne/modules/sensors/imu_mpu9250.h

@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file "modules/sensors/imu_mpu9250.h"
+ * @author Gautier Hattenberger
+ *
+ * Test module for the mpu9250
+ */
+
+#ifndef IMU_MPU9250_H
+#define IMU_MPU9250_H
+
+#include "peripherals/mpu9250_i2c.h"
+
+extern struct Mpu9250_I2c mpu9250;
+
+extern void imu_mpu9250_init(void);
+extern void imu_mpu9250_periodic(void);
+extern void imu_mpu9250_event(void);
+extern void imu_mpu9250_report(void);
+
+#endif
+

sw/airborne/peripherals/ak8963.c

@@ -0,0 +1,142 @@
+/*
+ * Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/ak8963.c
+ *
+ * Driver for the AKM AK8963 magnetometer.
+ */
+
+#include "peripherals/ak8963.h"
+#include "std.h"
+
+/**
+ * Initialize AK8963 struct
+ */
+void ak8963_init(struct Ak8963 *ak, struct i2c_periph *i2c_p, uint8_t addr)
+{
+  /* set i2c_peripheral */
+  ak->i2c_p = i2c_p;
+  /* set i2c address */
+  ak->i2c_trans.slave_addr = addr;
+  ak->i2c_trans.status = I2CTransDone;
+  ak->initialized = FALSE;
+  ak->init_status = AK_CONF_UNINIT;
+  ak->data_available = FALSE;
+}
+
+void ak8963_configure(struct Ak8963 *ak)
+{
+  // Only configure when not busy
+  if (ak->i2c_trans.status != I2CTransSuccess && ak->i2c_trans.status != I2CTransFailed && ak->i2c_trans.status != I2CTransDone) {
+    return;
+  }
+
+  // Only when succesfull continue with next
+  if (ak->i2c_trans.status == I2CTransSuccess) {
+    ak->init_status++;
+  }
+
+  ak->i2c_trans.status = I2CTransDone;
+  switch (ak->init_status) {
+
+      // Soft Reset the device
+    case AK_CONF_UNINIT:
+      ak->i2c_trans.buf[0] = AK8963_REG_CNTL2;
+      ak->i2c_trans.buf[1] = 1;
+      i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2);
+      break;
+
+      // Set it to continious measuring mode 2
+    case AK_CONF_MODE:
+      ak->i2c_trans.buf[0] = AK8963_REG_CNTL1;
+      ak->i2c_trans.buf[1] = AK8963_CNTL1_CM_2;
+      i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2);
+      break;
+
+      // Initialization done
+    default:
+      ak->initialized = TRUE;
+      break;
+  }
+}
+
+void ak8963_read(struct Ak8963 *ak)
+{
+  if (ak->status != AK_STATUS_IDLE) {
+    return;
+  }
+
+  // Read the status register
+  ak->i2c_trans.buf[0] = AK8963_REG_ST1;
+  i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 1);
+}
+
+#define Int16FromBuf(_buf,_idx) ((int16_t)(_buf[_idx] | (_buf[_idx+1] << 8)))
+void ak8963_event(struct Ak8963 *ak)
+{
+  if (!ak->initialized) {
+    return;
+  }
+
+  switch (ak->status) {
+    case AK_STATUS_IDLE:
+      // When DRDY start reading
+      if (ak->i2c_trans.status == I2CTransSuccess && ak->i2c_trans.buf[0] & 1) {
+        ak->i2c_trans.buf[0] = AK8963_REG_HXL;
+        i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 6);
+        ak->status++;
+      }
+      break;
+
+    case AK_STATUS_READ:
+      if (ak->i2c_trans.status == I2CTransSuccess) {
+        // Copy the data
+        ak->data.vect.x = Int16FromBuf(ak->i2c_trans.buf, 0);
+        ak->data.vect.y = Int16FromBuf(ak->i2c_trans.buf, 2);
+        ak->data.vect.z = Int16FromBuf(ak->i2c_trans.buf, 4);
+        ak->data_available = TRUE;
+
+        // Read second status register to be ready for reading again
+        ak->i2c_trans.buf[0] = AK8963_REG_ST2;
+        i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 1);
+        ak->status++;
+        break;
+      }
+
+      break;
+
+    default:
+      if (ak->i2c_trans.status == I2CTransSuccess || ak->i2c_trans.status == I2CTransFailed) {
+        // Goto idle
+        ak->i2c_trans.status = I2CTransDone;
+        ak->status = AK_STATUS_IDLE;
+        // check overrun
+        //if (bit_is_set(ak->i2c_trans.buf[0], 3)) {
+        //  ak->data_available = FALSE;
+        //} else {
+        //  ak->data_available = TRUE;
+        //}
+      }
+      break;
+  }
+}
+