Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[imu] add support of mpu9150 on Apogee
- driver for ak8975 mag - only working in passthrough mode
- Loading branch information
1 parent
47409e0
commit 9596aae
Showing
5 changed files
with
360 additions
and
1 deletion.
There are no files selected for viewing
16 changes: 16 additions & 0 deletions
16
conf/firmwares/subsystems/shared/imu_apogee_mpu9150.makefile
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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; | ||
| } | ||
| } | ||
|
|
Oops, something went wrong.