There is a problem calculating the yaw value of mpu6050...

[schosdas]

Hello

I'm using mpu6050 to get the value of roll,pitch,yaw.
I used a filter to get it right. The roll and pitch are good, but the Z-axis yaw has a strange value.

There are two problems.
1. Even if the sensor stays flat, the yaw value increases slightly (like accumulation).
2. After a few moves, the initial value of yaw changes.

In the picture below, the mpu6050 is left flat. All should be zero, but yaw values are completely different.  It was tested for less than 5 minutes.

Here's the code I wrote it's written.  I used complementary filter filter.

void mpu_init(void)
{
    ret_code_t ret_code;
    // Initiate MPU driver
    ret_code = app_mpu_init();
    APP_ERROR_CHECK(ret_code); // Check for errors in return value
    
    // Setup and configure the MPU with intial values
    app_mpu_config_t p_mpu_config = MPU_DEFAULT_CONFIG(); // Load default values
    p_mpu_config.smplrt_div = 19;   // Change sampelrate. Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV). 19 gives a sample rate of 50Hz
    p_mpu_config.accel_config.afs_sel = AFS_2G; // Set accelerometer full scale range
    p_mpu_config.gyro_config.fs_sel = GFS_250DPS; // Set gyro full scale range
    ret_code = app_mpu_config(&p_mpu_config); // Configure the MPU with above values
    APP_ERROR_CHECK(ret_code); // Check for errors in return value 
}


int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ; 
//double mpu_accel_x, mpu_accel_y, mpu_accel_z;
//double mpu_gyro_x, mpu_gyro_y, mpu_gyro_z;


double dt;
double accel_angle_x, accel_angle_y, accel_angle_z;
double gyro_angle_x, gyro_angle_y, gyro_angle_z;
//double filtered_angle_x, filtered_angle_y, filtered_angle_z;
double baseAcX, baseAcY, baseAcZ;
double baseGyX, baseGyY, baseGyZ;
float tmp_angle_x, tmp_angle_y, tmp_angle_z;


static void readAccelGyro() //send to app
{
    uint32_t err_code;
    double angle_x, angle_y;

    accel_values_t accel_values; //mpu6050
    gyro_values_t gyro_values;

    app_mpu_read_accel(&accel_values);          
    err_code = app_mpu_read_accel(&accel_values);
    APP_ERROR_CHECK(err_code);

    app_mpu_read_gyro(&gyro_values);
    err_code = app_mpu_read_gyro(&gyro_values);
    APP_ERROR_CHECK(err_code);

    mpu_accel_x = accel_values.x;
    mpu_accel_y = accel_values.y;
    mpu_accel_z = accel_values.z;

    mpu_gyro_x = gyro_values.x;
    mpu_gyro_y = gyro_values.y;
    mpu_gyro_z = gyro_values.z;
}


float sumAcX = 0, sumAcY = 0, sumAcZ = 0;
float sumGyX = 0, sumGyY = 0, sumGyZ = 0;

void calibAccelGyro(){
  sumAcX = 0; sumAcY = 0; sumAcZ = 0;
  sumGyX = 0; sumGyY = 0; sumGyZ = 0;

  readAccelGyro();

  for(int i=0; i<10; i++){
    readAccelGyro();
    sumAcX += mpu_accel_x; 
    sumAcY += mpu_accel_y; 
    sumAcZ += mpu_accel_z;

    sumGyX += mpu_gyro_x; 
    sumGyY += mpu_gyro_y; 
    sumGyZ += mpu_gyro_z;

    nrf_delay_ms(100);
  }

  baseAcX = sumAcX / 10; 
  baseAcY = sumAcY / 10; 
  baseAcZ = sumAcZ / 10;

  baseGyX = sumGyX / 10; 
  baseGyY = sumGyY / 10; 
  baseGyZ = sumGyZ / 10;
}


uint32_t t_now;
uint32_t t_prev;

void initDT(){
  t_prev = app_timer_cnt_get()*1000/32768;//milli-sec
  //printf("t_prev : %d\n", (int)t_prev); //511
}


void calcDT(){
  t_now = app_timer_cnt_get()*1000/32768;//milli-sec
  dt = (t_now - t_prev) / 1000.0; //millis()

  t_prev = t_now;
}


double acc_angle_x, acc_angle_y, acc_angle_z;
double acc_angle_x_2, acc_angle_y_2, acc_angle_z_3;

void calcAccelYPR(){
  float accel_x, accel_y, accel_z;
  float accel_xz, accel_yz;
  const float RADIANS_TO_DEGREES = 180/3.14159;

  accel_x = mpu_accel_x - baseAcX;
  accel_y = mpu_accel_y - baseAcY;
  //accel_z = mpu_accel_z + (16384 - baseAcZ); //different some example
  accel_z = mpu_accel_z - (16384 - baseAcZ); //test

  accel_yz = sqrt(pow(accel_y, 2) + pow(accel_z, 2));
  accel_angle_y = atan(-accel_x / accel_yz)*RADIANS_TO_DEGREES;

  accel_xz = sqrt(pow(accel_x, 2) + pow(accel_z, 2));
  accel_angle_x = atan(accel_y / accel_xz)*RADIANS_TO_DEGREES;

  accel_angle_z = 0;


  //Accel angle test
  /*acc_angle_x_2 = sqrt(pow(mpu_accel_x, 2) + pow(mpu_accel_z, 2));
  acc_angle_x = atan(mpu_accel_y / acc_angle_x_2);
  acc_angle_x = acc_angle_x * RADIANS_TO_DEGREES;

  acc_angle_y_2 = sqrt(pow(mpu_accel_y, 2) + pow(mpu_accel_z, 2));
  acc_angle_y = atan(-mpu_accel_x / acc_angle_y_2);
  acc_angle_y = acc_angle_y * RADIANS_TO_DEGREES;*/
}


double gyro_x, gyro_y, gyro_z; 
double test_angle_z;

void calcGyroYPR(){
  const float GYROXYZ_TO_DEGREES_PER_SED = 131;

  gyro_x = (mpu_gyro_x - baseGyX) / GYROXYZ_TO_DEGREES_PER_SED;
  gyro_y = (mpu_gyro_y - baseGyY) / GYROXYZ_TO_DEGREES_PER_SED;
  gyro_z = (mpu_gyro_z - baseGyZ) / GYROXYZ_TO_DEGREES_PER_SED;

  gyro_angle_x += gyro_x * dt;
  gyro_angle_y += gyro_y * dt;
  gyro_angle_z += gyro_z * dt;
}

double test_filter_z;

void calcFilteredYPR(){
  const float ALPHA = 0.96; 

  tmp_angle_x = 0;
  tmp_angle_y = 0;
  tmp_angle_z = 0;

  tmp_angle_x = filtered_angle_x + gyro_x * dt;
  tmp_angle_y = filtered_angle_y + gyro_y * dt;
  tmp_angle_z = filtered_angle_z + gyro_z * dt; //original

  filtered_angle_x = ALPHA * tmp_angle_x + (1.0 - ALPHA) * accel_angle_x;
  filtered_angle_y = ALPHA * tmp_angle_y + (1.0 - ALPHA) * accel_angle_y;
  filtered_angle_z = tmp_angle_z;

} 


void SendData()
{
    ret_code_t err_code;

        //Filter
        printf("%.0lf", filtered_angle_x); //filter value
        printf(", ");
        printf("%.0lf", filtered_angle_y);
        printf(", ");
        printf("%.0lf\n", filtered_angle_z); 
        
        nrf_delay_ms(10);
}


int main(void)
{
    uint32_t err_code;
    bool erase_bonds = false;

    // Initialize.
    gpio_init(); 
    uart_init();
    log_init();
    timers_init();
    app_buttons_init(); 
    power_management_init();
        
    ble_stack_init();
    gap_params_init();
    gatt_init();
    services_init();
    advertising_init();
    conn_params_init();
    peer_manager_init();

    err_code = app_button_enable();
    APP_ERROR_CHECK(err_code);

    adc_configure(); //baterry value & LED
    pwm_init();
    mpu_init(); //mpu6050
    bsp_board_init(BSP_INIT_LEDS); //use bsp

    // Start execution.
    printf("\r\nHelmet Device started.\r\n");
    NRF_LOG_INFO("Debug logging for UART over RTT started.");
    
    advertising_start(erase_bonds); //false

    All_Stop();

    do_play_buzzer(); //When start play buzzer
    nrf_gpio_pin_set(POWER_SW); //HIGH : power on

    calibAccelGyro(); //filter average value
    initDT();

    // Enter main loop.
    for (;;)
    {
        readAccelGyro();
        calcDT();
        calcAccelYPR(); //accel
        calcGyroYPR(); //gyro
        calcFilteredYPR(); //filter 
        SendData();

        pm_sec_params_set(NULL); //No Security parameters      
        //idle_state_handle(); //if no data sleep mode
    }
}

Is there something wrong with gyro? Can I know about this problem?

Thank you.

[helsing]

Hello

I will be taking a look at your case this afternoon.

Best regards,

Håkon

[helsing]

Hello,

Sorry for not coming back to you, this case slipped trough. As I did not hear back from you in your other case regarding this sensor(which is EOL), I assumed you were not working on this anymore.

Regarding your problem, inertial navigation systems, including MEMS gyros, are know to drift.The yaw axis of the gyro is most sensitive to drift due to the flat form factor of the MEMS sensor IC. Have you compared the accumulation you are seeing to the manufacturer's maximum rating of the Z-axis? Nulling the gyro when in a known state is one technique for handling drift. Your sensor manufacturer might have more comments and ideas on this topic. Here is one answer regarding accumulated gyroscope drift error over time. 

Best regards,

Håkon