Sensor Fusion

[tonvanhieu]

Dear Teacher, Between DMP, Kalman Filter and Complementary filter which way you think that is the most stable and reliable. How can we add bias in the DMP (i saw that there are dmp_setbias...() functions but can we set bias for each axis?) _ There is gyro calib feature in DMP, but it only works after 8 secs of no motion, so can we save the bias for the next time? Thank you very much my teacher.

[tonvanhieu]

Help me teacher 😢 ....

[EFeru]

_ Between DMP, Kalman Filter and Complementary filter which way you think that is the most stable and reliable.

• I recommend DMP, because it runs on the MPU6050 and thus reduces significantly the computational effort on the main GD32/STM32 micro-controller.

_ How can we add bias in the DMP (i saw that there are dmp_setbias...() functions but can we set bias for each axis?)

• Yes, it is possible by calling mpu_set_gyro_bias_reg(argument) and mpu_set_accel_bias_6050_reg(argument) where the argument is a vector with 3 elements (X, Y, Z).

_ There is gyro calib feature in DMP, but it only works after 8 secs of no motion, so can we save the bias for the next time?

• You can also trigger calibration when you want by calling mpu_start_self_test().
• If you want to store the offsets in the MPU6050 registers, you need to un-comment USE_CAL_HW_REGISTERS in config.h
• However, be aware that the offsets will be lost at Power-off. In the future, I will implement something to store them in Flash, so they are not lost. For now as a temporary work-around, if want to maintain them, use VARIANT_DEBUG, do the calibration and put the offsets in variables, that you load each time at Power-on.

[tonvanhieu]

Thank u very much teacher. ❤️

[tonvanhieu]

What wrong with my code teacher? why it's the same as before, nothing change.

        long gyro_bias[3];
    gyro_bias[0]=(long) 270;
    gyro_bias[1]=(long)57;
    gyro_bias[2]=(long) -19;
        long accel_bias[3] = {352,234,0};
    unsigned char i;
    for(i = 0; i<3; i++) {
            gyro_bias[i] = (long)(gyro_bias[i] * 32.8f); //convert to +-1000dps
            accel_bias[i] *= 2048.f; //convert to +-16G
            accel_bias[i] = accel_bias[i] >> 16;
            gyro_bias[i] = (long)(gyro_bias[i] >> 16);
        }
    mpu_set_gyro_bias_reg(gyro_bias);
    mpu_set_accel_bias_6050_reg(accel_bias);

[EFeru]

What do you mean by "the same"? You mean the offsets are not removed?

[tonvanhieu]

What do you mean by "the same"? You mean the offsets are not removed?

Yes sir. I mean the offsets are not removed.

[EFeru]

Strange... Your code seems right to me. To be honest I didn't play much with the Calibration functionality, so I don't know what to expect.

2 points that come into my mind:

• are you looking at the Raw accel and gyro data? Because maybe the offsets are only taken into account when DMP calculates the Quaternions, so they will only be visible when you look at the Euler angles.
• and 2nd, can you try VARIANT_DEBUG with USE_CAL_HW_REGISTERS un-commented, check the Euler angles, then perform a Self-test. If the test passed, check Euler angles again.

[tonvanhieu]

Strange... Your code seems right to me.

Yes, i learn from you in mpu_start_self_test()..

and 2nd, can you try VARIANT_DEBUG with USE_CAL_HW_REGISTERS un-commented, check the Euler angles, then perform a Self-test. If the test passed, check Euler angles again.

I tried it, Euler change, and gyro, accel also change too. I dont know what is diffrent here @@.

If the test passed, check Euler angles again

The test always fail. But offsets are removed.

EDIT: gyro_bias[i] = (long)(gyro_bias[i] >> 7); when i change from 16 --> 7. It works, the gyro raw change.

[EFeru]

The test always fail. But offsets are removed.

I've seen also that sometimes the test fails... I need to check the criteria later, maybe the MPU expects to be really steady, no movement, no vibrations.. At least good that the behavior is as expected and the offsets are removed.

I tried it, Euler change, and gyro, accel also change too. I dont know what is diffrent here @@.

Where did you place your code?

[tonvanhieu]

Where did you place your code?

I place it inside mpu_config();.

[EFeru]

I guess at the end of mpu_config(), here?

Just in case I would put an aditional delay_ms(100); before your code, just to be sure that all settings are received by the MPU.

[tonvanhieu]

EDIT: gyro_bias[i] = (long)(gyro_bias[i] >> 7); when i change from 16 --> 7. It works, the gyro raw change.

I think problem is about MATH here. maybe gyro[3] and accel [3] in mpu_start_self_test() are not just simple a number.

Just in case I would put an aditional delay_ms(100); before your code, just to be sure that all settings are received by the MPU.

Still same with ">>16" (no offsets removed)

[EFeru]

I think problem is about MATH here. maybe gyro[3] and accel [3] in mpu_start_self_test() are not just simple a number.

You printed them, so they must have those values. It is quite a strange... Thinking out loud... maybe we need to call mpu_get_data(); at least once, then send the offsets. It would be quite strange... but who knows. So maybe call mpu_get_data(); and then put your code before this line

[tonvanhieu]

You printed them, so they must have those values

I mean gyro and accel at this.

i print that gyro in mpu_start_self_test() it's not raw gyro in mpu_get_data();

Yes. It is lager much than normal values (154084 > 270).

[EFeru]

Those are the values you should store in your variables and then run this code or your code, they are similar.

[tonvanhieu]

I've got it teacher.

CONCLUSION: get argument for mpu_set_gyro_bias_reg(argument) and mpu_set_accel_bias_6050_reg(argument) in mpu_start_self_test(). It will work.

Thank you teacher, thank you very much.

I will donate tuition when i get a job after graduation. I promise. Wait for me. 😄

[EFeru]

Nice. No problem ;)

By the way feel free to join the Telegram group for more Info and insights of this project and other projects too: https://t.me/joinchat/BHWO_RKu2LT5ZxEkvUB8uw