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.
Evorowy

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

Join diydrones

Email me when people reply –

Replies

  • 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

    • 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.
        http://philstech.blogspot.com/2014/09/fast-quaternion-integration-f...
        http://philstech.blogspot.com/2015/06/complimentary-filter-example-...

        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.

        Regards,

        Phillip
        http://well.My/
  • 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:

    https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

    https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU9150

    jrowberg/i2cdevlib
    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

    THROTLE_PID PITCH_PID ROLL_PID YAW_PID

    Let's have a look:
    http://pastebin.com/XGUGmNyD

    Used code:
    http://pastebin.com/wYZwRhBp


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

    Are these formulas wrong?

    Thanks in advance!

    PS.
    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.

    PS

    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:
    http://pastebin.com/FgN67Gqm
    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:
    http://pastebin.com/NRvxSX48

    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.

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

  • This is also a great resource:
    https://sites.google.com/site/fall2011engr697arducopter/home/refere...
    *-References to Control Algorithms for Existing Open Source Code - Fall 2011 ENGR697 Arducopter
  • This pdf explains much of the math, including gyro drift

    https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf
    https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf
This reply was deleted.

Activity

Neville Rodrigues liked Neville Rodrigues's profile
Jun 30
More…