MPU 9250 MPL Vibration

Hi, MOD_Mustafa，

Thanks for your reply. Yes, I'm using the Invensense Embedded MotionDrivers, motion_driver_6.12.

Hi, MOD_Mustafa，

Sorry, i re-upload the file, including the main code and the test video.

Thanks for your reply.

In the video, you can see the heading data is always changing according to the vibration. I have tried setting the mpu lpf parameter, lpf from 10 tto 42, it still can't work. Please give some more advise.

Hi, MOD_Mustafa，

the main code show here:

#define DEFAULT_MPU_HZ (20)
#define COMPASS_READ_MS (100)
#define TEMP_READ_MS (500) // temperature sensor reading frequency
#define DMP_ENABLE (0)
#define COMPASS_ENABLE (1)
#define HEADING_READ_MS (100)
#define HEADING_INIT_MS (30000) // init time for 9-axis fusion, system will work back to 6-axis fusion when the init heading is ok
#define pi 3.141592653589793f
#define NUM_CAL_DATA_BYTES 200
#define FLAHS_HEADER 0xAD

// new imu data int signal
static unsigned char newGyro = 0;
static unsigned long next_temp_ms = 0;
static unsigned long next_compass_ms = 0;
static unsigned long next_hdt_ms = 0;
static unsigned long hdt_compass_hold_ms = 0;

static uint8_t new_compass = 0;
static uint8_t new_temp = 0;
static uint8_t new_hdt_report = 0;

// currrent system timestamp
static unsigned long sys_timestamp = 0;

//My parameters
uint8_t flashData[NUM_CAL_DATA_BYTES+2];
char debugMsg[200];
float initHeading = 0;

static COMPASS_STATUS compass_status = HEADING_INIT_STATUS;

// MPL
unsigned char *mpl_key = (unsigned char*)"eMPL 5.1";
struct platform_data_s {
signed char orientation[9];
};

static struct platform_data_s gyro_pdata = {
.orientation = { 1, 0, 0,
0, 1, 0,
0, 0, 1}
};

static struct platform_data_s compass_pdata = {
.orientation = { 0, 1, 0,
1, 0, 0,
0, 0, -1}
};

/**
* @brief init mpu9250, return 0 if success
*/
uint8_t mpu9250_init(void)
{
inv_error_t result;
unsigned char accel_fsr;
unsigned short gyro_rate, gyro_fsr;
unsigned short compass_fsr;
struct int_param_s int_param;
uint8_t whoiam = 0;
Sensors_I2C_ReadRegister(0x68, 0x75,1, &whoiam);
printf("device is %02X\n",whoiam);
// init mpu9250
result = mpu_init(&int_param);
if(result){
printf("mpu initialize fail..\n");
return 1;
}
// init mpl
result = inv_init_mpl();

inv_enable_quaternion();
inv_enable_9x_sensor_fusion();
inv_enable_fast_nomot();
// enable temperature compensate
inv_enable_gyro_tc();
// compass settings
inv_enable_vector_compass_cal();
inv_enable_magnetic_disturbance();

inv_enable_eMPL_outputs();
result = inv_start_mpl();

mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sample_rate(DEFAULT_MPU_HZ);
mpu_set_compass_sample_rate(1000 / COMPASS_READ_MS);

mpu_get_sample_rate(&gyro_rate);
mpu_get_gyro_fsr(&gyro_fsr);
mpu_get_accel_fsr(&accel_fsr);
mpu_get_compass_fsr(&compass_fsr);

inv_set_gyro_sample_rate(1000000L / gyro_rate);
inv_set_accel_sample_rate(1000000L / gyro_rate);
inv_set_compass_sample_rate(COMPASS_READ_MS * 1000L);

inv_set_gyro_orientation_and_scale(
inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
(long)gyro_fsr<<15);
inv_set_accel_orientation_and_scale(
inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
(long)accel_fsr<<15);

inv_set_compass_orientation_and_scale(
inv_orientation_matrix_to_scalar(compass_pdata.orientation),
(long)compass_fsr<<15);

dmp_load_motion_driver_firmware();
dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_pdata.orientation));
result = 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);

dmp_set_fifo_rate(DEFAULT_MPU_HZ);
mpu_set_dmp_state(DMP_ENABLE);
// load bias;
load_bias_from_flash();
run_self_test();
next_compass_ms = getTimerCnt();
next_temp_ms = next_compass_ms;
next_hdt_ms = next_compass_ms;
return 0;
}

/**
* @brief read mpu data from sensor and do inv_build_xxx
*/
void mpu_build_data(void)
{
unsigned long timestamp;
short gyro[3], accel_short[3];
unsigned char sensors, more;
long accel[3], temperature;
unsigned long sensor_timestamp, temp_timestamp, comp_timestamp;
short compass_short[3];
long compass[3];
int new_data = 0;
timestamp = getTimerCnt();
if(timestamp > next_compass_ms){
new_compass = 1;
next_compass_ms = timestamp + COMPASS_READ_MS;
}
if(timestamp > next_temp_ms){
new_temp = 1;
next_temp_ms = timestamp + TEMP_READ_MS;
}
if(timestamp > next_hdt_ms){
new_hdt_report = 1;
next_hdt_ms = timestamp + HEADING_READ_MS;
}
if(newGyro == 0){
return;
}
delay_ms(2);
newGyro = 0;
if(mpu_read_fifo(gyro, accel_short, &sensor_timestamp,
&sensors, &more) != 0){
return;
}
if (more)
newGyro = 1;
if (sensors & INV_XYZ_GYRO) {
/* Push the new data to the MPL. */
inv_build_gyro(gyro, sensor_timestamp);
new_data = 1;
if (new_temp) {
new_temp = 0;
/* Temperature only used for gyro temp comp. */
mpu_get_temperature(&temperature, &temp_timestamp);
inv_build_temp(temperature, sensor_timestamp);
}
}
if (sensors & INV_XYZ_ACCEL) {
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel, 0, sensor_timestamp);
new_data = 1;
}
if (new_compass) {
new_compass = 0;
/* For any MPU device with an AKM on the auxiliary I2C bus, the raw
* magnetometer registers are copied to special gyro registers.
*/
if (!mpu_get_compass_reg(compass_short, &comp_timestamp)) {
compass[0] = (long)compass_short[0];
compass[1] = (long)compass_short[1];
compass[2] = (long)compass_short[2];
inv_build_compass(compass, 0, comp_timestamp);
}
new_data = 1;
}
if(new_data){
inv_execute_on_data();
if(new_hdt_report){
report_data(timestamp - sys_timestamp);
new_hdt_report = 0;
sys_timestamp = timestamp;
}
}
}

/**
* @brief report the heading data from the mpl library to the uart
* when power on, system will running 9-axis fusion for HEADING_INIT_MS, and
* system accuracy should be 3 all through the init time.
* @param time_interval_ms , ms unit
* @author zuodahua
*/
void report_data(uint64_t time_interval_ms)
{
long data[3] = {0};
int8_t accuracy = 0;
inv_time_t mpl_timestamp = 0;
if(time_interval_ms > 500){
printf("interval: %llu\n", time_interval_ms);
}
if(inv_get_sensor_type_heading(data, &accuracy, &mpl_timestamp)){
if(accuracy == 3){
hdt_compass_hold_ms += time_interval_ms;
}else{
hdt_compass_hold_ms = 0;
}
if(hdt_compass_hold_ms > HEADING_INIT_MS && (get_compass_status() != COMPASS_TEST_STATUS)){
set_compass_status(HEADING_IMU_STATUS);
}
packageHdt((data[0]/65536.0f), accuracy);
}
}

bool run_self_test(void)
{
int result;
long gyro[3], accel[3];
result = mpu_run_6500_self_test(gyro, accel, 0);
if(result == 0x7){
unsigned short accel_sens;
float gyro_sens;
mpu_get_accel_sens(&accel_sens);
accel[0] *= accel_sens;
accel[1] *= accel_sens;
accel[2] *= accel_sens;
inv_set_accel_bias(accel, 3);
mpu_get_gyro_sens(&gyro_sens);
gyro[0] = (long) (gyro[0] * gyro_sens);
gyro[1] = (long) (gyro[1] * gyro_sens);
gyro[2] = (long) (gyro[2] * gyro_sens);
inv_set_gyro_bias(gyro, 3);
printf("self test pass!!\n");
printf("acc:%ld, %ld, %ld\n",accel[0],accel[1],accel[2]);
printf("gyr:%ld, %ld, %ld\n",gyro[0],gyro[1],gyro[2]);
return true;
}else{
printf("self test fail!!\n");
return false;
}
}

/**
* @brief start 9-axi fusion
*/
void init_mpl_heading(void)
{
inv_init_9x_fusion();
inv_start_9x_sensor_fusion();
}

void load_bias_from_flash(void)
{
memset(flashData, 0, sizeof(flashData));
flash_read(USER_FLASH_ADDR,(uint16_t*)flashData,NUM_CAL_DATA_BYTES/2+1);
if(flashData[0] == FLAHS_HEADER)
{
inv_load_mpl_states(flashData+2,NUM_CAL_DATA_BYTES);

long bais[3],temp;
inv_get_accel_bias(bais,&temp);
sprintf(debugMsg,"acc bais:%ld %ld %ld\n",bais[0],bais[1],bais[2]);
appendTxMission((uint8_t*)debugMsg,strlen(debugMsg));
inv_get_gyro_bias(bais,&temp);
sprintf(debugMsg,"gyr bais:%ld %ld %ld\n",bais[0],bais[1],bais[2]);
appendTxMission((uint8_t*)debugMsg,strlen(debugMsg));
inv_get_compass_bias(bais);
sprintf(debugMsg,"mag bais:%ld %ld %ld\n",bais[0],bais[1],bais[2]);
appendTxMission((uint8_t*)debugMsg,strlen(debugMsg));
}
}

int save_bias_to_flash(void)
{
int result = 0;
inv_error_t inv_error;
error_status flash_error;
memset(flashData, 0, sizeof(flashData));
inv_error = inv_save_mpl_states(flashData+2, NUM_CAL_DATA_BYTES);
if(inv_error == INV_SUCCESS){
flashData[0] = 0xAD;
flash_error = flash_write(USER_FLASH_ADDR, (uint16_t *)flashData, NUM_CAL_DATA_BYTES/2+1);
result = (flash_error == SUCCESS) ? 1 : 0;
}
set_compass_status(HEADING_INIT_STATUS);
return result;
}

bool turnOnOffCompassFusion(bool turnOn)
{
inv_error_t error;
if(turnOn){
error = inv_start_9x_sensor_fusion();
}
else{
error = inv_stop_9x_sensor_fusion();
}
return (error == INV_SUCCESS);
}

/**
* @brief external interrupt signal
*/
void gyro_data_ready_cb()
{
newGyro = 1;
}

COMPASS_STATUS get_compass_status(void)
{
return compass_status;
}

void set_compass_status(COMPASS_STATUS status)
{
compass_status = status;
switch(compass_status){
case HEADING_9X_STATUS:
hdt_compass_hold_ms = 0;
inv_start_9x_sensor_fusion();
break;
case HEADING_IMU_STATUS:
inv_stop_9x_sensor_fusion();
break;
case COMPASS_TEST_STATUS:
inv_start_9x_sensor_fusion();
break;
case HEADING_INIT_STATUS:
inv_start_9x_sensor_fusion();
hdt_compass_hold_ms = 0;
break;
default:
break;
}
}