Homemade FlightController.

Hello and welcome everyone!

Some time ago influenced by DJI Phantom I started to build my own Quadcopter.
To understand the basic principles of aerophysics and RC I started to build my own FlightController based on Arduino Mega 2560 r3, Pololu AltIMU v5 and FlySky iA6 Receiver.

Working with RC Receiver was quite a good feeling, it wasn't that hard to understand.

However, working with the gyro, accelerometer and magnetometer is painful.

I surfed the web for about two weeks and didn't found anything to solve the problem that I came across recently.

I started to write an IMU code for AltIMU, it's looking like so: http://pastebin.com/rPEjMM9y

In fact, when I start the Arduino, launch the Serial Mon, angles are OK, but after some period they started to drift dramatically.
For the first time I though that it's connected with the resolution of gyro, but after changing it appropriately, it still exists.  
I also looked on the AHRS from Pololu, but the time consumption for all the calculation is probably too long for the quadcopter to maintain the stable flight. (It's meant for the 50Hz, but after watching whole YMFC series I thought that 250Hz is rather the minimum).

I'm stuck and I really don't know what to do now.

Kind regards.

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –


  • A couple of tips:

      Gyro integration speed is critical to stability (400Hz is reasonable for an 8bit micro).  Drift correction can happen at a slower rate to save computation time (~100Hz).

      You have to translate Mag, and Accel readings from sensor reference frame to world reference frame before computing corrections.

      You have way too many trig functions, they cost too much computation time.  Everything in a complemntary filter for IMU can be done with vectors and not trigfunctions.  The units for accelerometers should be G's and the units for gyro should be radians/s.  Magnetometer units are special and should be turned to a unit vector after magnetic inclination is eliminated.

      Avoid square root functions as well.  I needed only one in my IMU and it is in a 50Hz loop.  Approximations can be used for most cases.

    Good Luck,


    • Phil, I'm writing an arduino flight controller. I started with code from Joop Brooking (YouTube) and migrated up to the STM32 microcontroller but still using Arduino IDE. Joop's code is based on Euler Angles so I'm now trying to convert my program to Quaternions. I've found some great information out there (some of it admittedly over my head - I'm a 59 year old mechanical engineer, and I must confess my matrix math skills are quite rusty). I've figured out how to use the gyro signal to track my attitude (orientation), but now I'm trying to merge that (like you would with a complementary filter) with the accelerometer and magnetometer signals. But I don't know how to remove the motion portion of the accelerometer vector from the gravity portion. Do you have your arduino flight controller code posted anywhere so that I could take a look at it (including any custom routines or open-source libraries you are calling)
      Also, wondering if you're still in Minneapolis. I'm in the Minneapolis metro area. Thanks. Randy
      • I hang out in Plymouth area and am also a mechanical engineer.

        Joop's code is great for learning some of the basic concepts of level flight, but it it not a good place to start if you actually want to make something fly well.

        My code is really not intended for public consumption, but the vast majority of it is here: https://github.com/daPhoosa
        Most of it is pretty old and needs to be cleaned up, but it's just not at the top of my to-do list.

        I also have a blog with chicken scratching posts with some of the things I did back when I was working on it more.

        The only place you should need matrix math when using a quaternion based complementary filter is doing hard/soft iron compensation for the magnetometer.

        For most flying machines, you do not need to perfectly compensate for inertial effects of motion. In situations where there is heavy acceleration loads, you reduce or eliminate the drift corrections coming from the accelerometer for that period of time (basically you're flying off of the gyro only). Since gyro drift is slow for a reasonably calibrated gyro, this the simplest way to avoid your attitude estimate being effected by motion. Low acceleration motion will not effect the accelerometer's ability to counter gyro drift. If you NEED to fly very tight, very fast circles for extremely long periods of time, then you must compensate, but for most applications this is not normal.


  • I think you're using a raw gyro and accelerometer, these things are not accurate and have a lot of drift, to solve the drift problem you will need some sort of anchor, like a megnetometer, which is very slow but relatively accurate.

    In MPU6050 there's a built in magnetometer, gyro and accelerometer AND a DSP on top of that, the code for the DSP is proprietary and therefore not available as open source, the way to combine inputs from multiple sensors and derive the output is called sensor fusion, which is probably what you're missing.

    No amount of noise filtering will fix the drift problem for you due to the way MEMS sensors work.

    You can even see the way these drifts are being corrected by the DSP if you move a quadcopter in a certain way, the accelerometer, gyro and magnetometer will get out of alignment and the quadcopter will be unstable for a second or two.

    If you're interested to try a different IMU, try the MPU6050 or MPU9150, both have great performance and some flight controllers use them already and you can get sensor fusion done in hardware by the DSP.

    Take a look at these libraries:



    I2C device library collection for AVR/Arduino or other C++-based MCUs - jrowberg/i2cdevlib
  • My experience has been similar Evorowy.
    Accelerometers and magnetometers are extremely noisy and gyros drift.
    That's just a fact with any of those that isn't thousands of dollars each.
    There's also plenty resources are ways to combine these pieces to form something useful.
    That doesn't mean they don't still have noise though. 
    Because the input data isn't perfect the essential filtering method is to combine the good aspects of the individual measurements, i.e. Kalman or complementary filter. 
    Beyond that, there's only filters that balance between a responsive or a smooth output, these all filters over time i.e. a running average.
    Before spending too much time on it, be sure the amplitude of the noise or drift your seeing is actually consequential.
    There's a really awesome rundown of the behavior of acc, gyro, mag here: https://youtu.be/C7JQ7Rpwn2k

  • Hello.
    Thanks for your replay.

    After lecture of most pitch,roll,yaw I managed to write rather simple fc (base on brokking complimentary filter) but it still contains huge drift. I will try to add some of gathered date to ilustrate that problem.

    Data is made in this way:

    angle_pitch angle_roll


    Let's have a look:

    Used code:

    Plus RadioController profiler which basically converts us to deg <-167.66 ; +167.66>

    Are these formulas wrong?

    Thanks in advance!

    Gyro with low-pass filter could possibly reduce some of the shaking noise. I also use rubber pods under motors in addition to balancing them works quite well. :) Give it a try!

  • I have tried all these things and when it comes to noise there is a lot of it.

    Did you try building the self balancing robot.  It's not as easy as it looks.  The math needed is the same math you need to balance a copter but it only has one direction.

    The motors generate a lot of noise that the gyro sees and reports back as movement.  Whether you use Complimentary Filter or Kalman that noise has to be dampened.  Also the Accelerometer doesn't like vibrations either.

    Most copter code use Quaternions to balance the copter.  This calculation tries to represent the position of the object in 3 dimensions so that you can balance more effectively.  Using Euler angles the adjustment in those three direction can be determined.

    One would have you believe that if you have a sensor that can give you the angle that it's just a simple mater to correct for that angle.  The real world has something to say about that.

    Good luck as I went down your path and failed badly.


    I did make my own radio transmitter and receiver and did connect it to a copter and it did get off the ground but it didn't really fly.

  • Hello!

    Thank you very much for this well of information.

    I've tried to use Kalman Filter with the following code:
    Angle, which was then produced, wasn't really good (on spirit level it shows pitch correctly but the roll was messy and after a while, both angles goes like crazy).

    Next, I wanted to use simple Complimentary Filter with that code:

    but after any movement, it looks like no drift has been corrected.

    Did I do something wrong with math or LSM6 is too sensitive?
    Any help appreciated.

    Used Kalman filter algorithm: http://pastebin.com/QDht6RaW in courtesy of Kristian Lauszus, TKJ Electronics

  • This is also a great resource:
    *-References to Control Algorithms for Existing Open Source Code - Fall 2011 ENGR697 Arducopter
  • This pdf explains much of the math, including gyro drift

This reply was deleted.


DIY Drones via Twitter
RT @chr1sa: After more than a year of only virtual races, @DIYRobocars returns to the newly renovated @circuitlaunch on May 22 for the resu…
8 hours ago
DIY Robocars via Twitter
RT @DAVGtech: And now available with LiDAR 🔥 https://twitter.com/Heavy02011/status/1381137016381964293
8 hours ago
DIY Robocars via Twitter
RT @Heavy02011: #VirtualRaceLeague: @DIYRobocars Race #9 - #ParkingLotNerds #thunderhillracetrack, CA Join us for the next race April 24th,…
8 hours ago
DIY Robocars via Twitter
RT @DWalmroth: Weather's finally cooperating, looking forward to racing 1:10 scale autonomous cars outdoors again! @diyrobocars, @NVIDIAEm…
DIY Robocars via Twitter
RT @AIDRI_: I finally succeeded in optimizing the trajectory and speed of a car on a #racetrack. Next step: implement a 2d controller and…
DIY Robocars via Twitter
@jetdillo @circuitlaunch Actually the second *in person* event in a year. We do virtual races every month
Apr 2
DIY Robocars via Twitter
Update: we're moving it back one day to Sunday (the 4th) at 11:00am instead
Apr 2
DIY Robocars via Twitter
@GrantEMoe @circuitlaunch Update: we're doing it on Sunday (4th) at 11:00am instead
Apr 2
Laurie J. Troy liked Jasper Kueppers's profile
Apr 1
DIY Robocars via Twitter
RT @chr1sa: Maybe we should have a mini @DIYRobocars race in our lower school's playground https://t.co/xLFeua1R6X
Mar 29
DIY Robocars via Twitter
If anybody wants to join us for an informal outdoors hack/race we're going to be meeting at the @CircuitLaunch park… https://twitter.com/i/web/status/1375907409223249923
Mar 27
DIY Robocars via Twitter
RT @SmallpixelCar: Ready to reopen, innovation has to continue. Inside/outside, LiDAR/GPS, race/delivery https://t.co/jpmvttoHEd
Mar 26
DIY Drones via Twitter
RT @DAVGtech: By far best race yet! Congratulations to the winner @Heavy02011 🥇🏆🍾👏👏👏 @diyrobocars @donkey_car @NVIDIAEmbedded https://t.co/…
Mar 20
DIY Robocars via Twitter
RT @Heavy02011: #VirtualRaceLeague: @DIYRobocars Race #8 - #ParkingLotNerds ⁦@DAVGtech⁩ ⁦@DWalmroth⁩ ⁦@OttawaAVGroup⁩ - join us tomorrow h…
Mar 20
DIY Drones via Twitter
RT @mrpollo: 11 years ago, the pxIMU was announced to the world on @DIYDrones, and it changed the life of many (mine included). The followi…
Mar 18
DIY Drones via Twitter
RT @ishcahealth: This is awesome! ☘️ @DroneDJ @DIYDrones @WorkerDrones @DroneMedia_UK @dublinaviation https://twitter.com/media_ireland/status/1372077878536462336
Mar 18