1122 lines
38 KiB
C
1122 lines
38 KiB
C
#include "app_config.h"
|
||
#include "asm/clock.h"
|
||
#include "asm/cpu.h"
|
||
#include "generic/typedef.h"
|
||
#include "generic/gpio.h"
|
||
#include "mpu9250.h"
|
||
#include "typedef.h"
|
||
#include "system/includes.h"
|
||
#include "media/includes.h"
|
||
#include "asm/iic_hw.h"
|
||
#include "asm/iic_soft.h"
|
||
#include "asm/timer.h"
|
||
#include "imuSensor_manage.h"
|
||
|
||
#if TCFG_TP_MPU9250_ENABLE
|
||
|
||
#undef LOG_TAG_CONST
|
||
#define LOG_TAG "[mpu9250]"
|
||
#define LOG_ERROR_ENABLE
|
||
#define LOG_INFO_ENABLE
|
||
#include "debug.h"
|
||
|
||
#if TCFG_MPU9250_USER_IIC_TYPE
|
||
#define iic_init(iic) hw_iic_init(iic)
|
||
#define iic_uninit(iic) hw_iic_uninit(iic)
|
||
#define iic_start(iic) hw_iic_start(iic)
|
||
#define iic_stop(iic) hw_iic_stop(iic)
|
||
#define iic_tx_byte(iic, byte) hw_iic_tx_byte(iic, byte)
|
||
#define iic_rx_byte(iic, ack) hw_iic_rx_byte(iic, ack)
|
||
#define iic_read_buf(iic, buf, len) hw_iic_read_buf(iic, buf, len)
|
||
#define iic_write_buf(iic, buf, len) hw_iic_write_buf(iic, buf, len)
|
||
#define iic_suspend(iic) hw_iic_suspend(iic)
|
||
#define iic_resume(iic) hw_iic_resume(iic)
|
||
#else
|
||
#define iic_init(iic) soft_iic_init(iic)
|
||
#define iic_uninit(iic) soft_iic_uninit(iic)
|
||
#define iic_start(iic) soft_iic_start(iic)
|
||
#define iic_stop(iic) soft_iic_stop(iic)
|
||
#define iic_tx_byte(iic, byte) soft_iic_tx_byte(iic, byte)
|
||
#define iic_rx_byte(iic, ack) soft_iic_rx_byte(iic, ack)
|
||
#define iic_read_buf(iic, buf, len) soft_iic_read_buf(iic, buf, len)
|
||
#define iic_write_buf(iic, buf, len) soft_iic_write_buf(iic, buf, len)
|
||
#define iic_suspend(iic) soft_iic_suspend(iic)
|
||
#define iic_resume(iic) soft_iic_resume(iic)
|
||
#endif
|
||
|
||
static mpu9250_param *mpu9250_iic_info;
|
||
static mpu9250_data mpu9250_raw_data = {0};
|
||
|
||
|
||
#define MPU9250_INT_IO (-1)//TCFG_TP_INT_IO //pg5
|
||
/* #define MPU9250_INT_IO IO_PORTA_04 */
|
||
/* #define MPU9250_NCS_IO IO_PORTA_05 */
|
||
/* #define MPU9250_FSYNC_IO IO_PORTA_05 */
|
||
#define MPU9250_INT_R() gpio_read(MPU9250_INT_IO)
|
||
static u8 mpu9250_int_pin = MPU9250_INT_IO;
|
||
static u8 mpu_i2c_buf_write(u8 slave_addr, u8 reg_addr, u8 *buf, u8 len)
|
||
{
|
||
u8 i;
|
||
iic_start(mpu9250_iic_info->iic_hdl);
|
||
if (0 == iic_tx_byte(mpu9250_iic_info->iic_hdl, slave_addr)) {
|
||
iic_stop(mpu9250_iic_info->iic_hdl);
|
||
return 0;
|
||
}
|
||
udelay(mpu9250_iic_info->iic_delay);
|
||
if (0 == iic_tx_byte(mpu9250_iic_info->iic_hdl, reg_addr)) {
|
||
iic_stop(mpu9250_iic_info->iic_hdl);
|
||
return 0;
|
||
}
|
||
|
||
for (i = 0; i < len; i++) {
|
||
udelay(mpu9250_iic_info->iic_delay);
|
||
if (0 == iic_tx_byte(mpu9250_iic_info->iic_hdl, buf[i])) {
|
||
iic_stop(mpu9250_iic_info->iic_hdl);
|
||
return 0;
|
||
}
|
||
}
|
||
iic_stop(mpu9250_iic_info->iic_hdl);
|
||
return i;
|
||
}
|
||
|
||
|
||
static u8 mpu_i2c_buf_read(u8 slave_addr, u8 reg_addr, u8 *buf, u8 len)
|
||
{
|
||
u8 i;
|
||
iic_start(mpu9250_iic_info->iic_hdl);
|
||
if (0 == iic_tx_byte(mpu9250_iic_info->iic_hdl, slave_addr)) {
|
||
iic_stop(mpu9250_iic_info->iic_hdl);
|
||
return 0;
|
||
}
|
||
udelay(mpu9250_iic_info->iic_delay);
|
||
if (0 == iic_tx_byte(mpu9250_iic_info->iic_hdl, reg_addr)) {
|
||
iic_stop(mpu9250_iic_info->iic_hdl);
|
||
return 0;
|
||
}
|
||
udelay(mpu9250_iic_info->iic_delay);
|
||
|
||
iic_start(mpu9250_iic_info->iic_hdl);
|
||
if (0 == iic_tx_byte(mpu9250_iic_info->iic_hdl, slave_addr + 1)) {
|
||
iic_stop(mpu9250_iic_info->iic_hdl);
|
||
return 0;
|
||
}
|
||
for (i = 0; i < len; i++) {
|
||
udelay(mpu9250_iic_info->iic_delay);
|
||
if (i == (len - 1)) {
|
||
*buf++ = iic_rx_byte(mpu9250_iic_info->iic_hdl, 0);
|
||
} else {
|
||
*buf++ = iic_rx_byte(mpu9250_iic_info->iic_hdl, 1);
|
||
}
|
||
}
|
||
iic_stop(mpu9250_iic_info->iic_hdl);
|
||
|
||
return i;
|
||
}
|
||
|
||
//设置MPU9250陀螺仪传感器满量程范围
|
||
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
|
||
//返回值:1,设置成功
|
||
// 0,设置失败
|
||
u8 mpu6500_set_full_scale_gyro_range(u8 fsr)//0x1b(bit34)
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
fsr &= 0x3;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_GYRO_CFG_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
if (((temp_data & 0x18) >> 3) != fsr) {
|
||
SFR(temp_data, 3, 2, fsr);
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_GYRO_CFG_REG, &temp_data, 1);
|
||
}
|
||
}
|
||
return res;
|
||
}
|
||
//设置MPU9250加速度传感器满量程范围
|
||
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
|
||
//返回值:1,设置成功
|
||
// 0,设置失败
|
||
u8 mpu6500_set_full_scale_accel_range(u8 fsr)//0x1c(bit34)
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
fsr &= 0x3;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_ACCEL_CFG_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
if (((temp_data & 0x18) >> 3) != fsr) {
|
||
SFR(temp_data, 3, 2, fsr);
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_ACCEL_CFG_REG, &temp_data, 1);
|
||
}
|
||
}
|
||
return res;
|
||
}
|
||
|
||
/**
|
||
*For the DLPF to be used, fchoice[1:0] must be set to 2’b11, fchoice_b[1:0] is 2’b00.
|
||
See table 3 below.
|
||
The DLPF is configured by DLPF_CFG, when FCHOICE_B [1:0] = 2b’00. The gyroscope and
|
||
temperature sensor are filtered according to the value of DLPF_CFG and FCHOICE_B as shown in the table below. Note that FCHOICE mentioned in the table below is the inverted value of FCHOICE_B (e.g. FCHOICE=2b’00 is same as FCHOICE_B=2b’11).
|
||
|
||
//设置MPU9250gyroscope and temperature sensor的数字低通滤波器
|
||
//lpf:数字低通滤波频率(Hz)
|
||
//返回值:1,设置成功
|
||
// 0,设置失败
|
||
* */
|
||
u8 mpu_set_dlpf(u16 lpf)//0x1a(bit012)
|
||
{
|
||
u8 data = 0;
|
||
if (lpf >= 188) {
|
||
data = 1;
|
||
} else if (lpf >= 98) {
|
||
data = 2;
|
||
} else if (lpf >= 42) {
|
||
data = 3;
|
||
} else if (lpf >= 20) {
|
||
data = 4;
|
||
} else if (lpf >= 10) {
|
||
data = 5;
|
||
} else {
|
||
data = 6;
|
||
}
|
||
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
data &= 0x07;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_CFG_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
if ((temp_data & 0x07) != data) {
|
||
temp_data &= ~0x07;
|
||
temp_data |= data;
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_CFG_REG, &temp_data, 1); //设置数字低通滤波器
|
||
}
|
||
}
|
||
return res;
|
||
}
|
||
|
||
/*
|
||
*Divides the internal sample rate (see register CONFIG) to generate the
|
||
sample rate that controls sensor data output rate, FIFO sample rate.
|
||
NOTE: This register is only effective when Fchoice = 2’b11 (fchoice_b
|
||
register bits are 2’b00), and (0 < dlpf_cfg < 7), such that the average filter’s
|
||
output is selected (see chart below).
|
||
This is the update rate of sensor register.
|
||
SAMPLE_RATE= Internal_Sample_Rate / (1 + SMPLRT_DIV)
|
||
Data should be sampled at or above sample rate; SMPLRT_DIV is only used for1kHz internal sampling.
|
||
//rate:4~1000(Hz)
|
||
//设置mpu9250gyroscope的采样率(假定 Fs=1KHz)
|
||
//返回值:1,设置成功
|
||
// 0,设置失败
|
||
* */ //0x19
|
||
u8 mpu_set_rate(u16 rate)// 设置采样速率: 1000 / (1 + rate)
|
||
{
|
||
u8 data;
|
||
if (rate > 1000) {
|
||
rate = 1000;
|
||
}
|
||
if (rate < 4) {
|
||
rate = 4;
|
||
}
|
||
data = 1000 / rate - 1;
|
||
mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_SAMPLE_RATE_REG, &data, 1); //设置数字低通滤波器
|
||
/* return mpu_set_dlpf(rate/2); //自动设置LPF为采样率的一半 */
|
||
return mpu_set_dlpf(98);
|
||
}
|
||
|
||
bool mpu6500_set_sleep_enabled(u8 enable);// 唤醒MPU6500
|
||
//返回值:1,成功
|
||
// 0,错误代码
|
||
u8 mpu9250_init1(void *param)//bypass模式读取ak8963数据
|
||
{
|
||
u8 res = 0, temp_data = 0;
|
||
if (param == NULL) {
|
||
log_info("mpu9250 init fail(no param)\n");
|
||
return false;
|
||
}
|
||
mpu9250_iic_info = (mpu9250_param *)param;
|
||
|
||
temp_data = 0X80;
|
||
mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_PWR_MGMT1_REG, &temp_data, 1); //复位MPU9250
|
||
mdelay(100); //延时100ms
|
||
temp_data = 0X00;
|
||
mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_PWR_MGMT1_REG, &temp_data, 1); //唤醒MPU9250
|
||
mdelay(10); //延时
|
||
mpu6500_set_full_scale_gyro_range(MPU6500_GYRO_FS_2000);//陀螺仪传感器,±2000dps
|
||
mpu6500_set_full_scale_accel_range(0); //加速度传感器,±2g
|
||
mpu_set_rate(50); //设置采样率50Hz
|
||
mpu6500_set_sleep_enabled(false); // 唤醒MPU6500
|
||
temp_data = 0X00;
|
||
mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_INT_EN_REG, &temp_data, 1); //关闭所有中断
|
||
temp_data = 0X00;
|
||
mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_USER_CTRL_REG, &temp_data, 1); //I2C主模式关闭
|
||
temp_data = 0X00;
|
||
mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_FIFO_EN_REG, &temp_data, 1); //关闭FIFO
|
||
temp_data = 0X82;
|
||
mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_INTBP_CFG_REG, &temp_data, 1); //INT引脚低电平有效，开启bypass模式，可以直接读取磁力计
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_DEVICE_ID_REG, &temp_data, 1); //读取MPU6500的ID
|
||
if (temp_data == 0x71) { //器件ID正确
|
||
log_info("read mpu id:0x%x", temp_data);
|
||
temp_data = 0X01;
|
||
mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_PWR_MGMT1_REG, &temp_data, 1); //设置CLKSEL,PLL X轴为参考
|
||
temp_data = 0X00;
|
||
mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_PWR_MGMT2_REG, &temp_data, 1); //加速度与陀螺仪都工作
|
||
mpu_set_rate(50); //设置采样率为50Hz
|
||
} else {
|
||
log_error("read mpu id(0x%x) fail!", temp_data);
|
||
return false;
|
||
}
|
||
|
||
res = mpu_i2c_buf_read(MAG_IIC_ADDRESS_W, MAG_WHO_AM_I, &temp_data, 1); //读取AK8963 ID
|
||
if (temp_data == 0x48) {
|
||
log_info("read mag id:0x%x", temp_data);
|
||
temp_data = 0X11;
|
||
mpu_i2c_buf_write(MAG_IIC_ADDRESS_W, MAG_CNTL_1, &temp_data, 1); //设置AK8963为单次测量模式
|
||
} else {
|
||
log_error("read mag id(0x%x) fail!", temp_data);
|
||
/* return false; */
|
||
}
|
||
return true;
|
||
}
|
||
|
||
//得到温度值
|
||
//temp温度值
|
||
//返回值:2,成功
|
||
// 其他,错误代码
|
||
u8 mpu_get_temperature(float *temp)
|
||
{
|
||
u8 buf[2], res;
|
||
short raw;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_TEMP_OUTH_REG, buf, 2);
|
||
if (res == 2) {
|
||
raw = ((s16)buf[0] << 8) | buf[1];
|
||
*temp = 21 + (raw) / 333.87;
|
||
}
|
||
return res;
|
||
}
|
||
//得到陀螺仪值(原始值)
|
||
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
|
||
//返回值:6,成功
|
||
// 其他,错误代码
|
||
u8 mpu_get_gyroscope(short *gx, short *gy, short *gz)
|
||
{
|
||
u8 buf[6], res;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_GYRO_XOUTH_REG, buf, 6);
|
||
if (res == 6) {
|
||
*gx = ((u16)buf[0] << 8) | buf[1];
|
||
*gy = ((u16)buf[2] << 8) | buf[3];
|
||
*gz = ((u16)buf[4] << 8) | buf[5];
|
||
}
|
||
return res;
|
||
}
|
||
//得到加速度值(原始值)
|
||
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
|
||
//返回值:6,成功
|
||
// 其他,错误代码
|
||
u8 mpu_get_accelerometer(short *ax, short *ay, short *az)
|
||
{
|
||
u8 buf[6], res;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_ACCEL_XOUTH_REG, buf, 6);
|
||
if (res == 6) {
|
||
*ax = ((u16)buf[0] << 8) | buf[1];
|
||
*ay = ((u16)buf[2] << 8) | buf[3];
|
||
*az = ((u16)buf[4] << 8) | buf[5];
|
||
}
|
||
return res;
|
||
}
|
||
|
||
//得到磁力计值(原始值)
|
||
//mx,my,mz:磁力计x,y,z轴的原始读数(带符号)
|
||
//返回值:6,成功
|
||
// 其他,错误代码
|
||
u8 mpu_get_magnetometer(short *mx, short *my, short *mz)
|
||
{
|
||
u8 buf[6], res;
|
||
res = mpu_i2c_buf_read(MAG_IIC_ADDRESS_W, MAG_XOUT_L, buf, 6);
|
||
if (res == 6) {
|
||
*mx = ((u16)buf[1] << 8) | buf[0];
|
||
*my = ((u16)buf[3] << 8) | buf[2];
|
||
*mz = ((u16)buf[5] << 8) | buf[4];
|
||
}
|
||
buf[0] = 0X11;
|
||
mpu_i2c_buf_write(MAG_IIC_ADDRESS_W, MAG_CNTL_1, buf, 1); //AK8963每次读完以后都需要重新设置为单次测量模式
|
||
return res;
|
||
}
|
||
|
||
//iic scl:<=400KHz
|
||
|
||
|
||
#if 0
|
||
|
||
static mpu9250_param mpu9250_iic_info_test1 = {
|
||
.iic_hdl = 0,
|
||
.iic_delay = 0, //字节间延时，单位us
|
||
};
|
||
|
||
void mpu9250_test1()
|
||
{
|
||
iic_init(mpu9250_iic_info_test1.iic_hdl);
|
||
gpio_set_direction(mpu9250_int_pin, 1);
|
||
gpio_set_die(mpu9250_int_pin, 1);
|
||
/* gpio_set_pull_up(mpu9250_int_pin, 1); */
|
||
/* gpio_set_pull_down(mpu9250_int_pin, 0); */
|
||
|
||
if (mpu9250_init1(&mpu9250_iic_info_test1)) {
|
||
log_info("mpu9250 Device init pass!\n");
|
||
|
||
while (1) {
|
||
mpu_get_temperature(&mpu9250_raw_data.temp_data);
|
||
mpu_get_gyroscope(&mpu9250_raw_data.gyro_data.x_data, &mpu9250_raw_data.gyro_data.y_data, &mpu9250_raw_data.gyro_data.z_data);
|
||
mpu_get_accelerometer(&mpu9250_raw_data.accel_data.x_data, &mpu9250_raw_data.accel_data.y_data, &mpu9250_raw_data.accel_data.z_data);
|
||
mpu_get_magnetometer(&mpu9250_raw_data.mag_data.x_data, &mpu9250_raw_data.mag_data.y_data, &mpu9250_raw_data.mag_data.z_data);
|
||
log_info("mpu9250_raw_data.accel_data:X:%d,Y:%d,Z:%d", mpu9250_raw_data.accel_data.x_data, mpu9250_raw_data.accel_data.y_data, mpu9250_raw_data.accel_data.z_data);
|
||
log_info("mpu9250_raw_data.gyro_data:X:%d,Y:%d,Z:%d", mpu9250_raw_data.gyro_data.x_data, mpu9250_raw_data.gyro_data.y_data, mpu9250_raw_data.gyro_data.z_data);
|
||
log_info("mpu9250_raw_data.mag_data:X:%d,Y:%d,Z:%d", mpu9250_raw_data.mag_data.x_data, mpu9250_raw_data.mag_data.y_data, mpu9250_raw_data.mag_data.z_data);
|
||
log_info("mpu9250_raw_data.temp_data:%d", (s16)mpu9250_raw_data.temp_data);
|
||
wdt_clear();
|
||
mdelay(10);
|
||
}
|
||
} else {
|
||
log_info("mpu9250 Device init fail!\n");
|
||
}
|
||
}
|
||
|
||
#endif
|
||
|
||
|
||
|
||
|
||
/******************************mpu6500 config****************************/
|
||
//return:1:ok, 0:fail
|
||
bool mpu6500_reset()// 复位MPU6500
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_PWR_MGMT1_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
temp_data |= BIT(7);
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_PWR_MGMT1_REG, &temp_data, 1);
|
||
}
|
||
return res;
|
||
}
|
||
//return:1:ok, 0:fail
|
||
bool mpu6500_set_sleep_enabled(u8 enable)// 唤醒MPU6500
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_PWR_MGMT1_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
if (enable) {
|
||
temp_data |= BIT(6);
|
||
} else {
|
||
temp_data &= ~ BIT(6);
|
||
}
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_PWR_MGMT1_REG, &temp_data, 1);
|
||
}
|
||
return res;
|
||
}
|
||
|
||
/** Set clock source setting.
|
||
* An internal 8MHz oscillator, gyroscope based clock, or external sources c&= ~ * be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillat|= * or an external source is chosen as the clock source, the MPU-60X0 can operate in low power modes with the gyroscopes disabled.
|
||
*
|
||
* Upon power up, the MPU-60X0 clock source defaults to the internal oscillator.
|
||
* However, it is highly recommended that the device be configured to use one of the gyroscopes (or an external clock source) as the clock reference f|= * improved stability. The clock source can be selected according to the following table:
|
||
* CLK_SEL | Clock Source
|
||
* --------+--------------------------------------
|
||
* 0 | Internal oscillat|= * 1 | PLL with X Gyro reference
|
||
* 2 | PLL with Y Gyro reference
|
||
* 3 | PLL with Z Gyro reference
|
||
* 4 | PLL with external 32.768kHz reference
|
||
* 5 | PLL with external 19.2MHz reference
|
||
* 6 | Reserved
|
||
* 7 | Stops the clock and keeps the timing generator in reset
|
||
* */
|
||
//return:1:ok, 0:fail
|
||
bool mpu6500_set_clocksource(u8 clocksource)// 设置X轴陀螺作为时钟
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
clocksource &= 0x07;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_PWR_MGMT1_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
if ((temp_data & 0x07) != clocksource) {
|
||
temp_data &= ~0x07;
|
||
temp_data |= clocksource;
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_PWR_MGMT1_REG, &temp_data, 1);
|
||
}
|
||
}
|
||
return res;
|
||
}
|
||
|
||
//return:1:ok, 0:fail
|
||
bool mpu6500_set_temp_enabled(u8 enable)// 使能温度传感器
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_PWR_MGMT1_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
if (enable) {
|
||
temp_data &= ~ BIT(3);
|
||
} else {
|
||
temp_data |= BIT(3);
|
||
}
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_PWR_MGMT1_REG, &temp_data, 1);
|
||
}
|
||
return res;
|
||
}
|
||
|
||
//return:1:ok, 0:fail
|
||
bool mpu6500_set_int_enabled(u8 int_data)// 关闭中断
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = int_data;
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_INT_EN_REG, &temp_data, 1);
|
||
return res;
|
||
}
|
||
//return:1:ok, 0:fail
|
||
//数据就绪中断使能
|
||
// This event occurs each time a write operation to all of the sensor registers has been completed. Will be set 0 for disabled, 1 for enabled.
|
||
bool mpu6500SetIntDataReadyEnabled(u8 enable)//0x38(bit0)
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_INT_EN_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
if (enable) {
|
||
temp_data |= BIT(0);
|
||
} else {
|
||
temp_data &= ~ BIT(0);
|
||
}
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_INT_EN_REG, &temp_data, 1);
|
||
}
|
||
return res;
|
||
}
|
||
|
||
//return:1:ok, 0:fail
|
||
bool mpu6500_set_iic_MST_enabled(u8 enable)//I2C主模式开关
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_USER_CTRL_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
if (enable) {
|
||
temp_data |= BIT(5);
|
||
} else {
|
||
temp_data &= ~ BIT(5);
|
||
}
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_USER_CTRL_REG, &temp_data, 1);
|
||
}
|
||
return res;
|
||
}
|
||
|
||
//return:1:ok, 0:fail
|
||
// I2C_MST_EN (Register 106 bit[5]) has to be equal to 0
|
||
// 1:旁路模式，磁力计和其它连接到主IIC ; 0:主机模式
|
||
bool mpu6500_set_iic_bypass_enabled(u8 enable)//0x37(bit1)
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
/* mpu6500_set_iic_MST_enabled(false)//I2C主模式关 */
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_INTBP_CFG_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
if (enable) {
|
||
temp_data |= BIT(1);
|
||
} else {
|
||
temp_data &= ~ BIT(1);
|
||
}
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_INTBP_CFG_REG, &temp_data, 1);
|
||
}
|
||
return res;
|
||
}
|
||
|
||
//默认0x00:rate4k
|
||
//return:1:ok, 0:fail
|
||
bool mpu6500_set_accel_DLPF(u8 dlpf_data)// 设置加速计数字低通滤波
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
dlpf_data &= 0x07;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_ACCEL_CFG_REG_2, &temp_data, 1);
|
||
if (res == 1) {
|
||
if ((temp_data & 0x07) != dlpf_data) {
|
||
temp_data &= ~0x07;
|
||
temp_data |= dlpf_data;
|
||
temp_data |= BIT(3);//bit3=1,dlpf才才有效
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_ACCEL_CFG_REG_2, &temp_data, 1);
|
||
}
|
||
}
|
||
return res;
|
||
}
|
||
|
||
|
||
//return:1:ok, 0:fail
|
||
// 从机读取速率: 100Hz = (1000Hz / (1 + slave_delay))
|
||
bool mpu6500SetSlave4MasterDelay(u8 slave_delay)//0x34(bit0~4)
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
slave_delay &= 0x1f;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_I2CSLV4_CTRL_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
if ((temp_data & 0x1f) != slave_delay) {
|
||
temp_data &= ~0x1f;
|
||
temp_data |= slave_delay;
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_I2CSLV4_CTRL_REG, &temp_data, 1);
|
||
}
|
||
}
|
||
return res;
|
||
}
|
||
|
||
//return:1:ok, 0:fail
|
||
//Set wait-for-external-sensor-data enabled value.
|
||
bool mpu6500SetWaitForExternalSensorEnabled(u8 enable)//0x24(bit6)
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_I2CMST_CTRL_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
if (enable) {
|
||
temp_data |= BIT(6);
|
||
} else {
|
||
temp_data &= ~ BIT(6);
|
||
}
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_I2CMST_CTRL_REG, &temp_data, 1);
|
||
}
|
||
return res;
|
||
}
|
||
|
||
//return:1:ok, 0:fail
|
||
//This bit controls the I2C Master’s transition from one slave read to the next
|
||
//slave read. If 0, there is a restart between reads. If 1, there is a stop between
|
||
//reads.
|
||
bool mpu6500SetSlaveReadWriteTransitionEnabled(u8 enable)
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_I2CMST_CTRL_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
if (enable) {
|
||
temp_data |= BIT(4);
|
||
} else {
|
||
temp_data &= ~ BIT(4);
|
||
}
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_I2CMST_CTRL_REG, &temp_data, 1);
|
||
}
|
||
return res;
|
||
}
|
||
|
||
//return:1:ok, 0:fail
|
||
//Set I2C master clock speed.
|
||
bool mpu6500SetMasterClockSpeed(u8 iic_master_rate)
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
iic_master_rate &= 0x0f;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_I2CMST_CTRL_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
temp_data &= ~ 0x0f;
|
||
temp_data |= iic_master_rate;
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_I2CMST_CTRL_REG, &temp_data, 1);
|
||
}
|
||
return res;
|
||
}
|
||
|
||
//return:1:ok, 0:fail
|
||
//bit7: Set interrupt logic level mode.(0=active-high, 1=active-low)
|
||
//bit6: Set interrupt drive mode.(0=push-pull, 1=open-drain)
|
||
//bit5: Set interrupt latch mode.(0=50us-pulse, 1=latch-until-int-cleared)
|
||
//bit4: Set interrupt latch clear mode.(0=status-read-only, 1=any-register-read)
|
||
bool mpu6500SetInterruptMode(u8 int_cfg)
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
int_cfg &= 0xf0;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_INTBP_CFG_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
temp_data &= ~ 0xf0;
|
||
temp_data |= int_cfg;
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_INTBP_CFG_REG, &temp_data, 1);
|
||
}
|
||
return res;
|
||
}
|
||
|
||
//return:1:ok, 0:fail
|
||
bool mpu6500SetSlaveAddress(u8 num, u8 addr)
|
||
{
|
||
u8 temp_data = 0;
|
||
if (num > 3) {
|
||
return false;
|
||
}
|
||
temp_data = addr;
|
||
return mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_I2CSLV0_ADDR_REG + num * 3, &temp_data, 1);
|
||
}
|
||
//return:1:ok, 0:fail
|
||
bool mpu6500SetSlaveRegister(u8 num, u8 reg)
|
||
{
|
||
u8 temp_data = 0;
|
||
if (num > 3) {
|
||
return false;
|
||
}
|
||
temp_data = reg;
|
||
return mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_I2CSLV0_REG + num * 3, &temp_data, 1);
|
||
}
|
||
//return:1:ok, 0:fail
|
||
bool mpu6500SetSlaveDataLength(u8 num, u8 length)
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
if (num > 3) {
|
||
return false;
|
||
}
|
||
length &= 0x0f;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_I2CSLV0_CTRL_REG + num * 3, &temp_data, 1);
|
||
if (res == 1) {
|
||
temp_data &= ~ 0x0f;
|
||
temp_data |= length;
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_I2CSLV0_CTRL_REG + num * 3, &temp_data, 1);
|
||
}
|
||
return res;
|
||
}
|
||
//return:1:ok, 0:fail
|
||
bool mpu6500SetSlaveEnabled(u8 num, u8 enable)
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
if (num > 3) {
|
||
return false;
|
||
}
|
||
enable &= 0x0f;
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_I2CSLV0_CTRL_REG + num * 3, &temp_data, 1);
|
||
if (res == 1) {
|
||
if (enable) {
|
||
temp_data |= BIT(7);
|
||
} else {
|
||
temp_data &= ~ BIT(7);
|
||
}
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_I2CSLV0_CTRL_REG + num * 3, &temp_data, 1);
|
||
}
|
||
return res;
|
||
}
|
||
|
||
//return:1:ok, 0:fail
|
||
bool mpu6500SetSlaveDelayEnabled(u8 num, u8 enable)
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
if (num > 4) {
|
||
return false;
|
||
}
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_I2CMST_DELAY_REG, &temp_data, 1);
|
||
if (res == 1) {
|
||
if (enable) {
|
||
temp_data |= BIT(num);
|
||
} else {
|
||
temp_data &= ~ BIT(num);
|
||
}
|
||
res = mpu_i2c_buf_write(MPU9250_ADDRESS_W, MPU_I2CMST_DELAY_REG, &temp_data, 1);
|
||
}
|
||
return res;
|
||
}
|
||
|
||
/*设置传感器从模式读取*/
|
||
static void sensors_setup_slave_read(void)
|
||
{
|
||
mpu6500SetSlave4MasterDelay(9);// 从机读取速率: 100Hz = (1000Hz / (1 + 9))//zxb
|
||
|
||
mpu6500_set_iic_bypass_enabled(false); //主机模式
|
||
mpu6500SetWaitForExternalSensorEnabled(true);
|
||
mpu6500SetInterruptMode(0x90);//中断低电平有效 推挽输出 中断锁存模式(0=50us-pulse) 中断清除模式(1=any-register-read)
|
||
mpu6500SetSlaveReadWriteTransitionEnabled(false); // 关闭从机间读写结束位的传输
|
||
mpu6500SetMasterClockSpeed(13); // 设置i2c速度400kHz
|
||
|
||
#if MAG_AK8963_ENABLE
|
||
// 设置MPU6500主机要读取的寄存器
|
||
mpu6500SetSlaveAddress(0, 0x80 | MAG_IIC_ADDRESS); // 设置磁力计为0号从机
|
||
mpu6500SetSlaveRegister(0, MAG_STATE_1); // 从机0需要读取的寄存器
|
||
mpu6500SetSlaveDataLength(0, SENSORS_MAG_BUFF_LEN); // 读取8个字节(ST1, x, y, z heading, ST2 (overflow check))
|
||
mpu6500SetSlaveDelayEnabled(0, true);
|
||
mpu6500SetSlaveEnabled(0, true);
|
||
#endif
|
||
mpu6500_set_iic_MST_enabled(true); //使能mpu6500主机模式
|
||
mpu6500SetIntDataReadyEnabled(true); //数据就绪中断使能
|
||
}
|
||
|
||
|
||
|
||
/******************************ak8963 config****************************/
|
||
//return:1:ok, 0:fail
|
||
bool ak8963_check_connection()
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
mpu_i2c_buf_read(MAG_IIC_ADDRESS_W, MAG_WHO_AM_I, &temp_data, 1); //读取AK8963 ID
|
||
if (temp_data == 0x48) {
|
||
log_info("read mag id:0x%x ok!", temp_data);
|
||
} else {
|
||
log_error("read mag id(0x%x) fail!", temp_data);
|
||
return false;
|
||
}
|
||
return true;
|
||
|
||
}
|
||
/* bool ak8963_check_connection() */
|
||
/* { */
|
||
/* u8 res=0; */
|
||
/* u8 temp_data=0; */
|
||
/* while(1){ */
|
||
/* if(res==0xfe)return false; */
|
||
/* mpu_i2c_buf_read(res,MAG_WHO_AM_I,&temp_data,1); //读取AK8963 ID */
|
||
/* if(temp_data==0x48) */
|
||
/* { */
|
||
/* log_info("read mag id:0x%x ok!",temp_data); */
|
||
/* }else { */
|
||
/* log_error("read mag id(0x%x) fail!",temp_data); */
|
||
/* res +=2; */
|
||
/* temp_data=0; */
|
||
/* #<{(| return false; |)}># */
|
||
/* } */
|
||
/* mdelay(10); */
|
||
/* } */
|
||
/* return true; */
|
||
/* */
|
||
/* } */
|
||
|
||
//return:1:ok, 0:fail
|
||
bool ak8963_set_mode(u8 data)
|
||
{
|
||
u8 temp_data = data;
|
||
return mpu_i2c_buf_write(MAG_IIC_ADDRESS_W, MAG_CNTL_1, &temp_data, 1);
|
||
}
|
||
|
||
//return:1:ok, 0:fail
|
||
bool ak8963Reset()
|
||
{
|
||
u8 res = 0;
|
||
u8 temp_data = 0;
|
||
res = mpu_i2c_buf_read(MAG_IIC_ADDRESS_W, MAG_CNTL_1, &temp_data, 1);
|
||
if (res == 1) {
|
||
temp_data &= ~0x0f;
|
||
res = mpu_i2c_buf_write(MAG_IIC_ADDRESS_W, MAG_CNTL_1, &temp_data, 1);
|
||
}
|
||
return res;
|
||
}
|
||
|
||
|
||
bool mpu9250_init2(void *param)//主模式获取ak8963
|
||
{
|
||
u8 res = 0, temp_data = 0;
|
||
if (param == NULL) {
|
||
log_info("mpu9250 init fail(no param)\n");
|
||
return false;
|
||
}
|
||
mpu9250_iic_info = (mpu9250_param *)param;
|
||
|
||
mdelay(10);
|
||
mpu6500_reset(); // 复位MPU6500
|
||
mdelay(20);
|
||
res = mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_DEVICE_ID_REG, &temp_data, 1); //读取MPU6500的ID
|
||
if (temp_data == 0x71) { //器件ID正确
|
||
log_info("read mpu id:0x%x ok!", temp_data);
|
||
} else {
|
||
log_error("read mpu id(0x%x) fail!", temp_data);
|
||
return false;
|
||
}
|
||
mpu6500_set_sleep_enabled(false); // 唤醒MPU6500
|
||
mdelay(10);
|
||
mpu6500_set_clocksource(MPU6500_CLOCK_PLL_XGYRO);// 设置X轴陀螺作为时钟
|
||
mdelay(10);// 延时等待时钟稳定
|
||
mpu6500_set_temp_enabled(true);// 使能温度传感器
|
||
mpu6500_set_int_enabled(0x00);// 关闭中断
|
||
|
||
mpu6500_set_iic_MST_enabled(false);//I2C主模式关//zxb
|
||
mpu6500_set_iic_bypass_enabled(true);// 旁路模式，磁力计和气压连接到主IICnot sure//zxb
|
||
|
||
mpu6500_set_full_scale_gyro_range(MPU6500_GYRO_FS_2000); // 设置陀螺量程
|
||
mpu6500_set_full_scale_accel_range(MPU6500_ACCEL_FS_16);// 设置加速计量程
|
||
/* mpu6500_set_accel_DLPF(MPU6500_ACCEL_DLPF_BW_41); // 设置加速计数字低通滤波 */
|
||
|
||
mpu_set_rate(MPU6500_DATA_SAMPLE_RATE);//设置采样速率: 1000Hz(max)//zxb
|
||
|
||
#if MAG_AK8963_ENABLE
|
||
if (ak8963_check_connection() == true) {
|
||
ak8963_set_mode(AK8963_MODE_16BIT | AK8963_MODE_CONT2); // 16bit 100Hz
|
||
log_info("AK8963 I2C connection [OK].\n");
|
||
} else {
|
||
log_error("AK8963 I2C connection [FAIL].\n");
|
||
return false;
|
||
}
|
||
#endif
|
||
|
||
//外部中断
|
||
mdelay(150);
|
||
sensors_setup_slave_read();
|
||
return true;
|
||
}
|
||
|
||
|
||
static u8 init_flag = 0;
|
||
static u8 imu_busy = 0;
|
||
static mpu9250_param mpu9250_info_data;
|
||
volatile u8 mpu9250_int_flag = 0;
|
||
#if MAG_AK8963_ENABLE
|
||
#define SENSORS_MPU_BUFF_LEN (SENSORS_MPU6500_BUFF_LEN + SENSORS_MAG_BUFF_LEN)
|
||
#else
|
||
#define SENSORS_MPU_BUFF_LEN SENSORS_MPU6500_BUFF_LEN
|
||
#endif
|
||
u8 read_mpu_buf[SENSORS_MPU_BUFF_LEN];
|
||
|
||
void mpu9250_int_callback()
|
||
{
|
||
if (init_flag == 0) {
|
||
log_error("mpu9250 init fail!");
|
||
return;
|
||
}
|
||
if (imu_busy) {
|
||
log_error("mpu9250 busy!");
|
||
return;
|
||
}
|
||
imu_busy = 1;
|
||
|
||
mpu9250_int_flag = 1;
|
||
mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_ACCEL_XOUTH_REG, read_mpu_buf, SENSORS_MPU_BUFF_LEN);
|
||
#if 1
|
||
s16 x = 0, y = 0, z = 0;
|
||
if (mpu9250_int_flag) {/*{{{*/
|
||
x = (((s16) read_mpu_buf[0]) << 8) | read_mpu_buf[1];
|
||
y = ((((s16) read_mpu_buf[2]) << 8) | read_mpu_buf[3]);
|
||
z = (((s16) read_mpu_buf[4]) << 8) | read_mpu_buf[5];
|
||
log_info("mpu9250_raw_data.accel_data:X:%d,Y:%d,Z:%d", x, y, z);
|
||
x = (((s16) read_mpu_buf[8]) << 8) | read_mpu_buf[9];
|
||
y = (((s16) read_mpu_buf[10]) << 8) | read_mpu_buf[11];
|
||
z = (((s16) read_mpu_buf[12]) << 8) | read_mpu_buf[13];
|
||
log_info("mpu9250_raw_data.gyro_data:X:%d,Y:%d,Z:%d", x, y, z);
|
||
#if MAG_AK8963_ENABLE
|
||
if (read_mpu_buf[0 + SENSORS_MPU6500_BUFF_LEN]&BIT(0)) {
|
||
x = (((s16) read_mpu_buf[2 + SENSORS_MPU6500_BUFF_LEN ]) << 8) | read_mpu_buf[1 + SENSORS_MPU6500_BUFF_LEN ];
|
||
y = (((s16) read_mpu_buf[4 + SENSORS_MPU6500_BUFF_LEN ]) << 8) | read_mpu_buf[3 + SENSORS_MPU6500_BUFF_LEN ];
|
||
z = (((s16) read_mpu_buf[6 + SENSORS_MPU6500_BUFF_LEN ]) << 8) | read_mpu_buf[5 + SENSORS_MPU6500_BUFF_LEN ];
|
||
log_info("mpu9250_raw_data.mag_data:X:%d,Y:%d,Z:%d", x, y, z);
|
||
}
|
||
#endif
|
||
x = (((s16) read_mpu_buf[6]) << 8) | read_mpu_buf[7];
|
||
x = 21 + (s16)((float)(x) / 333.87);
|
||
log_info("mpu9250_raw_data.temp_data:%d", x);
|
||
|
||
mpu9250_int_flag = 0;
|
||
}/*}}}*/
|
||
#endif
|
||
imu_busy = 0;
|
||
}
|
||
|
||
s8 mpu9250_dev_init(void *arg)
|
||
{
|
||
if (arg == NULL) {
|
||
log_error("mpu9250 init fail(no arg)\n");
|
||
return -1;
|
||
}
|
||
#if 1//MPU9250_USER_INTERFACE_I2C
|
||
mpu9250_info_data.iic_hdl = ((struct imusensor_platform_data *)arg)->peripheral_hdl;
|
||
mpu9250_info_data.iic_delay = ((struct imusensor_platform_data *)arg)->peripheral_param0; //这个延时并非影响iic的时钟频率，而是2Byte数据之间的延时
|
||
// u8 iic_clk; //iic_clk: <=400kHz
|
||
#else
|
||
mpu9250_info_data.spi_hdl = ((struct imusensor_platform_data *)arg)->peripheral_hdl, //SPIx (role:master)
|
||
mpu9250_info_data.spi_cs_pin = ((struct imusensor_platform_data *)arg)->peripheral_param0; //IO_PORTA_05
|
||
mpu9250_info_data.spi_work_mode = ((struct imusensor_platform_data *)arg)->peripheral_param1; //1:3wire(SPI_MODE_UNIDIR_1BIT) or 0:4wire(SPI_MODE_BIDIR_1BIT) (与spi结构体一样)
|
||
// u8 port; //SPIx group:A,B,C,D (spi结构体)
|
||
// U8 spi_clk; //spi_clk: <=1MHz (spi结构体)
|
||
#endif
|
||
iic_init(mpu9250_info_data.iic_hdl);
|
||
//int module io init
|
||
mpu9250_int_pin = ((struct imusensor_platform_data *)arg)->imu_sensor_int_io;
|
||
gpio_set_direction(mpu9250_int_pin, 1);
|
||
gpio_set_die(mpu9250_int_pin, 1);
|
||
/* gpio_set_pull_up(mpu9250_int_pin, 1); */
|
||
/* gpio_set_pull_down(mpu9250_int_pin, 0); */
|
||
|
||
if (imu_busy) {
|
||
log_error("mpu9250 busy!");
|
||
return -1;
|
||
}
|
||
imu_busy = 1;
|
||
|
||
if (mpu9250_init2(&mpu9250_info_data)) {
|
||
log_info("mpu9250 Device init success!\n");
|
||
log_info("int mode en!");
|
||
/* port_wkup_enable(mpu9250_int_pin, 1, mpu9250_int_callback); //PA08-IO中断，1:下降沿触发，回调函数mpu9250_int_callback*/
|
||
#ifdef CONFIG_CPU_BR23
|
||
io_ext_interrupt_init(mpu9250_int_pin, 1, mpu9250_int_callback);
|
||
#elif defined(CONFIG_CPU_BR28)
|
||
// br28外部中断回调函数，按照现在的外部中断注册方式
|
||
// io配置在板级，定义在板级头文件，这里只是注册回调函数
|
||
/* port_edge_wkup_set_callback_by_index(3, mpu9250_int_callback); // 序号需要和板级配置中的wk_param对应上 */
|
||
port_edge_wkup_set_callback(mpu9250_int_callback);
|
||
#elif defined(CONFIG_CPU_BR27)
|
||
port_edge_wkup_set_callback(mpu9250_int_callback);
|
||
#endif
|
||
init_flag = 1;
|
||
imu_busy = 0;
|
||
return 0;
|
||
} else {
|
||
log_info("mpu9250 Device init fail!\n");
|
||
imu_busy = 0;
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
|
||
int mpu9250_dev_ctl(u8 cmd, void *arg);
|
||
REGISTER_IMU_SENSOR(mpu9250_sensor) = {
|
||
.logo = "mpu9250",
|
||
.imu_sensor_init = mpu9250_dev_init,
|
||
.imu_sensor_check = NULL,
|
||
.imu_sensor_ctl = mpu9250_dev_ctl,
|
||
};
|
||
|
||
int mpu9250_dev_ctl(u8 cmd, void *arg)
|
||
{
|
||
int ret = -1;
|
||
if (init_flag == 0) {
|
||
log_error("mpu9250 init fail!");
|
||
return ret;//0:ok,,<0:err
|
||
}
|
||
if (imu_busy) {
|
||
log_error("mpu9250 busy!");
|
||
return ret;//0:ok,,<0:err
|
||
}
|
||
imu_busy = 1;
|
||
|
||
switch (cmd) {
|
||
case IMU_GET_SENSOR_NAME:
|
||
memcpy((u8 *)arg, &(mpu9250_sensor.logo), 20);
|
||
break;
|
||
case IMU_SENSOR_ENABLE:
|
||
/* cbuf_init(&hrsensor_cbuf, hrsensorcbuf, 24 * sizeof(int)); */
|
||
mpu9250_init2(&mpu9250_info_data);
|
||
break;
|
||
case IMU_SENSOR_DISABLE:
|
||
/* cbuf_clear(&hrsensor_cbuf); */
|
||
break;
|
||
case IMU_SENSOR_RESET:
|
||
break;
|
||
case IMU_SENSOR_SLEEP:
|
||
break;
|
||
case IMU_SENSOR_WAKEUP:
|
||
break;
|
||
case IMU_SENSOR_INT_DET://传感器中断状态检查
|
||
break;
|
||
case IMU_SENSOR_DATA_READY://传感器数据准备就绪待读
|
||
mpu9250_int_callback();
|
||
break;
|
||
case IMU_SENSOR_CHECK_DATA://检查传感器缓存buf是否存满
|
||
break;
|
||
case IMU_SENSOR_READ_DATA://默认读传感器所有数据
|
||
break;
|
||
case IMU_GET_ACCEL_DATA://加速度数据
|
||
break;
|
||
case IMU_GET_GYRO_DATA://陀螺仪数据
|
||
break;
|
||
case IMU_GET_MAG_DATA://磁力计数据
|
||
break;
|
||
case IMU_SENSOR_SEARCH://检查传感器id
|
||
mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_DEVICE_ID_REG, (u8 *)arg, 1); //读取MPU9250的ID
|
||
if (*(u8 *)arg == 0x71) {
|
||
ret = 0;
|
||
log_info("mpu9250 online!\n");
|
||
} else {
|
||
log_error("mpu9250 offline!\n");
|
||
}
|
||
break;
|
||
default:
|
||
log_error("--cmd err!\n");
|
||
break;
|
||
}
|
||
imu_busy = 0;
|
||
return ret;
|
||
}
|
||
|
||
|
||
/***************************mpu9250 test*******************************/
|
||
#if 0
|
||
|
||
static mpu9250_param mpu9250_iic_info_test2 = {
|
||
.iic_hdl = 1,
|
||
.iic_delay = 0, //iic字节间间隔，单位us
|
||
};
|
||
|
||
void port_wkup_irq_cbfun_test(u8 index, u8 gpio)
|
||
{
|
||
mpu9250_int_flag = 1;
|
||
mpu_i2c_buf_read(MPU9250_ADDRESS_W, MPU_ACCEL_XOUTH_REG, read_mpu_buf, SENSORS_MPU_BUFF_LEN);
|
||
}
|
||
void read_mpu_all_reg();
|
||
void mpu9250_test2()
|
||
{
|
||
s16 x = 0, y = 0, z = 0;
|
||
iic_init(mpu9250_iic_info_test2.iic_hdl);
|
||
if (mpu9250_init2(&mpu9250_iic_info_test2)) {
|
||
log_info("mpu9250 Device init pass!\n");
|
||
log_info("-------------------port wkup isr---------------------------");
|
||
/* port_wkup_enable(mpu9250_int_pin, 1, port_wkup_irq_cbfun_test); //PA08-IO中断，1:下降沿触发，回调函数port_wkup_irq_cbfun_test */
|
||
#ifdef CONFIG_CPU_BR23
|
||
io_ext_interrupt_init(mpu9250_int_pin, 1, port_wkup_irq_cbfun_test);
|
||
#elif defined(CONFIG_CPU_BR28)
|
||
// br28外部中断回调函数，按照现在的外部中断注册方式
|
||
// io配置在板级，定义在板级头文件，这里只是注册回调函数
|
||
/* port_edge_wkup_set_callback_by_index(3, port_wkup_irq_cbfun_test); // 序号需要和板级配置中的wk_param对应上 */
|
||
port_edge_wkup_set_callback(port_wkup_irq_cbfun_test);
|
||
#elif defined(CONFIG_CPU_BR27)
|
||
port_edge_wkup_set_callback(port_wkup_irq_cbfun_test);
|
||
#endif
|
||
|
||
/* read_mpu_all_reg(); */
|
||
while (1) {
|
||
if (mpu9250_int_flag) {
|
||
x = (((s16) read_mpu_buf[0]) << 8) | read_mpu_buf[1];
|
||
y = ((((s16) read_mpu_buf[2]) << 8) | read_mpu_buf[3]);
|
||
z = (((s16) read_mpu_buf[4]) << 8) | read_mpu_buf[5];
|
||
log_info("mpu9250_raw_data.accel_data:X:%d,Y:%d,Z:%d", x, y, z);
|
||
x = (((s16) read_mpu_buf[8]) << 8) | read_mpu_buf[9];
|
||
y = (((s16) read_mpu_buf[10]) << 8) | read_mpu_buf[11];
|
||
z = (((s16) read_mpu_buf[12]) << 8) | read_mpu_buf[13];
|
||
log_info("mpu9250_raw_data.gyro_data:X:%d,Y:%d,Z:%d", x, y, z);
|
||
#if MAG_AK8963_ENABLE
|
||
if (read_mpu_buf[0 + SENSORS_MPU6500_BUFF_LEN]&BIT(0)) {
|
||
x = (((s16) read_mpu_buf[2 + SENSORS_MPU6500_BUFF_LEN ]) << 8) | read_mpu_buf[1 + SENSORS_MPU6500_BUFF_LEN ];
|
||
y = (((s16) read_mpu_buf[4 + SENSORS_MPU6500_BUFF_LEN ]) << 8) | read_mpu_buf[3 + SENSORS_MPU6500_BUFF_LEN ];
|
||
z = (((s16) read_mpu_buf[6 + SENSORS_MPU6500_BUFF_LEN ]) << 8) | read_mpu_buf[5 + SENSORS_MPU6500_BUFF_LEN ];
|
||
log_info("mpu9250_raw_data.mag_data:X:%d,Y:%d,Z:%d", x, y, z);
|
||
}
|
||
#endif
|
||
x = (((s16) read_mpu_buf[6]) << 8) | read_mpu_buf[7];
|
||
x = 21 + (s16)((float)(x) / 333.87);
|
||
log_info("mpu9250_raw_data.temp_data:%d", x);
|
||
|
||
mpu9250_int_flag = 0;
|
||
}
|
||
wdt_clear();
|
||
mdelay(10);
|
||
}
|
||
} else {
|
||
log_info("mpu9250 Device init fail!\n");
|
||
}
|
||
}
|
||
|
||
void read_mpu_all_reg()
|
||
{
|
||
u8 addr_tab[62] = {0, 1, 2, 13, 14, 15, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 58, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 114, 115, 116, 117, 119, 120, 122, 123, 125, 126};
|
||
u8 ii = 0;
|
||
u8 temp1_data[62];
|
||
for (ii = 0; ii < 62; ii++) {
|
||
mpu_i2c_buf_read(MPU9250_ADDRESS_W, addr_tab[ii], &temp1_data[ii], 1);
|
||
}
|
||
log_info("addr:%d", addr_tab[61]);
|
||
log_info_hexdump(temp1_data, 62);
|
||
log_info("id:0x%x", temp1_data[55]);
|
||
}
|
||
|
||
#endif
|
||
|
||
|
||
#endif
|
||
|
||
|
||
|
||
|