From 26ebe9daef3055c8680bc4a6de9d40c5fc0fbeda Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 6 Jan 2016 00:50:46 +0100 Subject: [PATCH] [imu] add support of mpu9150 on Apogee - driver for ak8975 mag - only working in passthrough mode --- .../shared/imu_apogee_mpu9150.makefile | 16 ++ sw/airborne/boards/apogee/imu_apogee.c | 50 ++++- sw/airborne/boards/apogee/imu_apogee.h | 6 + sw/airborne/peripherals/ak8975.c | 197 ++++++++++++++++++ sw/airborne/peripherals/ak8975.h | 92 ++++++++ 5 files changed, 360 insertions(+), 1 deletion(-) create mode 100644 conf/firmwares/subsystems/shared/imu_apogee_mpu9150.makefile create mode 100644 sw/airborne/peripherals/ak8975.c create mode 100644 sw/airborne/peripherals/ak8975.h diff --git a/conf/firmwares/subsystems/shared/imu_apogee_mpu9150.makefile b/conf/firmwares/subsystems/shared/imu_apogee_mpu9150.makefile new file mode 100644 index 0000000000..1e4dcecb88 --- /dev/null +++ b/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 + diff --git a/sw/airborne/boards/apogee/imu_apogee.c b/sw/airborne/boards/apogee/imu_apogee.c index 12052bdab0..5b0400f27a 100644 --- a/sw/airborne/boards/apogee/imu_apogee.c +++ b/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 } diff --git a/sw/airborne/boards/apogee/imu_apogee.h b/sw/airborne/boards/apogee/imu_apogee.h index 04a1c75e16..c68c1e1ed4 100644 --- a/sw/airborne/boards/apogee/imu_apogee.h +++ b/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; diff --git a/sw/airborne/peripherals/ak8975.c b/sw/airborne/peripherals/ak8975.c new file mode 100644 index 0000000000..4a2f972c8d --- /dev/null +++ b/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 + * . + */ + +/** + * @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; + } +} + diff --git a/sw/airborne/peripherals/ak8975.h b/sw/airborne/peripherals/ak8975.h new file mode 100644 index 0000000000..bf06c8fcc5 --- /dev/null +++ b/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 + * . + */ + +/** + * @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 */