There is a problem applying the 'complementary filter' to the value of mpu6050

Question

Hello 
 
 I use mpu6050 and I want to use a 'Complementary Filter' to get the angle of the X, Y, and Z axes. 
 Applying this filter to the value of mpu6050 seems to reduce noise at first and produce good results. However, after some time, the value of the Z-axis continues to grow. 
 It keeps getting bigger and bigger, and after a while, the value is printed out too strangely. 
 
 This is the output when the sensor is placed on the floor. The X and Y axes seem to come out normally, but the Z axis has a strange value. 
 
 This is the code I wrote about filtering. I wrote it while referring to the example of Arduino, is there anything strange? Or is it possible that this is a sensor problem? 
 //mpu6050
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 to 2G
 p_mpu_config.gyro_config.fs_sel = AFS_2G; // Set gyro full scale range to 2G
 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;

float dt;
float accel_angle_x, accel_angle_y, accel_angle_z;
float gyro_angle_x, gyro_angle_y, gyro_angle_z;
float filtered_angle_x, filtered_angle_y, filtered_angle_z;
float baseAcX, baseAcY, baseAcZ;
float baseGyX, baseGyY, baseGyZ;


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;
}

void calibAccelGyro(){
 float sumAcX = 0, sumAcY = 0, sumAcZ = 0;
 float 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
}

void calcDT(){
 t_now = app_timer_cnt_get()*1000/32768;//milli-sec
 dt = (t_now - t_prev) / 1000.0; //millis()

 t_prev = t_now;
}


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);

 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;
}


float gyro_x, gyro_y, gyro_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;
}

void calcFilteredYPR(){
 const float ALPHA = 0.96;
 float tmp_angle_x, tmp_angle_y, tmp_angle_z;

 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;

 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;

 /*printf("%.1f", filtered_angle_x);
 printf(", ");
 printf("%.1f", filtered_angle_y);
 printf(", ");
 printf("%.1f", filtered_angle_z);
 printf("
"); */
}


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();
 set_static_passkey(); 
 gatt_init();
 services_init();
 advertising_init();
 conn_params_init();
 peer_manager_init();

 //err_code = app_button_enable();
 err_code = app_button_disable();
 APP_ERROR_CHECK(err_code);

 adc_configure();
 pwm_init();
 mpu_init(); //mpu6050
 bsp_board_init(BSP_INIT_LEDS); //use bsp led

 // Start execution.
 printf("
UART started.
");
 NRF_LOG_INFO("Debug logging for UART over RTT started.");
 
 //CenterLED_timers_start();
 advertising_start(erase_bonds); //false
 //LeftLED_timers_start(); //Left Led timer

 All_Stop();

 calibAccelGyro(); //start filter
 initDT();

 pwm_change_frequency(500); 
 do_play_buzzer(); //When start play buzzer
 
 // Enter main loop.
 for (;;)
 {
 pm_sec_params_set(NULL); //No Security parameters 
 idle_state_handle(); //if no data sleep mode

 readAccelGyro();
 calcDT();
 calcAccelYPR();
 calcGyroYPR();
 calcFilteredYPR(); 
 
 printf("%.1f", filtered_angle_x); //filter value
 printf(", ");
 printf("%.1f", filtered_angle_y);
 printf(", ");
 printf("%.1f", filtered_angle_z);
 printf("
"); 

 nrf_delay_ms(100);
 
 }
} 
 
 Thank you. 
 
 //================================================== 
 I think I found a problem. The value of dt in void calcDT() comes out as a fixed value as shown below, and suddenly becomes very large. 
 Can you tell me the cause of this problem?? 
 dt : 0.153000
Gyro_xyz : 0.0, 0.0, 0.0

Filter : -0.3, 0.1, -8.5

dt : 0.152000
Gyro_xyz : 0.1, 0.0, -0.0

Filter : -0.3, 0.1, -8.5

dt : 4294836.500000
Gyro_xyz : 0.0, -0.0, 0.0

Filter : 195136.5, -31473.5, 19662.5

dt : 0.153000
Gyro_xyz : -0.0, -0.0, 0.0

Filter : 187331.0, -30214.6, 19662.5

dt : 0.153000
Gyro_xyz : -0.0, -0.0, -0.0

Filter : 179837.7, -29006.0, 19662.5

dt : 0.154000
Gyro_xyz : 0.0, -0.1, -0.0

Filter : 172644.2, -27845.7, 19662.5

schosdas · Accepted Answer

Hello, Kenneth. Code has been modified. Thank you.

Kenneth · Answer

I suggest to print out t_now and t_prev, have in mind that app_timer_cnt_get() may be 0 and/or wrap around depending on whether there is any timers running or not. So you may need a check for if t_now is smaller than t_prev and/or if any is 0.

There is a problem applying the 'complementary filter' to the value of mpu6050

Top Replies