bool MPUManager::SetAccelOffsetZ( short p_sVal )
{
unsigned char ucEnabled;
m_sAccelOffset[2] = p_sVal;
if( !m_bRunning )
{
if( 0 != mpu_get_dmp_state(&ucEnabled) )
{
printf("mpu_get_dmp_statefailed\n");
return false;
}
if( 0 != ucEnabled && 0 != mpu_set_dmp_state(0) )
{
printf("mpu_set_dmp_state 0 failed\n");
return false;
}
if( !__mpu_set_z_accel_offset(p_sVal) )
{
printf("set_user_z_accel_offset %d failed\n", p_sVal);
}
if( 0 != mpu_set_dmp_state(ucEnabled) )
{
printf("mpu_set_dmp_state 1 failed\n");
}
}
return true;
}
int mympu_open(unsigned int rate) {
mpu_select_device(0);
mpu_init_structures();
ret = mpu_init(NULL);
#ifdef MPU_DEBUG
if (ret) return 10+ret;
#endif
ret = mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);
#ifdef MPU_DEBUG
if (ret) return 20+ret;
#endif
ret = mpu_set_gyro_fsr(FSR);
#ifdef MPU_DEBUG
if (ret) return 30+ret;
#endif
ret = mpu_set_accel_fsr(2);
#ifdef MPU_DEBUG
if (ret) return 40+ret;
#endif
mpu_get_power_state((unsigned char *)&ret);
#ifdef MPU_DEBUG
if (!ret) return 50+ret;
#endif
ret = mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);
#ifdef MPU_DEBUG
if (ret) return 60+ret;
#endif
dmp_select_device(0);
dmp_init_structures();
ret = dmp_load_motion_driver_firmware();
#ifdef MPU_DEBUG
if (ret) return 80+ret;
#endif
ret = dmp_set_fifo_rate(rate);
#ifdef MPU_DEBUG
if (ret) return 90+ret;
#endif
ret = mpu_set_dmp_state(1);
#ifdef MPU_DEBUG
if (ret) return 100+ret;
#endif
ret = dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL);
// ret = dmp_enable_feature(DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL);
#ifdef MPU_DEBUG
if (ret) return 110+ret;
#endif
return 0;
}
/*******************************************************************************
* FUNCTION: ssInitIMU
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ Return 0 if successful.
*
* DESCRIPTIONS:
* Initialized the IMU.
*
*******************************************************************************/
signed short ssInitIMU(void)
{
// Initialize the IMU. Return if failed.
signed short ssErrorCode = mpu_init(NULL);
if (ssErrorCode != 0) {
return ssErrorCode;
}
// Wake up all sensors.
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
// Push both gyro and accel data into the FIFO.
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
/* To initialize the DMP:
* 1. Call dmp_load_motion_driver_firmware(). This pushes the DMP image in
* inv_mpu_dmp_motion_driver.h into the MPU memory.
* 2. Push the gyro and accel orientation matrix to the DMP.
* 3. Register gesture callbacks. Don't worry, these callbacks won't be
* executed unless the corresponding feature is enabled.
* 4. Call dmp_enable_feature(mask) to enable different features.
* 5. Call dmp_set_fifo_rate(freq) to select a DMP output rate.
* 6. Call any feature-specific control functions.
*
* To enable the DMP, just call mpu_set_dmp_state(1). This function can
* be called repeatedly to enable and disable the DMP at runtime.
*
* The following is a short summary of the features supported in the DMP
* image provided in inv_mpu_dmp_motion_driver.c:
* DMP_FEATURE_LP_QUAT: Generate a gyro-only quaternion on the DMP at
* 200Hz. Integrating the gyro data at higher rates reduces numerical
* errors (compared to integration on the MCU at a lower sampling rate).
* DMP_FEATURE_6X_LP_QUAT: Generate a gyro/accel quaternion on the DMP at
* 200Hz. Cannot be used in combination with DMP_FEATURE_LP_QUAT.
* DMP_FEATURE_TAP: Detect taps along the X, Y, and Z axes.
* DMP_FEATURE_ANDROID_ORIENT: Google's screen rotation algorithm. Triggers
* an event at the four orientations where the screen should rotate.
* DMP_FEATURE_GYRO_CAL: Calibrates the gyro data after eight seconds of
* no motion.
* DMP_FEATURE_SEND_RAW_ACCEL: Add raw accelerometer data to the FIFO.
* DMP_FEATURE_SEND_RAW_GYRO: Add raw gyro data to the FIFO.
* DMP_FEATURE_SEND_CAL_GYRO: Add calibrated gyro data to the FIFO. Cannot
* be used in combination with DMP_FEATURE_SEND_RAW_GYRO.
*/
dmp_load_motion_driver_firmware();
/*
* Known Bug -
* DMP when enabled will sample sensor data at 200Hz and output to FIFO at the rate
* specified in the dmp_set_fifo_rate API. The DMP will then sent an interrupt once
* a sample has been put into the FIFO. Therefore if the dmp_set_fifo_rate is at 25Hz
* there will be a 25Hz interrupt from the MPU device.
*
* There is a known issue in which if you do not enable DMP_FEATURE_TAP
* then the interrupts will be at 200Hz even if fifo rate
* is set at a different rate. To avoid this issue include the DMP_FEATURE_TAP
*/
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_GYRO_CAL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_SEND_RAW_ACCEL);
dmp_set_fifo_rate(10);
mpu_set_dmp_state(1);
}
int init(){
open_bus();
unsigned char whoami=0;
i2c_read(MPU6050_ADDR, MPU6050_WHO_AM_I, 1, &whoami);
if(!silent_flag) {
printf("WHO_AM_I: %x\n", whoami);
}
struct int_param_s int_param;
if(!silent_flag) {
printf("MPU init: %i\n", mpu_init(&int_param));
printf("MPU sensor init: %i\n", mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL));
printf("MPU configure fifo: %i\n", mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL));
printf("DMP firmware: %i\n ",dmp_load_motion_driver_firmware());
printf("DMP orientation: %i\n ",dmp_set_orientation(
inv_orientation_matrix_to_scalar(gyro_orientation)));
}
unsigned short dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;
if(!silent_flag) {
printf("DMP feature enable: %i\n", dmp_enable_feature(dmp_features));
printf("DMP set fifo rate: %i\n", dmp_set_fifo_rate(DEFAULT_MPU_HZ));
printf("DMP enable %i\n", mpu_set_dmp_state(1));
}
if(!no_interrupt_flag) {
mpu_set_int_level(1); // Interrupt is low when firing
dmp_set_interrupt_mode(DMP_INT_CONTINUOUS); // Fire interrupt on new FIFO value
}
return 0;
}
/**
* @brief mpu6050_init
* @param void
* @retval 成功为0
* @note 初始化mpu6050 频率为100*1000HZ
*/
int16_t mpu6050_init(void)
{
I2C_QuickInit(I2C0_SCL_PB02_SDA_PB03,100*1000);
int result=0;
result=mpu_init();
if(!result)
{
PrintChar("mpu initialization complete......\n "); //mpu initialization complete
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_set_sensor
PrintChar("mpu_set_sensor complete ......\n");
else
PrintChar("mpu_set_sensor come across error ......\n");
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo
PrintChar("mpu_configure_fifo complete ......\n");
else
PrintChar("mpu_configure_fifo come across error ......\n");
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //mpu_set_sample_rate
PrintChar("mpu_set_sample_rate complete ......\n");
else
PrintChar("mpu_set_sample_rate error ......\n");
if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare
PrintChar("dmp_load_motion_driver_firmware complete ......\n");
else
PrintChar("dmp_load_motion_driver_firmware come across error ......\n");
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation
PrintChar("dmp_set_orientation complete ......\n");
else
PrintChar("dmp_set_orientation come across error ......\n");
if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL)) //dmp_enable_feature
PrintChar("dmp_enable_feature complete ......\n");
else
PrintChar("dmp_enable_feature come across error ......\n");
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //dmp_set_fifo_rate
PrintChar("dmp_set_fifo_rate complete ......\n");
else
PrintChar("dmp_set_fifo_rate come across error ......\n");
run_self_test(); //自检
if(!mpu_set_dmp_state(1))
PrintChar("mpu_set_dmp_state complete ......\n");
else
PrintChar("mpu_set_dmp_state come across error ......\n");
}
return 0;
}
/**************************************************************************
函数功能：MPU6050内置DMP的初始化
入口参数：无
返回 值：无
作 者：平衡小车之家
**************************************************************************/
void DMP_Init(void)
{
u8 temp[1]={0};
i2cRead(0x68,0x75,1,temp);
printf("mpu_set_sensor complete ......\r\n");
if(temp[0]!=0x68)NVIC_SystemReset();
if(!mpu_init())
{
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
printf("mpu_set_sensor complete ......\r\n");
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
printf("mpu_configure_fifo complete ......\r\n");
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))
printf("mpu_set_sample_rate complete ......\r\n");
if(!dmp_load_motion_driver_firmware())
printf("dmp_load_motion_driver_firmware complete ......\r\n");
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
printf("dmp_set_orientation complete ......\r\n");
if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL))
printf("dmp_enable_feature complete ......\r\n");
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))
printf("dmp_set_fifo_rate complete ......\r\n");
run_self_test();
if(!mpu_set_dmp_state(1))
printf("mpu_set_dmp_state complete ......\r\n");
}
}
void mpulib_exit() {
// turn off the DMP on exit
if (mpu_set_dmp_state(0))
printf("mpu_set_dmp_state(0) failed\n");
// TODO: Should turn off the sensors too
}
void init_mpu6050(struct eeprom *e)
{
int result=0;
die_if(eeprom_open("/dev/i2c/0", 0x68, EEPROM_TYPE_8BIT_ADDR, e) < 0,
"unable to open MPU6050 device file ");
result=mpu_init();
if(!result)
{
printf("mpu initialization complete......\n "); //mpu initialization complete
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_set_sensor
printf("mpu_set_sensor complete ......\n");
else
printf("mpu_set_sensor come across error ......\n");
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo
printf("mpu_configure_fifo complete ......\n");
else
printf("mpu_configure_fifo come across error ......\n");
if(!mpu_set_sample_rate(50)) //mpu_set_sample_rate
printf("mpu_set_sample_rate complete ......\n");
else
printf("mpu_set_sample_rate error ......\n");
if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare
printf("dmp_load_motion_driver_firmware complete ......\n");
else
printf("dmp_load_motion_driver_firmware come across error ......\n");
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation
printf("dmp_set_orientation complete ......\n");
else
printf("dmp_set_orientation come across error ......\n");
if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL)) //dmp_enable_feature
printf("dmp_enable_feature complete ......\n");
else
printf("dmp_enable_feature come across error ......\n");
if(!dmp_set_fifo_rate(50)) //dmp_set_fifo_rate
printf("dmp_set_fifo_rate complete ......\n");
else
printf("dmp_set_fifo_rate come across error ......\n");
run_self_test(); //自检
if(!mpu_set_dmp_state(1))
printf("mpu_set_dmp_state complete ......\n");
else
printf("mpu_set_dmp_state come across error ......\n");
}
else
{
printf("mpu INIT INIT INIT error ......\r\n");
}
}
/******************************************************************************
* 函数名称: MPU6050_Dmp_Init
* 函数功能: MPU dmp初始化
* 入口参数: 无
* 返回值 无
******************************************************************************/
void MPU9250_Dmp_Init(void)
{
SPI_Init();
mpu_set_lpf(5);
while(mpu_init());
//mpu_set_sensor
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
//mpu_configure_fifo
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
//mpu_set_sample_rate
mpu_set_sample_rate(DEFAULT_MPU_HZ);
//dmp_load_motion_driver_firmvare
dmp_load_motion_driver_firmware();
//dmp_set_orientation
dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
//dmp_enable_feature
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL);
//dmp_set_fifo_rate
dmp_set_fifo_rate(DEFAULT_MPU_HZ);
//run_self_test();
mpu_set_dmp_state(1);
}
inv_error_t MPU9250_DMP::dmpBegin(unsigned short features, unsigned short fifoRate)
{
unsigned short feat = features;
unsigned short rate = fifoRate;
if (dmpLoad() != INV_SUCCESS)
return INV_ERROR;
// 3-axis and 6-axis LP quat are mutually exclusive.
// If both are selected, default to 3-axis
if (feat & DMP_FEATURE_LP_QUAT)
{
feat &= ~(DMP_FEATURE_6X_LP_QUAT);
dmp_enable_lp_quat(1);
}
else if (feat & DMP_FEATURE_6X_LP_QUAT)
dmp_enable_6x_lp_quat(1);
if (feat & DMP_FEATURE_GYRO_CAL)
dmp_enable_gyro_cal(1);
if (dmpEnableFeatures(feat) != INV_SUCCESS)
return INV_ERROR;
rate = constrain(rate, 1, 200);
if (dmpSetFifoRate(rate) != INV_SUCCESS)
return INV_ERROR;
return mpu_set_dmp_state(1);
}
//// MPU9150 IMU ////
int initialize_imu(int sample_rate, signed char orientation[9]){
printf("Initializing IMU\n");
//set up gpio interrupt pin connected to imu
if(gpio_export(INTERRUPT_PIN)){
printf("can't export gpio %d \n", INTERRUPT_PIN);
return (-1);
}
gpio_set_dir(INTERRUPT_PIN, INPUT_PIN);
gpio_set_edge(INTERRUPT_PIN, "falling"); // Can be rising, falling or both
linux_set_i2c_bus(1);
if (mpu_init(NULL)) {
printf("\nmpu_init() failed\n");
return -1;
}
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
mpu_set_sample_rate(sample_rate);
// compass run at 100hz max.
if(sample_rate > 100){
// best to sample at a fraction of the gyro/accel
mpu_set_compass_sample_rate(sample_rate/2);
}
else{
mpu_set_compass_sample_rate(sample_rate);
}
mpu_set_lpf(188); //as little filtering as possible
dmp_load_motion_driver_firmware(sample_rate);
dmp_set_orientation(inv_orientation_matrix_to_scalar(orientation));
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL);
dmp_set_fifo_rate(sample_rate);
if (mpu_set_dmp_state(1)) {
printf("\nmpu_set_dmp_state(1) failed\n");
return -1;
}
if(loadGyroCalibration()){
printf("\nGyro Calibration File Doesn't Exist Yet\n");
printf("Use calibrate_gyro example to create one\n");
printf("Using 0 offset for now\n");
};
// set the imu_interrupt_thread to highest priority since this is used
// for real-time control with time-sensitive IMU data
set_imu_interrupt_func(&null_func);
pthread_t imu_interrupt_thread;
struct sched_param params;
pthread_create(&imu_interrupt_thread, NULL, imu_interrupt_handler, (void*) NULL);
params.sched_priority = sched_get_priority_max(SCHED_FIFO);
pthread_setschedparam(imu_interrupt_thread, SCHED_FIFO, ¶ms);
return 0;
}
void __exit mpu9150_exit()
{
// turn off the DMP on exit
if (mpu_set_dmp_state(0))
printk(KERN_WARNING"mpu_set_dmp_state(0) failed\n");
// TODO: Should turn off the sensors too
}
int INVMPU9150::dispose() {
// Set dmp status to off
if (mpu_set_dmp_state(0)) {
return MPU9150_ERROR;
} else {
return MPU9150_OK;
}
}
int8_t IMUTask::enable_dmp()
{
int8_t result = 0;
result = mpu_set_dmp_state(1);
debug::log("IMUTask: mpu_set_dmp_state(1) returned " + String(result));
if (result != 0) {
return -1;
}
dmp_state = IMU_DMP_ON;
return 0;
}
/**********************************MPU6050*************************************/
void MPU6050_INIT()
{
mpu_init();
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);//#(INV_XYZ_GYRO | INV_XYZ_ACCEL)在inv_mpu.h定义
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sample_rate(DEFAULT_MPU_HZ);//#本文件开头定义
dmp_load_motion_driver_firmware();
dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//#本文定义
dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL |
DMP_FEATURE_SEND_CAL_GYRO |DMP_FEATURE_GYRO_CAL;//#全部在(inv_mpu_dmp_motion_driver.h)中定义
dmp_enable_feature(dmp_features);
dmp_set_fifo_rate(DEFAULT_MPU_HZ);
run_self_test();
mpu_set_dmp_state(1);
}
/**
****************************************************************************************
* @brief start mpu6050 dmp
* @description
* This function is used to initialize mpu6050 dmp.
*****************************************************************************************
*/
void mpu_start()
{
int result = mpu_init(NULL);
while(result != MPU_OK)
{
result = mpu_init(NULL);
}
if(result == MPU_OK)
{
printf("mpu initialization complete......\n "); //mpu_set_sensor
if(mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) == MPU_OK)
printf("mpu_set_sensor complete ......\n");
else
printf("mpu_set_sensor come across error ......\n");
if(mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL) == MPU_OK) //mpu_configure_fifo
printf("mpu_configure_fifo complete ......\n");
else
printf("mpu_configure_fifo come across error ......\n");
if(mpu_set_sample_rate(DEFAULT_MPU_HZ) == MPU_OK) //mpu_set_sample_rate
printf("mpu_set_sample_rate complete ......\n");
else
printf("mpu_set_sample_rate error ......\n");
if(dmp_load_motion_driver_firmware() == MPU_OK) //dmp_load_motion_driver_firmvare
printf("dmp_load_motion_driver_firmware complete ......\n");
else
printf("dmp_load_motion_driver_firmware come across error ......\n");
if(dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)) == MPU_OK) //dmp_set_orientation
printf("dmp_set_orientation complete ......\n");
else
printf("dmp_set_orientation come across error ......\n");
if(dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL) == MPU_OK) //dmp_enable_feature
printf("dmp_enable_feature complete ......\n");
else
printf("dmp_enable_feature come across error ......\n");
if(dmp_set_fifo_rate(DEFAULT_MPU_HZ) == MPU_OK) //dmp_set_fifo_rate
printf("dmp_set_fifo_rate complete ......\n");
else
printf("dmp_set_fifo_rate come across error ......\n");
run_self_test();
if(mpu_set_dmp_state(1) == MPU_OK)
printf("mpu_set_dmp_state complete ......\n");
else
printf("mpu_set_dmp_state come across error ......\n");
}
}
void dmp_init()
{
Timer4_init();
while (1 == mpu_init());
//mpu_set_sensor
while (1 == mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL));
//mpu_configure_fifo
while (1 == mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL));
//mpu_set_sample_rate
while (1 == mpu_set_sample_rate(DEFAULT_MPU_HZ));
//dmp_load_motion_driver_firmvare
while (1 == dmp_load_motion_driver_firmware());
//dmp_set_orientation
while (1 == dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)));
//dmp_enable_feature
while (1 == dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL));
//dmp_set_fifo_rate
while (1 == dmp_set_fifo_rate(DEFAULT_MPU_HZ));
run_self_test();
while (1 == mpu_set_dmp_state(1));
rt_kprintf("start mpu6050\n");
rt_kprintf("start ahrs\n");
}
/**
* @brief MPU （6500）初始化
* @param None
* @retval None
*/
void InvMPU_Driver_Init(void)
{
u8 ret = 0;
long accel[3];
struct int_param_s int_param;
assert_param(MPU_SPI == &hspi1);
MPU_SPIDeselect();
ret += mpu_init(&int_param);
ret += mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
ret += mpu_set_sample_rate(1000);
ret += mpu_set_gyro_fsr( GYRO_FSR);
ret += mpu_set_accel_fsr( ACCEL_FSR);
accel[0] = 46;
accel[1] = 78;
accel[2] = 0;
mpu_set_accel_bias_6500_reg(accel);
#ifdef USE_DMP
dmp_load_motion_driver_firmware();
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_GYRO_CAL |
DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_SEND_RAW_ACCEL);
dmp_set_fifo_rate(200);
u8 offset[12] = {0,0, 0xd2, 0x2b, 0x00, 0x01, 0xba, 0xd8, 0x00, 0x00, 0x9f, 0xa0};
mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, &offset[0]);
mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, &offset[4]);
mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, &offset[8]);
mpu_set_dmp_state(1);
#endif
}
void MPUManager::Stop()
{
if( 0 != m_iThreadID )
{
m_bStop = true;
pthread_join( m_iThreadID, 0 );
}
if( 0 != mpu_set_dmp_state(0) )
{
printf("mpu_set_dmp_state 0 failed\n");
}
if( 0 != __mpu_set_sleep_mode(1) )
{
printf("__mpu_set_sleep_mode 1 failed\n");
}
m_bRunning = false;
m_bStop = false;
m_iThreadID = 0;
printf("DMP stopped\n");
}
int mpulib_init(int i2c_bus, int sample_rate, int mix_factor) {
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
if (i2c_bus < MIN_I2C_BUS || i2c_bus > MAX_I2C_BUS) {
printf("Invalid I2C bus %d\n", i2c_bus);
return -1;
}
if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) {
printf("Invalid sample rate %d\n", sample_rate);
return -1;
}
if (mix_factor < 0 || mix_factor > 100) {
printf("Invalid mag mixing factor %d\n", mix_factor);
return -1;
}
yaw_mixing_factor = mix_factor;
linux_set_i2c_bus(i2c_bus);
printf("\nInitializing IMU .");
fflush(stdout);
if (mpu_init(NULL)) {
printf("\nmpu_init() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
printf("\nmpu_set_sensors() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
printf("\nmpu_configure_fifo() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_set_sample_rate(sample_rate)) {
printf("\nmpu_set_sample_rate() failed\n");
return -1;
}
printf(".");
fflush(stdout);
int compass_sample_rate = sample_rate;
if( compass_sample_rate > MAX_COMPASS_SAMPLE_RATE ) {
compass_sample_rate = MAX_COMPASS_SAMPLE_RATE;
}
if (mpu_set_compass_sample_rate(compass_sample_rate)) {
printf("\nmpu_set_compass_sample_rate() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_load_motion_driver_firmware()) {
printf("\ndmp_load_motion_driver_firmware() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
printf("\ndmp_set_orientation() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printf("\ndmp_enable_feature() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_set_fifo_rate(sample_rate)) {
printf("\ndmp_set_fifo_rate() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_set_dmp_state(1)) {
printf("\nmpu_set_dmp_state(1) failed\n");
return -1;
}
printf(" done\n\n");
return 0;
}
int INVMPU9150::init(int i2cBus, int frequency) {
// Set linux i2c bus
linux_set_i2c_bus(i2cBus);
// The gyro orientation
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
// Init MPU
if (mpu_init(NULL)) {
return MPU9150_INIT_MPU_INIT_ERROR;
}
// Set sensors
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
return MPU9150_INIT_MPU_SET_SENSORS_ERROR;
}
// Get gyro sensitivity scale factor and convert from [LSB/(degree/s)] to [LSB/(rad/s)]
float gyroSsfDegreePerSecond;
if (mpu_get_gyro_sens(& gyroSsfDegreePerSecond)) {
return MPU9150_INIT_MPU_GET_GYRO_FSR_ERROR;
}
gyroSsf = gyroSsfDegreePerSecond * 180 / M_PI;
// Get accel sensitivity scale factor
if (mpu_get_accel_sens(& accelSsf)) {
return MPU9150_INIT_MPU_GET_ACCEL_FSR_ERROR;
}
// Configure FIFO
if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
return MPU9150_INIT_MPU_CONFIGURE_FIFO_ERROR;
}
// Set sample rate for gyro/acc
if (mpu_set_sample_rate(frequency)) {
return MPU9150_INIT_MPU_SET_SAMPLE_RATE_ERROR;
}
// Load motion driver firmare (DMP)
if (dmp_load_motion_driver_firmware()) {
return MPU9150_INIT_DMP_LOAD_MOTION_DRIVER_FIRMWARE_ERROR;
}
// Set gyro orientation
if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
return MPU9150_INIT_DMP_SET_ORIENTATION_ERROR;
}
// Enable DMP features
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
return MPU9150_INIT_DMP_ENABLE_FEATURES_ERROR;
}
// Set sample rate for DMP
if (dmp_set_fifo_rate(frequency)) {
return MPU9150_INIT_DMP_SET_FIFO_RATE_ERROR;
}
// Enable DMP
if (mpu_set_dmp_state(1)) {
return MPU9150_INIT_DMP_SET_STATE_ERROR;
}
return MPU9150_OK;
}
int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor)
{
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) {
printf("Invalid sample rate %d\n", sample_rate);
return -1;
}
if (mix_factor < 0 || mix_factor > 100) {
printf("Invalid mag mixing factor %d\n", mix_factor);
return -1;
}
yaw_mixing_factor = mix_factor;
linux_set_i2c_bus(i2c_bus);
printf("\nInitializing IMU .");
if (mpu_init(NULL)) {
printf("\nmpu_init() failed\n");
return -1;
}
printf(".");
#if defined AK8963_SECONDARY || defined AK8975_SECONDARY
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
printf("\nmpu_set_sensors() failed\n");
return -1;
}
#else
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
printf("\nmpu_set_sensors() failed\n");
return -1;
}
#endif
printf(".");
if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
printf("\nmpu_configure_fifo() failed\n");
return -1;
}
printf(".");
if (mpu_set_sample_rate(200)) {
printf("\nmpu_set_sample_rate() failed\n");
return -1;
}
printf(".");
#if defined AK8963_SECONDARY || defined AK8975_SECONDARY
if (mpu_set_compass_sample_rate(50)) {
printf("\nmpu_set_compass_sample_rate() failed\n");
return -1;
}
#endif
printf(".");
if (dmp_load_motion_driver_firmware()) {
printf("\ndmp_load_motion_driver_firmware() failed\n");
return -1;
}
printf(".");
if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
printf("\ndmp_set_orientation() failed\n");
return -1;
}
printf(".");
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printf("\ndmp_enable_feature() failed\n");
return -1;
}
printf(".");
if (dmp_set_fifo_rate(sample_rate)) {
printf("\ndmp_set_fifo_rate() failed\n");
return -1;
}
printf(".");
if (mpu_set_dmp_state(1)) {
printf("\nmpu_set_dmp_state(1) failed\n");
return -1;
}
printf(" done\n\n");
return 0;
}
boolean MPU9150Lib::init(int mpuRate, int magMix)
{
struct int_param_s int_param;
int result;
long accelOffset[3];
m_magMix = magMix;
m_lastDMPYaw = 0;
m_lastYaw = 0;
// get calibration data if it's there
if (calSensRead(&m_calData)) { // use calibration data if it's there and wanted
m_useMagCalibration &= m_calData.magValid;
m_useAccelCalibration &= m_calData.accelValid;
// Process calibration data for runtime
if (m_useMagCalibration) {
m_magXOffset = (short)(((long)m_calData.magMaxX + (long)m_calData.magMinX) / 2);
m_magXRange = m_calData.magMaxX - m_magXOffset;
m_magYOffset = (short)(((long)m_calData.magMaxY + (long)m_calData.magMinY) / 2);
m_magYRange = m_calData.magMaxY - m_magYOffset;
m_magZOffset = (short)(((long)m_calData.magMaxZ + (long)m_calData.magMinZ) / 2);
m_magZRange = m_calData.magMaxZ - m_magZOffset;
}
if (m_useAccelCalibration) {
accelOffset[0] = -((long)m_calData.accelMaxX + (long)m_calData.accelMinX) / 2;
accelOffset[1] = -((long)m_calData.accelMaxY + (long)m_calData.accelMinY) / 2;
accelOffset[2] = -((long)m_calData.accelMaxZ + (long)m_calData.accelMinZ) / 2;
mpu_set_accel_bias(accelOffset);
m_accelXRange = m_calData.accelMaxX + accelOffset[0];
m_accelYRange = m_calData.accelMaxY + accelOffset[1];
m_accelZRange = m_calData.accelMaxZ + accelOffset[2];
}
}
#ifdef MPULIB_DEBUG
if (m_useMagCalibration)
Serial.println("Using mag cal");
if (m_useAccelCalibration)
Serial.println("Using accel cal");
#endif
mpu_init_structures();
// Not using interrupts so set up this structure to keep the driver happy
int_param.cb = NULL;
int_param.pin = 0;
int_param.lp_exit = 0;
int_param.active_low = 1;
result = mpu_init(&int_param);
if (result != 0) {
#ifdef MPULIB_DEBUG
Serial.print("mpu_init failed with code: ");
Serial.println(result);
#endif
return false;
}
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); // enable all of the sensors
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); // get accel and gyro data in the FIFO also
mpu_set_sample_rate(mpuRate); // set the update rate
mpu_set_compass_sample_rate(mpuRate); // set the compass update rate to match
#ifdef MPULIB_DEBUG
Serial.println("Loading firmware");
#endif
if ((result = dmp_load_motion_driver_firmware()) != 0) { // try to load the DMP firmware
#ifdef MPULIB_DEBUG
Serial.print("Failed to load dmp firmware: ");
Serial.println(result);
#endif
return false;
}
dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); // set up the correct orientation
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL);
dmp_set_fifo_rate(mpuRate);
if (mpu_set_dmp_state(1) != 0) {
#ifdef MPULIB_DEBUG
Serial.println("mpu_set_dmp_state failed");
#endif
return false;
}
return true;
}
int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor)
{
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
if (i2c_bus < 0 || i2c_bus > 3)
return -1;
if (sample_rate < 2 || sample_rate > 50)
return -1;
if (mix_factor < 0 || mix_factor > 100)
return -1;
yaw_mixing_factor = mix_factor;
linux_set_i2c_bus(i2c_bus);
printf("\nInitializing IMU .");
fflush(stdout);
if (mpu_init(NULL)) {
printf("\nmpu_init() failed\n");
return -1;
}
printf(".");
fflush(stdout);
#ifdef AK89xx_SECONDARY
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
#else
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
#endif
printf("\nmpu_set_sensors() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
printf("\nmpu_configure_fifo() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_set_sample_rate(sample_rate)) {
printf("\nmpu_set_sample_rate() failed\n");
return -1;
}
printf(".");
fflush(stdout);
#ifdef AK89xx_SECONDARY
if (mpu_set_compass_sample_rate(sample_rate)) {
printf("\nmpu_set_compass_sample_rate() failed\n");
return -1;
}
#endif
printf(".");
fflush(stdout);
if (dmp_load_motion_driver_firmware()) {
printf("\ndmp_load_motion_driver_firmware() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
printf("\ndmp_set_orientation() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printf("\ndmp_enable_feature() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_set_fifo_rate(sample_rate)) {
printf("\ndmp_set_fifo_rate() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_set_dmp_state(1)) {
printf("\nmpu_set_dmp_state(1) failed\n");
return -1;
}
printf(" done\n\n");
return 0;
}
/* New functions to enable / disable 6axis on the fly */
int enableAccelerometerFusion(void) {
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printf("Failure enabling accelerometer fusion\n");
return -1;
}
printf("mpu9150.c: Accelerometer fusion enabled\n");
return 0;
}
int disableAccelerometerFusion(void) {
if (dmp_enable_feature(DMP_FEATURE_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printf("Failure disabling accelerometer fusion\n");
return -1;
}
printf("mpu9150.c: Accelerometer fusion disabled\n");
return 0;
}
void mpu9150_exit()
{
// turn off the DMP on exit
if (mpu_set_dmp_state(0))
printf("mpu_set_dmp_state(0) failed\n");
// TODO: Should turn off the sensors too
}
void mpu9150_set_accel_cal(caldata_t *cal)
{
int i;
long bias[3];
if (!cal) {
use_accel_cal = 0;
return;
}
memcpy(&accel_cal_data, cal, sizeof(caldata_t));
for (i = 0; i < 3; i++) {
if (accel_cal_data.range[i] < 1)
accel_cal_data.range[i] = 1;
else if (accel_cal_data.range[i] > ACCEL_SENSOR_RANGE)
accel_cal_data.range[i] = ACCEL_SENSOR_RANGE;
bias[i] = -accel_cal_data.offset[i];
}
if (debug_on) {
printf("\naccel cal (range : offset)\n");
for (i = 0; i < 3; i++)
printf("%d : %d\n", accel_cal_data.range[i], accel_cal_data.offset[i]);
}
mpu_set_accel_bias(bias);
use_accel_cal = 1;
}
int ms_open() {
dmpReady=1;
initialized = 0;
for (int i=0;i<DIM;i++){
lastval[i]=10;
}
// initialize device
printf("Initializing MPU...\n");
if (mpu_init(NULL) != 0) {
printf("MPU init failed!\n");
return -1;
}
printf("Setting MPU sensors...\n");
if (mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) {
printf("Failed to set sensors!\n");
return -1;
}
printf("Setting GYRO sensitivity...\n");
if (mpu_set_gyro_fsr(2000)!=0) {
printf("Failed to set gyro sensitivity!\n");
return -1;
}
printf("Setting ACCEL sensitivity...\n");
if (mpu_set_accel_fsr(2)!=0) {
printf("Failed to set accel sensitivity!\n");
return -1;
}
// verify connection
printf("Powering up MPU...\n");
mpu_get_power_state(&devStatus);
printf(devStatus ? "MPU6050 connection successful\n" : "MPU6050 connection failed %u\n",devStatus);
//fifo config
printf("Setting MPU fifo...\n");
if (mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) {
printf("Failed to initialize MPU fifo!\n");
return -1;
}
// load and configure the DMP
printf("Loading DMP firmware...\n");
if (dmp_load_motion_driver_firmware()!=0) {
printf("Failed to enable DMP!\n");
return -1;
}
printf("Activating DMP...\n");
if (mpu_set_dmp_state(1)!=0) {
printf("Failed to enable DMP!\n");
return -1;
}
//dmp_set_orientation()
//if (dmp_enable_feature(DMP_FEATURE_LP_QUAT|DMP_FEATURE_SEND_RAW_GYRO)!=0) {
printf("Configuring DMP...\n");
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL)!=0) {
printf("Failed to enable DMP features!\n");
return -1;
}
printf("Setting DMP fifo rate...\n");
if (dmp_set_fifo_rate(rate)!=0) {
printf("Failed to set dmp fifo rate!\n");
return -1;
}
printf("Resetting fifo queue...\n");
if (mpu_reset_fifo()!=0) {
printf("Failed to reset fifo!\n");
return -1;
}
printf("Checking... ");
do {
delay_ms(1000/rate); //dmp will habve 4 (5-1) packets based on the fifo_rate
r=dmp_read_fifo(g,a,_q,&sensors,&fifoCount);
} while (r!=0 || fifoCount<5); //packtets!!!
printf("Done.\n");
initialized = 1;
return 0;
}
int ms_open(unsigned int rate) {
dmpReady=1;
initialized = 0;
// initialize device
printf("Initializing MPU...\n");
r=mpu_init(NULL);
if (r != 0) {
printf("MPU init failed! %i\n",r);
return -1;
}
printf("Setting MPU sensors...\n");
if (mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) {
printf("Failed to set sensors!\n");
return -1;
}
printf("Setting GYRO sensitivity...\n");
if (mpu_set_gyro_fsr(FSR)!=0) {
printf("Failed to set gyro sensitivity!\n");
return -1;
}
printf("Setting ACCEL sensitivity...\n");
if (mpu_set_accel_fsr(2)!=0) {
printf("Failed to set accel sensitivity!\n");
return -1;
}
// verify connection
printf("Powering up MPU...\n");
mpu_get_power_state(&devStatus);
printf(devStatus ? "MPU6050 connection successful\n" : "MPU6050 connection failed %u\n",devStatus);
//fifo config
printf("Setting MPU fifo...\n");
if (mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) {
printf("Failed to initialize MPU fifo!\n");
return -1;
}
// load and configure the DMP
printf("Loading DMP firmware...\n");
if (dmp_load_motion_driver_firmware()!=0) {
printf("Failed to load DMP firmware!\n");
return -1;
}
printf("Setting GYRO orientation...\n");
if (dmp_set_orientation( inv_orientation_matrix_to_scalar( gyro_orientation ) )!=0) {
printf("Failed to set gyro orientation!\n");
return -1;
}
printf("Setting DMP fifo rate to: %i\n",rate);
if (dmp_set_fifo_rate(rate)!=0) {
printf("Failed to set dmp fifo rate!\n");
return -1;
}
printf("Activating DMP...\n");
if (mpu_set_dmp_state(1)!=0) {
printf("Failed to enable DMP!\n");
return -1;
}
//dmp_set_orientation()
//if (dmp_enable_feature(DMP_FEATURE_LP_QUAT|DMP_FEATURE_SEND_RAW_GYRO)!=0) {
printf("Configuring DMP...\n");
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL)!=0) {
//if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_RAW_GYRO)!=0) {
printf("Failed to enable DMP features!\n");
return -1;
}
printf("Resetting fifo queue...\n");
if (mpu_reset_fifo()!=0) {
printf("Failed to reset fifo!\n");
return -1;
}
printf("Checking... ");
do {
delay_ms(5*1000/rate); //dmp will habve 4 (5-1) packets based on the fifo_rate
r=dmp_read_fifo(g,a,_q,&sensors,&fifoCount);
} while (r!=0 || fifoCount<5); //packtets!!!
printf("Collected: %i packets.\n",fifoCount);
ms_update();
initialized = 1;
uint16_t lpf,g_fsr,a_sens,dmp_rate;
uint8_t dmp,a_fsr;
float g_sens;
printf("MPU config:\n");
mpu_get_lpf(&lpf);
printf("MPU LPF: %u\n",lpf);
dmp_get_fifo_rate(&dmp_rate);
printf("DMP Fifo Rate: %u\n",dmp_rate);
mpu_get_dmp_state(&dmp);
printf("MPU DMP State: %u\n",dmp);
mpu_get_gyro_fsr(&g_fsr);
printf("MPU gyro FSR: %u\n",g_fsr);
mpu_get_gyro_sens(&g_sens);
printf("MPU gyro sens: %2.3f\n",g_sens);
mpu_get_accel_fsr(&a_fsr);
printf("MPU accel FSR: %u\n",a_fsr);
mpu_get_accel_sens(&a_sens);
printf("MPU accel sens: %u\n",a_sens);
return 0;
}
int mpu6050_init()
{
/*
** Configures I2C to Master mode to generate start
** condition on I2C bus and to transmit data at a
** speed of 100khz
*/
SetupI2C();
rt_hw_interrupt_install(SYS_INT_I2C0INT, mpu6050_isr, NULL, "mpu6050");
rt_hw_interrupt_mask(SYS_INT_I2C0INT);
int result = mpu_init();
if(!result)
{
rt_kprintf("mpu initialization complete......\n "); //mpu_set_sensor
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
rt_kprintf("mpu_set_sensor complete ......\n");
else
rt_kprintf("mpu_set_sensor come across error ......\n");
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo
rt_kprintf("mpu_configure_fifo complete ......\n");
else
rt_kprintf("mpu_configure_fifo come across error ......\n");
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //mpu_set_sample_rate
rt_kprintf("mpu_set_sample_rate complete ......\n");
else
rt_kprintf("mpu_set_sample_rate error ......\n");
if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare
rt_kprintf("dmp_load_motion_driver_firmware complete ......\n");
else
rt_kprintf("dmp_load_motion_driver_firmware come across error ......\n");
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation
rt_kprintf("dmp_set_orientation complete ......\n");
else
rt_kprintf("dmp_set_orientation come across error ......\n");
if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL)) //dmp_enable_feature
rt_kprintf("dmp_enable_feature complete ......\n");
else
rt_kprintf("dmp_enable_feature come across error ......\n");
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //dmp_set_fifo_rate
rt_kprintf("dmp_set_fifo_rate complete ......\n");
else
rt_kprintf("dmp_set_fifo_rate come across error ......\n");
run_self_test();
if(!mpu_set_dmp_state(1))
rt_kprintf("mpu_set_dmp_state complete ......\n");
else
rt_kprintf("mpu_set_dmp_state come across error ......\n");
}
while(1)
{
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
&more);
/* Gyro and accel data are written to the FIFO by the DMP in chip
* frame and hardware units. This behavior is convenient because it
* keeps the gyro and accel outputs of dmp_read_fifo and
* mpu_read_fifo consistent.
*/
/* if (sensors & INV_XYZ_GYRO )
send_packet(PACKET_TYPE_GYRO, gyro);
if (sensors & INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL, accel); */
/* Unlike gyro and accel, quaternions are written to the FIFO in
* the body frame, q30. The orientation is set by the scalar passed
* to dmp_set_orientation during initialization.
*/
if (sensors & INV_WXYZ_QUAT )
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
Pitch = asin(2 * q1 * q3 - 2 * q0* q2)* 57.3; // pitch
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
// printf("Pitch=%.2f ,Roll=%.2f ,Yaw=%.2f \n",Pitch,Roll,Yaw);
UART1_ReportIMU(Yaw*10, Pitch*10, Roll*10,0,0,0,100);
delay_ms(10);
}
}
return 0;
}
int __init mpu9150_init(int sample_rate, int mix_factor)
{
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) {
printk(KERN_WARNING "Invalid sample rate %d\n", sample_rate);
sprintf(status_string,"Invalid sample rate %d\n", sample_rate);
return -1;
}
if (mix_factor < 0 || mix_factor > 100) {
printk(KERN_WARNING "Invalid mag mixing factor %d\n", mix_factor);
return -1;
}
yaw_mixing_factor = mix_factor;
printk("\nInitializing IMU ...");
if (mmpu_init()) {
printk(KERN_WARNING "\nmpu_init() failed\n");
sprintf(status_string,"mpu_init() failed\n");
return -1;
}
//printk(".");
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
printk(KERN_WARNING "\nmpu_set_sensors() failed\n");
sprintf(status_string,"mpu_set_sensors() failed\n");
return -1;
}
//printk(".");
if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
printk(KERN_WARNING "\nmpu_configure_fifo() failed\n");
sprintf(status_string,"mpu_configure_fifo() failed\n");
return -1;
}
//printk(".");
if (mpu_set_sample_rate(sample_rate)) {
printk(KERN_WARNING "\nmpu_set_sample_rate() failed\n");
sprintf(status_string,"mpu_set_sample_rate() failed\n");
return -1;
}
//printk(".");
if (mpu_set_compass_sample_rate(sample_rate)) {
printk(KERN_WARNING "\nmpu_set_compass_sample_rate() failed\n");
return -1;
}
//printk(".");
if (dmp_load_motion_driver_firmware()) {
printk(KERN_WARNING "\ndmp_load_motion_driver_firmware() failed\n");
return -1;
}
//printk(".");
if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
printk(KERN_WARNING "\ndmp_set_orientation() failed\n");
return -1;
}
//printk(".");
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printk(KERN_WARNING "\ndmp_enable_feature() failed\n");
return -1;
}
//printk(".");
if (dmp_set_fifo_rate(sample_rate)) {
printk(KERN_WARNING "\ndmp_set_fifo_rate() failed\n");
return -1;
}
//printk(".");
if (mpu_set_dmp_state(1)) {
printk(KERN_WARNING "\nmpu_set_dmp_state(1) failed\n");
return -1;
}
printk(" done\n");
return 0;
}
uint8_t MPU_Init(void)
{
GPIO_InitTypeDef gpio;
NVIC_InitTypeDef nvic;
EXTI_InitTypeDef exti;
int res=0;
IIC_Init();
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
gpio.GPIO_Pin = GPIO_Pin_5;
gpio.GPIO_Mode = GPIO_Mode_IN;
gpio.GPIO_OType = GPIO_OType_PP;
gpio.GPIO_PuPd = GPIO_PuPd_UP;
gpio.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_Init(GPIOB, &gpio);
SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOB,GPIO_PinSource5);
exti.EXTI_Line = EXTI_Line5;
exti.EXTI_Mode = EXTI_Mode_Interrupt;
exti.EXTI_Trigger = EXTI_Trigger_Falling;//下降沿中断
exti.EXTI_LineCmd = ENABLE;
EXTI_Init(&exti);
nvic.NVIC_IRQChannel = EXTI9_5_IRQn;
nvic.NVIC_IRQChannelPreemptionPriority = ITP_MPU_EXTI9_5_PREEMPTION;
nvic.NVIC_IRQChannelSubPriority = ITP_MPU_EXTI9_5_SUB;
nvic.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&nvic);
if(mpu_init()==0) //初始化MPU6050
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
if(res)return 1;
res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置FIFO
if(res)return 2;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率
if(res)return 3;
res=dmp_load_motion_driver_firmware(); //加载dmp固件
if(res)return 4;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
if(res)return 5;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
DMP_FEATURE_GYRO_CAL);
if(res)return 6;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz)
if(res)return 7;
res=mpu_set_int_level(1);
if(res)return 8;
res=dmp_set_interrupt_mode(DMP_INT_CONTINUOUS);
if(res)return 9;
res=run_self_test(); //自检
if(res)return 10;
res=mpu_set_dmp_state(1); //使能DMP
if(res)return 11;
}
else
return 12;
return 0;
}
int MPU_DMP_Init()
{ int result = 0;
//mpu_init();
printf("mpu initialization complete......\n ");
//mpu_set_sensor
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
{
printf("mpu_set_sensor complete ......\n");
}
else
{
printf("mpu_set_sensor come across error ......\n");
result |=0x0001;
}
//mpu_configure_fifo
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
{
printf("mpu_configure_fifo complete ......\n");
}
else
{
printf("mpu_configure_fifo come across error ......\n");
result |=0x0002;
}
//mpu_set_sample_rate
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))
{
printf("mpu_set_sample_rate complete ......\n");
}
else
{
printf("mpu_set_sample_rate error ......\n");
result |=0x0004;
}
//dmp_load_motion_driver_firmvare
if(!dmp_load_motion_driver_firmware())
{
printf("dmp_load_motion_driver_firmware complete ......\n");
}
else
{
printf("dmp_load_motion_driver_firmware come across error ......\n");
result |=0x0008;
}
//dmp_set_orientation
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
{
printf("dmp_set_orientation complete ......\n");
}
else
{
printf("dmp_set_orientation come across error ......\n");
result |=0x0010;
}
//dmp_enable_feature
if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | //DMP_FEATURE_SEND_RAW_GYRO | // DMP_FEATURE_ANDROID_ORIENT | //By Damm Stagner
DMP_FEATURE_GYRO_CAL))
{
printf("dmp_enable_feature complete ......\n");
}
else
{
printf("dmp_enable_feature come across error ......\n");
result |=0x0020;
}
//dmp_set_fifo_rate
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))
{
printf("dmp_set_fifo_rate complete ......\n");
}
else
{
printf("dmp_set_fifo_rate come across error ......\n");
result |=0x0040;
}
run_self_test();
if(!mpu_set_dmp_state(1))
{
printf("mpu_set_dmp_state complete ......\n");
}
else
{
printf("mpu_set_dmp_state come across error ......\n");
result |=0x0080;
}
return result;
}