Saturday, December 1, 2012

UAV part 3: C code

Huge UAV progress in the last week. I had figured out that a gyroscope was needed for stable flight (that’s how the stock control boards do it) and installed a gyro on my helicopter, but I wasn’t getting anywhere on coming up with an algorithm that would accomplish stable flight. I kept working in my old source file which was written in assembly and was attempting to write very complex math with 16 bit values (actually 17 bit for some signed values) and it became totally unreadable. After years of working with PIC I reached the point where I couldn’t accomplish the goal without switching to a higher level language, so I set out one day to get things working with the C compiler.

I had been told by many people (including a Microchip FAE) that switching over to C would involve a whole lot of frustration in order to get the whole toolchain working, but it turns out they were full of crap. It was ridiculously easy, and in about 3 hours I had rewritten the entire helicopter source code in C and had it running. I keep running into bugs caused by the way the compiler handles math involving ints (16bit) and chars (8bit) (which I’m often forced to work with based on EEPROM hardware, radio packets, etc.), but once you solve a few you figure out the tricks to force it to work the way you want. Admittedly writing in C creates machine code that is bigger and slower than assembly, so for my cost- and/or time-critical applications (like the ones I write at work) assembly is still the way to go. However, for hobby projects like these, the cost of the processor is meaningless (even something ridiculously overpowered is generally under $1.50), so I have the freedom to run complex code on an awesome processor. Luckily (i.e. as planned) the 16F1824 kicks ass and can run huge amounts of compiled C with no trouble.

Once I got a version of sAAV v3 rewritten in C, I could do experiments and improvements way faster, so sAAVinC (semi-Autonomous Aerial Vehicle in C) evolved pretty fast and I learned all kinds of things about making machines like these fly. The code has now departed quite drastically from what the old assembly source was doing. I have it running a control loop based on the gyroscope reading at 10Hz, and polling the radio for new control packets at 4Hz. After a whole bunch of test flights I arrived at a gyro feedback control algorithm that’s awesome.

It has a proportional term based on a ¼ second moving average (4 readings) and an integral term at 1/4th the gain of the P term. I put in a slow decay on the integral term to counteract erratic buildup. I also do a long (5-second) period of measurements upon power-up to calibrate readings from the gyroscope to establish a baseline of what a rotation rate of zero looks like.

The gyro loop got so good that I didn’t need to do yaw trim any more. With no trimming in the code at all, the helicopter would spin for the first second of flight and then the gyro loop would figure it out and stabilize. To get it even cleaner, I started initializing the gyro integral term to an experimentally-derived value that compensates for extra drag on the top rotor. This initialization is done any time the rotors are stopped for any reason (before I did that, if I was flying and then landed (and possibly moved the heli by hand) and then started flying again, the gyro integral would have been building up the whole time so when I throttled up, only one rotor would getting power).

Another funny thing that happened is the gyro loop was so good that when I tried to send yaw commands, the gyro loop would see the turn as a disturbance and compensate for it, aiming back to the heading it started at. My solution was to remove the yaw command term from the rotor output calculation completely, and instead inject it into the calculations for the gyro P and I terms. Effectively what this accomplishes is: when a yaw command is sent, it makes the gyro loop think the vehicle has turned in the opposite direction, so the gyro loop does its job and counteracts it by turning in the direction the pilot wanted.

In order to make the controller more generic (to use with other vehicles), I moved the math that interprets joystick inputs over to the helicopter. Now, the controller essentially just sends the stick position and button information and the vehicle decides what it means. I’m still doing the same throttle algorithm where pushing the stick up and down adds or subtracts from the current throttle value at increasing intensity as the stick is angled farther from center. I also wrote an update for the controller that puts it into sleep mode so I don’t have to pull the battery when I’m done using it. The last control update was to have the helicopter gradually change to new pitch values (on the tail rotor) instead of changing instantaneously. This came after I kicked on the tail rotor while turning and caused the helicopter to roll and crash spectacularly, breaking the tail fin and tail rotor. Now, when I click the joystick in to toggle forward movement, the tail rotor speed gradually ramps up over a period of about 3 seconds.

The other items I put in the helicopter code are a low-power standby mode and some tones (played on the motor coils) to indicate things like power up, gyro calibration complete, paired with controller, loss of control link, and entering standby.

So now I’m at the point where I have control that’s just as good as the stock helicopter, but running on completely custom code and using a 2-way 2.4GHz radio link instead of infrared. The next steps are to build a more involved controller that will be better able to help develop autonomous functions, and to install ultrasonic altitude sensing on the helicopter. My weight is currently at 39g (stock weight is 42g). I plan to put double-capacity battery on that will bring the weight to 43g, and when I add ultrasonic I’ll strip down more of the metal framework to keep the weight at or below stock.

Here’s a video showing how good the control is. As far as I know, this is now the only Syma S107 in the world that is radio controlled, meaning I don’t need line of sight and I can fly outside in sunlight, which I’ve been wanting to do for so long. The second video shows an outside flight, which is fun because there’s nothing to crash into but difficult because the slightest breeze just carries it away. As you can see, the range on the radio is more than enough to fly so far away that you can’t see it well enough to fly anymore.

Here is the most recent code for the helicopter and the controller.

One more thing: if you have one of these helicopters, take the yellow USB battery charger and throw it in the garbage. I discovered recently that it significantly overcharges the lithium cell, leading to extreme danger of explosions and fire. It actually blew up two small hobbyking cells that I tried to charge with it, after it pushed the cell voltage up to 4.6V. Do yourself a favor and buy one of these chargers (but you should probably replace R2 with something around 5k to set a safe charge current).

No comments:

Post a Comment