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;
}
inv_error_t MPU9250_DMP::setAccelFSR(unsigned char fsr)
{
inv_error_t err;
err = mpu_set_accel_fsr(fsr);
if (err == INV_SUCCESS)
{
_aSense = getAccelSens();
}
return err;
}
/**
* @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
}
uint8_t mpu_init(){
uint8_t data[6];
uint8_t datai;
/* Reset device. */
data[0] = BIT_RESET;
if (mpu9250_writes(MPU6500_PWR_MGNT_1, 1, data))
return -1;
delay_ms(100);
/* Wake up chip. */
data[0] = 0x00;
if (mpu9250_writes(MPU6500_PWR_MGNT_1, 1, data))
return -1;
// とりあえず
//data[0] = 0x00;
//mpu9250_writes(MPU6500_USER_CTRL, 1, data);
//delay_ms(10);
//mpu9250_writes(MPU6500_ACCEL_CFG2, 1, data);
//delay_ms(10);
//mpu9250_writes(MPU6500_INT_PIN_CFG, 1, data);
//delay_ms(10);
//mpu9250_writes(MPU6500_LPF, 1, data);
//delay_ms(10);
data[0] = 0x02;
mpu9250_writes(MPU6500_INT_PIN_CFG, 1, data);
delay_ms(10);
//datai = 0;
//datai |= BIT_STBY_XYZA;
data[0] = 0x00;
mpu9250_writes(MPU6500_PWR_MGNT_2, 1, data[0]);
delay_ms(10);
//data[0] = 0x01;
//mpu9250_writes(MPU6500_INT_ENABLE, 1, data);
//delay_ms(10);
#if 0
/* user control */
data[0] = MPU9250_I2C_IF_DIS | MPU9250_I2C_MST_EN;
if (mpu9250_writes(MPU6500_USER_CTRL, 1, data))
return -1;
if (mpu9250_writes(MPU6500_USER_CTRL, 1, data))
return -1;
delay_ms(10);
mpu_set_bypass(0);
delay_ms(10);
mpu9250_st.accel_half = 0;
/* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the
* first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO.
*/
data[0] = BIT_FIFO_SIZE_1024 | 0x8;
if (mpu9250_writes(MPU6500_ACCEL_CFG2, 1, data))
return -1;
/* Set to invalid values to ensure no I2C writes are skipped. */
//mpu9250_st.sensors = 0xFF;
//mpu9250_st.gyro_fsr = 0xFF;
//mpu9250_st.accel_fsr = 0xFF;
//mpu9250_st.lpf = 0xFF;
//mpu9250_st.sample_rate = 0xFFFF;
//mpu9250_st.fifo_enable = 0xFF;
//mpu9250_st.bypass_mode = 0xFF;
//mpu9250_st.compass_sample_rate = 0xFFFF;
/* mpu_set_sensors always preserves this setting. */
mpu9250_st.clk_src = INV_CLK_PLL;
/* Handled in next call to mpu_set_bypass. */
mpu9250_st.active_low_int = 1;
mpu9250_st.latched_int = 0;
mpu9250_st.int_motion_only = 0;
mpu9250_st.lp_accel_mode = 0;
//とりあえずマスク
//memset(&mpu9250_st.cache, 0, sizeof(mpu9250_st.cache));
mpu9250_st.dmp_on = 0;
mpu9250_st.dmp_loaded = 0;
mpu9250_st.dmp_sample_rate = 0;
// ジャイロフルスケール設定
if (mpu_set_gyro_fsr(2000))
return -1;
// 加速度フルスケール設定
if (mpu_set_accel_fsr(2))
return -1;
// LPF設定
if (mpu_set_lpf(42))
return -1;
// サンプルレート設定
if (mpu_set_sample_rate(100))
return -1;
// ひとまずマスク
/*
if (mpu_configure_fifo(0))
return -1;
if (int_param)
reg_int_cb(int_param);
*/
#ifdef USE_AK8963
if (setup_compass())
return -1;
if (mpu_set_compass_sample_rate(100))
return -1;
#else
/* Already disabled by setup_compass. */
if (mpu_set_bypass(0))
return -1;
#endif
/*
mpu_set_sensors(0);
*/
#endif
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 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;
}
