i'm currently developing my own quadcopter software. i'm using the mpu-6050 but i ran into trouble while trying to calculate the angle from the gyroscope and i don't know if the problem is in the software or the hardware.

while initializing the sensor i've set the gyro scale to 2000 deg/sec which means that i divide the acquired data from each axis of the gyroscope by 16.4.

i get a new sample every 3 ms so if i want to get Gyro angle the code goes as follows :

     while(1){
     
     GetRate(); // gyro data in deg/sec after removing the offset 
     angle+= (GyroX*dt); // dt=0.003
     pc.printf("%f\n",angle);
     wait_ms(3);
     }

The output of the code is not correct.so if someone could possibly point out something that i did wrong i would highly appreciate it.

calculating Euler angle from the accelerometer works fine, i just need to fix the gyroscope angle issue so that i can feed the data into a DCM algorithm to calculate the angle accurately.

Thanks in advance

Uploading your entire program or showing bit more code would help generally, although in this case I think I found the reason without it. Also it would be useful to know how your output is not correct. Is it a bit too large (lets say it reports a 450 degrees rotation when rotating 360 degrees), or is it way too large? Or is it too small?

But anyway about your problem. I ran your code (editted a bit to use my functions), with the assumptions your pc connection is 9600 baud and your I2C connection to the MPU6050 is 100kbit/s. With that I got an calculated angle which was roughly a quarter of what I expected. I already had a guess about what was going wrong, so I printed the time it took to run the code with the previously mentioned assumptions: roughly 12ms, so 4 times slower than what you use in your calculations.

GetRate() probably gets X, Y and Z gyro data. That is 6 bytes, and it needs to send the register address byte, and twice the device address. Lets round it up to 10 bytes to include overhead, so 80 bits. At the default 100kbit/s that already takes 0.8 ms(yes that is long, running it at 400kbit/s, or faster works too, helps. You can at your own risk also have a look at my MODI2C library, which exactly for that reason is made interrupt based).

Then your printf statement totally sends around 10 characters. So also that is 80 bits, but this only at 9600bit/s, which then ends up at 8ms. Add the wait statement, and your total result is roughly 12ms.

Solutions? Well the blocking read/write statements in your loop take the majority of the time, at least setting their speed higher helps. But while useful, it does not actually solve the issue. Either use a ticker to determine the rate of your code (obviously the ticker must be slower than the time it takes to execute the loop once), or use a timer to measure the time it took since last update, and use that as 'dt' value. When I added that to your code it worked fine for me.

Hi Erik

First of all let me thank you for the fast response.

Yes, my angle is a quarter of what i should be getting. the pc connection is 9600 baud and the i2c frequency is 400kHz and the sensor sampling rate is 1kHz.

so i did what you told me and added a Timer to figure out how long it takes to run the loop once and print it over to Tera Term but i still get a 3ms result and that is strange. here is the code after i modified it :

     i2c.frequency(400000); 
     setupMPU();
     calibrateGyros();
     calibrateAcc(); 
     
     measure.start();
     GetRate();
     angle+= (GyroX*dt);
     wait_ms(3);
     measure.stop();
     pc.printf("%d\n",measure.read_ms());

Hey,

The problem is the majority of the time is spend in your printf function. That is now not included in the timer. Try this:

     i2c.frequency(400000); 
     setupMPU();
     calibrateGyros();
     calibrateAcc(); 
     
     measure.start();
     GetRate();
     angle+= (GyroX*dt);
     pc.printf("%f\n",angle);
     wait_ms(3);
     measure.stop();
     pc.printf("%d\n",measure.read_ms());

Probably the easiest way to get it accurate in your loop is to do something like this:

measure.reset();  
while(1) {
     GetRate();
     dt = measure.read();
     measure.reset();
     angle+= (GyroX*dt);
     pc.printf("%f\n",angle);
     wait_ms(3);
}

You're are absolutely correct Eric it takes about 10ms to run the loop once.

I've been trying to figure out what is wrong with my code for days now so i thought I'd ask on the Forum and just get it over with. silly me.

Thank You again Eric

And from my own experience (assuming you want a constant data stream - rather than an application needing a lot of sleep and just waking up on a burst of movement), implementing interrupts is not needed, given the speeds involved. By the time you've done something with the data read, it's time to read again, no need for delays or signals.

YMMV

Hey Anthony,

constant data stream will be needed when i start testing the quadcopter and at that point a ticker or a timer will track my loop performance and execute the flight control algorithm at a given frequency.

but what i want to know now is this : for a quadcopter to be stable a loop must be running a minimum of 100Hz and during testing if i'm streaming data from mbed i won't be able to achieve that timing. so is there anyway that i can make the mbed print data over the serial port faster ?

Normal RC motor drivers use 50Hz update rate (the standard servo protocol). I know faster is better, and there are drivers which can be loaded with custom firmware that operates faster. However while easier to have it stable at faster update rates, afaik there is no special reason why it would be impossible at 50Hz.

Anyway that wasnt your question, simple answer is to just increase your baudrate. Max allowed is 921600, (so just pc.baud(921600);), don't forget to also change it in your terminal program on your PC.