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("\n"); */
}
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("\r\nUART started.\r\n");
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("\n");
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
