/*******************************************************************************
* 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);
}
/**
* @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;
}
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;
}
/******************************************************************************
* 函数名称: 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);
}
/**************************************************************************
函数功能：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 MPU9250Device::init() {
mpu_init(&this->revision);
mpu_set_compass_sample_rate(100); // defaults to 100 in the libs
/* Get/set hardware configuration. Start gyro. Wake up all sensors. */
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
// mpu_set_gyro_fsr (2000);//250
// mpu_set_accel_fsr(2);//4
/* Push both gyro and accel data into the FIFO. */
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sample_rate(DEFAULT_MPU_HZ);
dmp_load_motion_driver_firmware();
dmp_set_orientation(GYRO_ORIENTATION);
//dmp_register_tap_cb(&tap_cb);
unsigned short dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL |
DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;
//dmp_features = dmp_features | DMP_FEATURE_TAP ;
dmp_enable_feature(dmp_features);
dmp_set_fifo_rate(DEFAULT_MPU_HZ);
}
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");
}
}
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;
}
/**********************************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");
}
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 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 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 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;
}
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;
}
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 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 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;
}
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;
}
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;
}
inv_error_t MPU9250_DMP::configureFifo(unsigned char sensors)
{
return mpu_configure_fifo(sensors);
}
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;
}
bool MPUManager::__StartDevice( bool p_bSelfTest )
{
if( 0 != mpu_init() )
{
printf("mpu_init failed\n");
return false;
}
#ifdef __DEBUG
__OutputDeviceStatus();
#endif
/* Wake up all sensors. */
if( 0 != mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) )
{
printf("mpu_set_sensors failed\n");
return false;
}
/* Push both gyro and accel data into the FIFO. */
if( 0 != mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL) )
{
printf("mpu_configure_fifo failed\n");
return false;
}
if( 0 != mpu_set_sample_rate(m_usSampleRate) )
{
printf("mpu_set_sample_rate failed\n");
return false;
}
if( 0!= dmp_load_motion_driver_firmware() )
{
printf("dmp_load_motion_driver_firmware failed\n");
return false;
}
if( 0 != dmp_enable_feature(m_usDefaultFeatures) )
{
printf("dmp_enable_feature failed\n");
return false;
}
if( 0 != dmp_set_fifo_rate(m_usSampleRate) )
{
printf("dmp_set_fifo_rate failed\n");
return false;
}
if( 0 != mpu_get_gyro_sens( &m_fCurGyroSensitivity ) )
{
printf("mpu_get_gyro_sens failed\n");
return false;
}
else
{
printf("Current Gyro Sensitivity: %.2f\n", m_fCurGyroSensitivity);
}
if( 0 != mpu_get_accel_sens(&m_usCurAccelSensitivity) )
{
printf("mpu_get_accel_sens failed\n");
return false;
}
else
{
printf("Current Accel Sensitivity: %d\n", m_usCurAccelSensitivity);
}
m_usPackageLength = __dmp_get_packet_length();
unsigned short usfr = 0;
dmp_get_fifo_rate( &usfr );
printf("DMP start success\npackage length = %d\nFIFO rate = %d\n", m_usPackageLength, usfr);
if( p_bSelfTest )
{
if( __RunSelfTest() )
{
printf("Self test passed\n");
}
}
if( 0 != m_sGyroOffset[0] )
{
if( !__mpu_set_user_x_gyro_offset(m_sGyroOffset[0]) )
{
printf("set_user_x_gyro_offset %d failed\n", m_sGyroOffset[0]);
}
}
if( 0 != m_sGyroOffset[1] )
{
if( !__mpu_set_user_y_gyro_offset(m_sGyroOffset[1]) )
{
printf("set_user_y_gyro_offset %d failed\n", m_sGyroOffset[1]);
}
}
if( 0 != m_sGyroOffset[2] )
{
if( !__mpu_set_user_z_gyro_offset(m_sGyroOffset[2]) )
{
printf("set_user_z_gyro_offset %d failed\n", m_sGyroOffset[2]);
}
}
if( 0 != m_sAccelOffset[0] )
{
if( !__mpu_set_x_accel_offset(m_sAccelOffset[0]) )
{
printf("mpu_set_x_accel_offset %d failed\n", m_sAccelOffset[0]);
}
}
if( 0 != m_sAccelOffset[1] )
{
if( !__mpu_set_y_accel_offset(m_sAccelOffset[1]) )
{
printf("mpu_set_y_accel_offset %d failed\n", m_sAccelOffset[1]);
}
}
if( 0 != m_sAccelOffset[2] )
{
if( !__mpu_set_z_accel_offset(m_sAccelOffset[2]) )
{
printf("mpu_set_z_accel_offset %d failed\n", m_sAccelOffset[2]);
}
}
if( 0 != mpu_set_dmp_state(1) )
{
printf("mpu_set_dmp_state 1 failed\n");
return false;
}
if( 0 == mpu_set_lpf(42) )
{
printf("mpu_set_lpf failed\n");
return false;
}
#ifdef __DEBUG
__OutputDeviceStatus();
#endif
return true;
}
