mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 22:17:01 +08:00
[imu] add support of mpu9150 on Apogee
- driver for ak8975 mag - only working in passthrough mode
This commit is contained in:
Gautier Hattenberger
@@ -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
|
||||
|
||||
@@ -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
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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 */
|
||||
Reference in New Issue
Block a user