Tuesday, October 01, 2013

Quadcopter Flight Controller + Closure on NBF Jeopardy

Let's start with NBF Jeopardy.  First, a video of the semi-final product.


After I created the video, I wanted some status LEDs so I wired them up to the top of the box.  Unfortunately I didn't get any great pictures of it, but imagine 3 very bright and large LEDs on top of the black box and you'll get the gist.  I'm happy to say that the game went without a hitch.  Arduino and game functioned flawlessly and everyone was happy.  I did learn a lesson at the end.  After I added the LEDs, I modified the code to turn the LEDs on when a contestant won.  I uploaded it to the Arduino and ran the test, only to find that nothing worked.  The buttons weren't working at all and the game was detecting button presses before they were pressed.  I took everything apart, checked wiring, checked button functionality, checked the 'mechanical' press and everything looked like it was right.  What the hell could it be?  I combed through the code and nothing was changed.. at all.  Or so I thought.  It turns out that the last time I edited the code, I was toying around and instead of detecting a high position on a switch, I switched it to detecting a low position.  Yup.  I apparently saved this without reverting it back to the correct status.  Yup.  3 hours I spent looking for a hardware problem when at the end of the day, it was a simple HIGH to LOW.  It was a good reminder that I should always a) backup my working code and b) if I make a change for toying around, revert before I save.

So that happened and it wasn't fun, but I got it all sorted out and everything was great.  Onto my next project.

I posted before about my quadcopter.  It's a ton of fun to fly around and see how high I can get without crashing.  I was using a OpenPilot CC3D flight controller because it's Open Source and fun to tinker with.  I also love the ground control software (GCS) and how easy it was to get everything set up.  There's a wizard that walks you through every single step from setting up your bird to setting up your controller.  Can't beat that.  Unfortunately, a few days ago I was flying in some high winds.  I'm not sure what happened, but out of nowhere my quad flipped a few times and slammed hard into the ground.  It was sad and I should've gotten pictures, but I ended up with 2 broken quad arms, all props were done for and the worst part: a broken cc3d.  At first I thought all was well, but then I noticed that channel 1 was no longer receiving input and the cc3d was not able to engage the speed controllers.  I tested every component of the quad separately and they all functioned fine.  It really was the cc3d.

So my next step was to research flight controllers.  I found several great ones.  ArduPilot stood out as my possible next purchase.  MultiWii looked awesome as well.  Then I remembered the main reason I got into this: I wanted to eventually make my own flight controller.  I have an Arduino.  I have sensors.  Why not try?

So I got to tinkering.  I started by ordering a BMP085 Pressure Sensor (for detecting altitude changes).  I also ordered a L3GD20 3-axis Gyro to pair with an accelerometer and detect the angle of the quad.  I have the accelerometer and a GPS already, along with an Arduino Uno, Micro, Mega 2560 and who knows what other Arduino.

So while I waited for the sensors I ordered to come in, I started reading about how I can get input from the receiver.  This was much simpler than I thought.  First off I tried using pulseIn().  This seemed like the most obvious approach until I realized that pulseIn halts the code until it either a) times out or b) receives input.  This is not acceptable for a craft that needs to stay in the air.  I then read about interrupts and found the pinChangeInt library.  This allowed me to use several pins on the Arduino as inputs from my receiver.  There are several approaches to measuring the signal, but the simplest one was getting the time in micro() from when the interrupt was HIGH and subtracting the time from when the interrupt goes LOW.  The difference is what I can convert to throttle, stick and switch position.  Once I got that squared away, it was time to convert all the stick inputs to something that can control 4 motors.

I'd like to start off all of this with a disclaimer.  I am not an electrical or aeronautical engineer.  I just love writing software.  There are probably better approaches to what I'm doing, but this is a great lesson for me and I'm going to plow forward and tweak as I go along.

So for starters, everything has to run on throttle input.  One of the issues I found was that the transmitter wasn't exact.  With the sticks not doing anything, I would get a variance of -40 to 40 for the input value.  I helped solve this by implementing an averaging filter.  I also only update the values to the ESC if the average changes by 6.  I found this to greatly reduce the "noise" I was getting.  Once I got that squared away, it was time to calculate the throttle.  Throttle is the foundation of every single future calculation.

  float throttleToUse = (1000 * throttlePercent) * THROTTLE_RATE;

Need to use floats because we need some precision.  throttlePercent is based off of stick input and THROTTLE_RATE is a constant that can be set to adjust how powerful the stick really is.  A rate of 1 is 1:1.  .8 smooths things out a little bit and generally the lower you go the less responsive the quad becomes.  It's the equivalent of twisting the throttle on a 50cc scooter and a 1000cc superbike.  

So now that we have the throttle we want to use, we can adjust the other calculations.

    float rollAmount = (throttleToUse * rollPercent) * ROLL_RATE;
    float pitchAmount = ((throttleToUse - rollAmount) * pitchPercent) * PITCH_RATE;
    float yawAmount = ((throttleToUse - rollAmount - pitchAmount) * yawPercent) * YAW_RATE;

I am not sure if these are right just yet.  So far they work in my serial window and the numbers look good.  I haven't done a flight test yet to confirm though.

Now that we have that under control, we need to convert these values into something the speed controllers can use.  First off, we need to make sure the motors are set up correctly.  I'm using the same motor setup that the CC3D uses which is:

Motor 1 = Front Left
Motor 2 = Front Right
Motor 3 = Rear Right
Motor 4 = Rear Left

The motors are defined in a clockwise pattern from the front left.  Knowing that, we can use the following to set the motor outputs:

  motorValues[1] = 1000+(throttleToUse + rollAmount - pitchAmount + yawAmount); // cw, front left
  motorValues[2] = 1000+(throttleToUse - rollAmount - pitchAmount - yawAmount); // ccw, front right
  motorValues[3] = 1000+(throttleToUse - rollAmount + pitchAmount + yawAmount); // cw, rear right
  motorValues[4] = 1000+(throttleToUse + rollAmount + pitchAmount - yawAmount); // ccw, rear left

1000 is the speed controller off position.  With the throttle being all the way down, nothing is going to happen.  As the throttle increases, the rate of roll, pitch and yaw can be adjusted.

Now that I had this setup (and tested in the serial output), I wanted to add altitude hold.  By now my sensors had come in and I have both the gyro and barometer connected through i2c.  The Arduino libraries make reading these inputs very, very easy.

For barometer, we just run bmp.readAltitude(); to get altitude.  I have a mix on my transmitter to allow a 3 position switch on channel 6.  Position 0 is normal flight.  Position 1 is altitude hold and position 2 is position hold (using GPS).  To hold the altitude, I get the current altitude as soon as that switch is put on position 1.  Each time I run through the loop, I get the current altitude and find the difference.  I then use that difference to calculate whether I need more or less throttle (to go up or down).

So right now, all libraries and my code are 13,634 bytes.  This leaves me 18,622 bytes left to finalize everything.  I still need to implement position hold and Attitude mode which I'm thinking is going to take me up to ~20,000 bytes.  I will do my best to do some optimization at that point and see how low I can get the codesize to be.  One of the things I don't like right now is that it's all purely functional.  There's no OOP present at all (besides the obvious stuff).  This was fine for a prototype, but I want to clean this up for future use.

So with everything running and looking ok in serial mode, I wanted to do a power-on test.  I plugged everything in and drew power from the speed controllers.  Everything turned on.  Then I plugged the speed controller power into the arduino.  Things beeped and the Arduino indicated it was ready.  I gave my tx a little throttle and the speed controllers burst to life.  5% max.  I then adjusted the pitch, roll and yaw and I could hear the motors adjusting accordingly.  I killed the power afterwards and considered it a successful first power-on test.

So what's next?

First, I need to fabricate a case for everything.  I have a spare proto-shield that I will set up so I can attach the sensors and GPS onto.  I'll then attach that onto my UNO and put together some pins for the ESCs (speed controllers) to attach to.

Once that's set up, I'll mount it to my code and have a first flight.  I'll start my having it strapped down just to be safe.

After first flight, I'll continue to refine the code more.

Here is a picture of my current setup:


I'm calling this flight controller dx.  dx had a successful first power-on test.  Within the next couple of weeks, I hope to be able to report that it also had a successful first flight.  Until then.. ~