[imu] add support of mpu9150 on Apogee

- driver for ak8975 mag
- only working in passthrough mode

conf/firmwares/subsystems/shared/imu_apogee_mpu9150.makefile

@@ -0,0 +1,16 @@
+# Hey Emacs, this is a -*- makefile -*-
+#
+# Apogee IMU
+#
+
+include $(CFG_SHARED)/imu_apogee.makefile
+
+IMU_APOGEE_MPU9150_CFLAGS = -DAPOGEE_USE_MPU9150
+IMU_APOGEE_MPU9150_SRCS = peripherals/ak8975.c
+
+# add it for all targets except sim, fbw and nps
+ifeq (,$(findstring $(TARGET),sim fbw nps))
+$(TARGET).CFLAGS += $(IMU_APOGEE_MPU9150_CFLAGS)
+$(TARGET).srcs += $(IMU_APOGEE_MPU9150_SRCS)
+endif
+

sw/airborne/boards/apogee/imu_apogee.c

@@ -66,11 +66,33 @@ PRINT_CONFIG_VAR(APOGEE_GYRO_RANGE)
 #endif
 PRINT_CONFIG_VAR(APOGEE_ACCEL_RANGE)
 
+#if APOGEE_USE_MPU9150
+/** Normal frequency with the default settings
+ *
+ * the mag read function should be called at around 50 Hz
+ */
+#ifndef APOGEE_MAG_FREQ
+#define APOGEE_MAG_FREQ 50
+#endif
+PRINT_CONFIG_VAR(APOGEE_MAG_FREQ)
+/** Mag periodic prescaler
+ */
+#define MAG_PRESCALER Max(1,((PERIODIC_FREQUENCY)/APOGEE_MAG_FREQ))
+PRINT_CONFIG_VAR(MAG_PRESCALER)
+
+// mag config will be done later in bypass mode
+bool_t configure_mag_slave(Mpu60x0ConfigSet mpu_set, void *mpu);
+bool_t configure_mag_slave(Mpu60x0ConfigSet mpu_set __attribute__((unused)), void *mpu __attribute__((unused)))
+{
+  return TRUE;
+}
+
+#endif
+
 struct ImuApogee imu_apogee;
 
 // baro config will be done later in bypass mode
 bool_t configure_baro_slave(Mpu60x0ConfigSet mpu_set, void *mpu);
-
 bool_t configure_baro_slave(Mpu60x0ConfigSet mpu_set __attribute__((unused)), void *mpu __attribute__((unused)))
 {
   return TRUE;
@@ -90,6 +112,12 @@ void imu_impl_init(void)
   imu_apogee.mpu.config.nb_slaves = 1;
   imu_apogee.mpu.config.slaves[0].configure = &configure_baro_slave;
   imu_apogee.mpu.config.i2c_bypass = TRUE;
+#if APOGEE_USE_MPU9150
+  // if using MPU9150, internal mag needs to be configured
+  ak8975_init(&imu_apogee.ak, &(IMU_APOGEE_I2C_DEV), AK8975_I2C_SLV_ADDR);
+  imu_apogee.mpu.config.nb_slaves = 2;
+  imu_apogee.mpu.config.slaves[1].configure = &configure_mag_slave;
+#endif
 }
 
 void imu_periodic(void)
@@ -97,6 +125,11 @@ void imu_periodic(void)
   // Start reading the latest gyroscope data
   mpu60x0_i2c_periodic(&imu_apogee.mpu);
 
+#if APOGEE_USE_MPU9150
+  // Start reading internal mag if available
+  RunOnceEvery(MAG_PRESCALER, ak8975_periodic(&imu_apogee.ak));
+#endif
+
   //RunOnceEvery(10,imu_apogee_downlink_raw());
 }
 
@@ -134,5 +167,20 @@ void imu_apogee_event(void)
     AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro);
     AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel);
   }
+
+#if APOGEE_USE_MPU9150
+  ak8975_event(&imu_apogee.ak);
+  if (imu_apogee.ak.data_available) {
+    struct Int32Vect3 mag = {
+        (int32_t)( imu_apogee.ak.data.value[IMU_APOGEE_CHAN_Y]),
+        (int32_t)(-imu_apogee.ak.data.value[IMU_APOGEE_CHAN_X]),
+        (int32_t)( imu_apogee.ak.data.value[IMU_APOGEE_CHAN_Z])
+    };
+    VECT3_COPY(imu.mag_unscaled, mag);
+    imu_apogee.ak.data_available = FALSE;
+    imu_scale_mag(&imu);
+    AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag);
+  }
+#endif
 }
 

sw/airborne/boards/apogee/imu_apogee.h

@@ -33,6 +33,9 @@
 #include "std.h"
 #include "generated/airframe.h"
 #include "subsystems/imu.h"
+#if APOGEE_USE_MPU9150
+#include "peripherals/ak8975.h"
+#endif
 
 #include "peripherals/mpu60x0_i2c.h"
 
@@ -96,6 +99,9 @@
 
 struct ImuApogee {
   struct Mpu60x0_I2c mpu;
+#if APOGEE_USE_MPU9150
+  struct Ak8975 ak;
+#endif
 };
 
 extern struct ImuApogee imu_apogee;

sw/airborne/peripherals/ak8975.c

@@ -0,0 +1,197 @@
+/*
+ * Copyright (C) 2015 Xavier Paris, Gautier Hattenberger
+ *
+ * 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 peripherals/ak8975.c
+ *
+ * Driver for the AKM AK8975 magnetometer.
+ *
+ */
+
+#include "peripherals/ak8975.h"
+#include "mcu_periph/sys_time.h"
+
+#define AK8975_MEAS_TIME_MS 9
+
+// Internal calibration coeff
+// Currently fetched at startup but not used after
+// Only relying on general IMU mag calibration
+static float calibration_values[3] = { 0, 0, 0 };
+
+static float __attribute__((unused)) get_ajusted_value(const int16_t val, const uint8_t axis)
+{
+  const float H = (float) val;
+  const float corr_factor = calibration_values[axis];
+  const float Hadj = corr_factor * H;
+
+  return Hadj;
+}
+
+void ak8975_init(struct Ak8975 *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->status = AK_STATUS_IDLE;
+  ak->init_status = AK_CONF_UNINIT;
+  ak->data_available = FALSE;
+}
+
+// Configure
+void ak8975_configure(struct Ak8975 *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) {
+
+    case AK_CONF_UNINIT:
+      // Set AK8975 in fuse ROM access mode to read ADC calibration data
+      ak->i2c_trans.buf[0] = AK8975_REG_CNTL_ADDR;
+      ak->i2c_trans.buf[1] = AK8975_MODE_FUSE_ACCESS;
+      i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2);
+      break;
+
+    case AK_REQ_CALIBRATION:
+      // Request AK8975 for ADC calibration data
+      ak->i2c_trans.buf[0] = AK8975_REG_ASASX;
+      i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 3);
+      break;
+
+    case AK_DISABLE_ACCESS_CALIBRATION:
+      // Read config
+      for (uint8_t i =0; i<=2; i++)
+        calibration_values[i] =
+          ((((float)(ak->i2c_trans.buf[i])  - 128.0f)*0.5f)/128.0f)+1.0f;
+
+      // Set AK8975 in power-down mode to stop read calibration data
+      ak->i2c_trans.buf[0] = AK8975_REG_CNTL_ADDR;
+      ak->i2c_trans.buf[1] = AK8975_MODE_POWER_DOWN;
+      i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2);
+      break;
+
+    case AK_CONF_REQUESTED:
+      ak->initialized = TRUE;
+      break;
+
+    default:
+      break;
+  }
+}
+
+void ak8975_read(struct Ak8975 *ak)
+{
+  if (ak->status != AK_STATUS_IDLE) {
+    return;
+  }
+
+  // Send single measurement request
+  ak->i2c_trans.buf[0] = AK8975_REG_CNTL_ADDR;
+  ak->i2c_trans.buf[1] = AK8975_MODE_SINGLE_MEAS;
+  i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2);
+  ak->last_meas_time = get_sys_time_msec();
+  ak->status = AK_STATUS_MEAS;
+}
+
+// Get raw value
+#define RawFromBuf(_buf,_idx) ((int16_t)(_buf[_idx] | (_buf[_idx+1] << 8)))
+// Raw is actually a 14 bits signed value
+#define Int16FromRaw(_raw) ( (_raw & 0x1FFF) > 0xFFF ? (_raw & 0x1FFF) - 0x2000 : (_raw & 0x0FFF) )
+void ak8975_event(struct Ak8975 *ak)
+{
+  if (!ak->initialized) {
+    return;
+  }
+
+  switch (ak->status) {
+
+    case AK_STATUS_MEAS:
+      // Send a read data command if measurement time is done (9ms max)
+      if (ak->i2c_trans.status == I2CTransSuccess &&
+          (get_sys_time_msec() - ak->last_meas_time >= AK8975_MEAS_TIME_MS)) {
+        ak->i2c_trans.buf[0] = AK8975_REG_ST1_ADDR;
+        i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 8);
+        ak->status++;
+      }
+      break;
+
+    case AK_STATUS_READ:
+      if (ak->i2c_trans.status == I2CTransSuccess) {
+        // Mag data :
+        //   Status 1
+        //     1 byte
+        //   Measures :
+        //     2 bytes
+        //     2 bytes
+        //     2 bytes
+        //   Status 2
+        //     1 byte
+
+        // Read status and error bytes
+        const bool_t dr = ak->i2c_trans.buf[0] & 0x01; // data ready
+        const bool_t de = ak->i2c_trans.buf[7] & 0x04; // data error
+        const bool_t mo = ak->i2c_trans.buf[7] & 0x08; // mag overflow
+        if (de || !dr) {
+          // read error or data not ready, keep reading
+          break;
+        }
+        if (mo) {
+          // overflow, back to idle
+          ak->status = AK_STATUS_IDLE;
+        }
+        // Copy the data
+        int16_t val;
+        val = RawFromBuf(ak->i2c_trans.buf, 1);
+        ak->data.vect.x = Int16FromRaw(val);
+        val = RawFromBuf(ak->i2c_trans.buf, 3);
+        ak->data.vect.y = Int16FromRaw(val);
+        val = RawFromBuf(ak->i2c_trans.buf, 5);
+        ak->data.vect.z = Int16FromRaw(val);
+        ak->data_available = TRUE;
+        // End reading, back to idle
+        ak->status = AK_STATUS_IDLE;
+        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;
+      }
+      break;
+  }
+}
+

sw/airborne/peripherals/ak8975.h

@@ -0,0 +1,92 @@
+/*
+ * Copyright (C) 2015 Xavier Paris, Gautier Hattenberger
+ *
+ * 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 peripherals/ak8975.h
+ *
+ * Driver for the AKM AK8975 magnetometer.
+ */
+
+#ifndef AK8975_H
+#define AK8975_H
+
+#include "std.h"
+#include "mcu_periph/i2c.h"
+#include "math/pprz_algebra_int.h"
+
+/* Address and register definitions */
+#define AK8975_I2C_SLV_ADDR       (0x0C<<1)
+#define AK8975_REG_ST1_ADDR       0x02
+#define AK8975_REG_CNTL_ADDR      0x0A
+#define AK8975_REG_ASASX          0x10
+#define AK8975_MODE_FUSE_ACCESS   0b00001111
+#define AK8975_MODE_POWER_DOWN    0b00000000
+#define AK8975_MODE_SINGLE_MEAS   0b00000001
+
+/** config status states */
+enum Ak8975ConfStatus {
+  AK_CONF_UNINIT,
+  AK_REQ_CALIBRATION,
+  AK_DISABLE_ACCESS_CALIBRATION,
+  AK_CONF_REQUESTED
+};
+
+/** Normal status states */
+enum Ak8975Status {
+  AK_STATUS_IDLE,
+  AK_STATUS_MEAS,
+  AK_STATUS_READ,
+  AK_STATUS_DONE
+};
+
+struct Ak8975 {
+  struct i2c_periph *i2c_p;           ///< peripheral used for communcation
+  struct i2c_transaction i2c_trans;   ///< i2c transaction used for communication with the ak8936
+  bool_t initialized;                 ///< config done flag
+
+  enum Ak8975Status status;           ///< main status
+  enum Ak8975ConfStatus init_status;  ///< init status
+  uint32_t last_meas_time;            ///< last measurement time in ms
+
+  volatile bool_t data_available;     ///< data ready flag
+  union {
+    struct Int16Vect3 vect;           ///< data vector in mag coordinate system
+    int16_t value[3];                 ///< data values accessible by channel index
+  } data;
+};
+
+
+// Functions
+extern void ak8975_init(struct Ak8975 *ak, struct i2c_periph *i2c_p, uint8_t addr);
+extern void ak8975_configure(struct Ak8975 *ak);
+extern void ak8975_event(struct Ak8975 *ak);
+extern void ak8975_read(struct Ak8975 *ak);
+
+/// convenience function: read or start configuration if not already initialized
+static inline void ak8975_periodic(struct Ak8975 *ak)
+{
+  if (ak->initialized) {
+    ak8975_read(ak);
+  } else {
+    ak8975_configure(ak);
+  }
+}
+
+#endif /* AK8975_H */