Sunday, August 4, 2013

Part 8: Control Laws!!!!!

List of parts to blog:
Part 1: Quadcopter Design
Part 2: Integrating rc Transmitter and Receiver with Arduino
Part 3: ESC and motors (wiring and code)
Part 4: Sensors (reading in raw data and applying calibration)
Part 5: IMU - Kalman Filter Orientation Estimator
Part 6: Discussion on Arduino Boards
Part 7: Designing a shield for the Due
Part 8: Control Laws!!!!!


Part 8: Control Laws!!!!
my favorite part, designing the control laws to fly the quadcopter


I started with a simple PID control law based on both attitude (angles) and rates. What I have right now is controlling pitch and roll angle and for yaw controlling yaw rate. I'll include a more detailed diagram later, as well as the code.

Part 7: Designing a shield for the Due

List of parts to blog:
Part 1: Quadcopter Design
Part 2: Integrating rc Transmitter and Receiver with Arduino
Part 3: ESC and motors (wiring and code)
Part 4: Sensors (reading in raw data and applying calibration)
Part 5: IMU - Kalman Filter Orientation Estimator
Part 6: Discussion on Arduino (boards and oddities)
Part 7: Designing a shield for the Due
Part 8: Control Laws!!!!!


Part 7: Designing a shield for the Due
I started this project using an Arduino Mega and I ripped the headers off and soldered straight to the board. This became a nightmare. I needed to move some of the wires to new pins but it was going to be impossible to get in there to desolder and resolder them. Here is a picture of the rats nest. Also my ground "plane" was a blob of solder that had like 12 wires going into it... this was quite the kluge. Tons of heat shrink and electrical tape wrapped around stuff. Scared me flying the thing, a wire could come loose at any second or something could short out.

I wanted to completely re-do all of the wiring. At the same time computational needs were pushing me towards the Due with the 84Mhz  ARM 32-bit chip. This will be hundreds of times faster than the Mega.

To do things the right way I needed to build a custom shield to sit on top of the Due to solder all of my connections into. I wanted to include plenty of room for expansion also which  means:
8 motors
9 channel reciever
4 Serial devices (GPS, SD card data logger, bluetooth interface, and a sensor device)
plenty of I2C devices (pressure sensor, IMU alternative to serial)
voltage regulator (ignore this for now, I have a use for it much later)
LEDs (red, yellow, green for status)
a buzzer for notifications (during quad testing I found this helpful)
spare set of 5 to 3.3 volt conversions in case a sensor is a 5v system
digital pin with a switch (I envision putting LED's that strobe or something around the frame but I want the option to turn it off)
maintain holes for as many other pins as possible.


With all of that in mind I set out to design a board. From what I read there is one standard software package for designing PCBs: Eagle. They have a freeware version that allows you to make small boards. The Arduino Due footprint falls within the freeware allowance. I must note I have no experience at all designing boards at all, never used eagle. I found it to be very easy to use. You just drop pin holes in and then draw the routing in the 2 layers. It ends up becoming a sort of puzzle to get everything laid out. I used mostly the top layer for routing, leaving the bottom layer almost completely a ground plane. I only had to use 1 set of vias. Here is the eagle board layout, note I later filled the bottom planes open spaces with ground.


For anyone who hasn't used Eagle and is scared away from designing your board, don't be. Its very easy to learn and use.

So after spending hours and hours laying all of that out... it was time to send it to get fabricated. There is a service called OSHpark that combines many peoples small projects together then has them fabricated on a giant panel. It helps reduce the cost for people like me doing simple prototype boards. Their pricing is fairly reasonable and only takes a couple weeks. Took my board only a couple days to be sent to fabrication. Then hopefully only a week for me to get it back in the mail.

Here is the render I got form OSHpark as to what the final board should look like:


[waiting for board to arrive from OSHpark, more to follow soon]

Part 6: Discussion on Arduino Boards

List of parts to blog:
Part 1: Quadcopter Design
Part 2: Integrating rc Transmitter and Receiver with Arduino
Part 3: ESC and motors (wiring and code)
Part 4: Sensors (reading in raw data and applying calibration)
Part 5: IMU - Kalman Filter Orientation Estimator
Part 6: Discussion on Arduino (boards and oddities)
Part 7: Designing a shield for the Due
Part 8: Control Laws!!!!!



Part 6: Discussion on Arduino Boards
I started with an Arduino Uno since I had several laying around. Realistically I was planning to use an Arduino Mega. So I got one, I ripped the headers off and desoldered the little clips. I've read online people saying this is hard to do, its really not. It took me maybe an hour. Just take some pliers to those black headers and bend them right off. Theres 2 little prongs under there you can just desolder off.

The problem I ran into with using the Mega was the number of interrupt routines it can use. I wanted 6 channels, while its totally possible, it wasn't real clear how to do this on the Mega. The computing power, being 8bit is severely limited. I want to be able to use doubles and not have to worry!

So I finally got a Due and this thing is awesome. Its quick and each pin can have an interrupt. It has everything I've ever wanted in a micro controller.... except a lot of libraries are still a work in progress like something as simple as tone() doesn't work. I've found workarounds for everything though. I still love how fast this thing is compared to the mega. My kalman filter runs in microseconds, can't even measure things in millis() anymore, now its all micros(). I am hoping to run my control loop at 200hz or more now, previously on the mega I was struggling to make 50hz.

After experience with the Mega, i decided to build a shield for the Due rather than rip the headers off. The next part details the design of the shield.

[place holder]

Part 5: IMU - Kalman Filter Orientation Estimator

List of parts to blog:
Part 1: Quadcopter Design
Part 2: Integrating rc Transmitter and Receiver with Arduino
Part 3: ESC and motors (wiring and code)
Part 4: Sensors (reading in raw data and applying calibration)
Part 5: IMU - Kalman Filter Orientation Estimator
Part 6: Discussion on Arduino (boards and oddities)
Part 7: Designing a shield for the Due
Part 8: Control Laws!!!!!


Part 5: IMU - Kalman Filter Orientation Estimator

Part 5A: Quaternion based Kalman Filter design





The kalman filter is the optimal estimator but comes at the price of computational time. There is a lot of matrix math and a matrix inversion that has to be done. But with the Arduino Due I should have plenty of power to handle it.


Part 5B: Simulink/Matlab simulation testing/tuning
I designed the Kalman filer and tuned it in Matlab/Simulink. I recorded data from my sensors and built up a noise model. Then fed the data into a kalman flter block running the same C-code as I put on the Arduino.

Part 5C: Arduino Code

Part 4: Sensors (reading in raw data, applying calibration)

List of parts to blog:
Part 1: Quadcopter Design
Part 2: Integrating rc Transmitter and Receiver with Arduino
Part 3: ESC and motors (wiring and code)
Part 4: Sensors (reading in raw data and applying calibration)
Part 5: IMU - Kalman Filter Orientation Estimator
Part 6: Discussion on Arduino (boards and oddities)
Part 7: Designing a shield for the Due
Part 8: Control Laws!!!!!


Part 4: Sensors (reading in raw data, applying calibration)
Just a warning, this section is going to be long. And post Part 4 is only half of it. This part only covers getting the sensor up and running and then applying a calibration to the sensor data. Part 5 will then be on how to use that data to determine which way the quadcopter is pointing.

Originally I had wanted to use the ATmega328 on the 9dof IMU razor board to offload the arduino and do all of the sensor data processing there, but that was when I was planning to use an Arduino mega. Now that I already bought the thing and am using an Arduino due, I want to do all the heavy lifting on the due side. All the ATmega needs to do is query each sensor and then send that data to the Arduino. Pretty lame I know, i might use the ATmega328 to do some filtering of the data, but for now its just a pass through. In reality I should have bought this chip for 25 dollars less:https://www.sparkfun.com/products/10724
 I recommend anyone reading this planning to use a Due to get this rather than the 9dof imu razor. Just cut out the middle man ATmega328. Let the Due query the sensors for data itself. But it is what it is. If you do go this way you can take most of my code that I put on the ATmega328 and just put it on the Due instead. It should be EXACTLY the same, then you can delete all the junk I had to make to set up serial communication between the Due and the ATmega328. Or if you just want to use my code without having to modify it, just go with the 9dof Razor... The last section in this part, Part 4C is important though, the data must be calibrated before you use it, and the calibration process is nontrivial.

Part 4A: i2c communication to each individual sensor chip, ftdi programming ATmega328

Part 4B: Getting data to the arduino (setting up serail comm between ATmega328 and Arduino)

Part 4C: Calibration of sensor data

Part 4D: Final code

Saturday, August 3, 2013

Part 2: Integrating rc transmitter and receiver with Arduino

List of parts to blog:
Part 1: Quadcopter Design
Part 2: Integrating rc Transmitter and Receiver with Arduino
Part 3: ESC and motors (wiring and code)
Part 4: Sensors (reading in raw data and applying calibration)
Part 5: IMU - Kalman Filter Orientation Estimator
Part 6: Discussion on Arduino (boards and oddities)
Part 7: Designing a shield for the Due
Part 8: Control Laws!!!!!



Part 2: Integrating transmitter and receiver with Arduino
The following video is ultimately what you should end up with after this post and the next (parts 2 and 3).  Part 2 is the transmitter and receiver, part 3 will be the esc and motor setup. Basically what I have is the transmitter in my hand is sending signals to the receiver which then sends that to the Arduino mico controller which read it in then sends a command back out to the ESC which controls the motor. In this post I'll detail how to wire all of this up and also include the c-code that I used on the Arduino.




Part 2A: Wiring up Receiver -> Arduino
First thing that needs to be said is the Arduino Due is a 3.3volt system and the receiver is a 5v system. So a 0 signal from the receiver is 0 volts and a 1 signal is 5v's. This 5v signal will destroy the arduino Due, it requires 0 to 3.3v. This is very very important and there is a very simple way to step down the 5v signal to 3.3v because 3.3 = 5*2/3. So the following circuit will do the job:

Where R1 = half of R2. You can pick R1 = 10k ohms then R2 would be 20k. Or R1 = 5k ohms and R2 = 10k. Instead of doing this yourself, you can get these tiny logic level shifters from sparkfun that do exactly this. Beware they have made a mistake and use 10k Ohms for each. This will be close but not exact but I've used them for a while now and have had no issue with frying the due. But I later fixed this anyways. I basically did this for all 6 channels. Here is 2 channels with a logic level board from sparkfun wired up:


Here is how its wired to the arduino. Sorry for the messy image, this was taken from a graphic of the entire wiring diagram for the quad. Here green signals are 0to5v, yellow are 0to3.3, orange is 3.3volt power, red is 5v power, black is ground. (please don't hate me for the color inconsistency between this and the picture I took above...)


Part 2B: Arduino code for reading in data from rc receiver.
Some quick background. The signal from the receiver is a PWM (or actually more accurately PPM). The receiver sends a pulse for each channel every so often thats about 1000 to 2000 microseconds long. So for the throttle a pulse that lasts 1000 microseconds means zero throttle, 2000 microseconds means full. Same for all channels, so with Elevator and aileron sticks centered the receiver sends a signal every so often that is 1500 microseconds long. If you push up, it will go to 2000 microseconds long, 1000 if you pull back on the stick. Fairly simple. The problem is when we connect that to the Arduino.  Who knows when that signal is going to arrive and we need to very accurately measure exactly when it started and when it ended then we can calculate how long it was. To do this we use interrupt routines. Basically what this does is any time that pin goes from 0 to 1, the arduino stops what its doing and tells us "hey this pin just changed" so then we record the time of that event. If it changes from 1 to 0, we know thats the end of the signal then we can calculate how long it lasted. Again fairly straight forward. Here is the code to implement this.  I stole a lot of this code from: http://rcarduino.blogspot.com/2012/01/how-to-read-rc-receiver-with.html I recommend reading this guys blog. He explains it wayyy better than me. The basics are: attach interrupt, write a intrupt routine that checks if pin went high or low and records time, then some fancy volatile variable use to store the recorded variable (fanciness is incase you get interrupted while writing to the variable, you will write half then write other half but the 2 halfs might not match so you do some programming logic to stop that)

Here is the code: (for just throttle and pitch stick)
=======================================================================
#define THROTTLE_PIN 53 // thro receiver signal connected to pin 53
#define ELEV_PIN 49  // ELEV receiver signal connected to pin 49

volatile boolean bNewSignal_throttle = false;  //Used to determine if a new command w
volatile boolean bNewSignal_pitch = false; 
volatile unsigned long ulStartPeriod_throttle = 0;
volatile unsigned long ulStartPeriod_pitch = 0;
volatile int throttle_pos_v = 1000; // _v set in ISR, stored for use in throttle_pos without _v
volatile int pitch_pos_v = 1500;
static int throttle_pos = 1000; //actual variable to use in code as the command
static int pitch_pos = 1500;

void setup() {
  Serial.begin(19200);

  attachInterrupt(THROTTLE_PIN,ISR_throttle,CHANGE); //(Pin, code to run when triggered - see
 // ...function at end named ISR_throttle, type of trigger ie when pin changes)

attachInterrupt(ELEV_PIN,ISR_pitch,CHANGE);

}

void loop() {

//Update Input commands (trickery to prevent being interrupted while writing to the same variable)
  noInterrupts();
  if(bNewSignal_throttle)
  {
    throttle_pos = throttle_pos_v;
    bNewSignal_throttle = false;
  }

    if(bNewSignal_pitch)
  {
    pitch_pos = pitch_pos_v;
    bNewSignal_pitch = false;
  }
 interrupts();

  Serial.print("Throttle:");
  Serial.print(throttle_pos);
  Serial.print("\t");
  Serial.print("Pitch:");
  Serial.print(pitch_pos);
  Serial.print("\t");
  Serial.println();
}


// Interrupt Routines
void ISR_throttle() {
  if(digitalRead(THROTTLE_PIN) == HIGH) //means it went 0 to 1
    ulStartPeriod_throttle = micros();
  else // it went from 0 to 1, thus end of pulse
  {
    if(ulStartPeriod_throttle && (bNewSignal_throttle == false))
    {
      throttle_pos_v = (int)(micros() - ulStartPeriod_throttle);
      ulStartPeriod_throttle = 0;
      bNewSignal_throttle = true;
}  }}
void ISR_pitch() {
  if(digitalRead(ELEV_PIN) == HIGH)
    ulStartPeriod_pitch = micros();
  else
  {
    if(ulStartPeriod_pitch && (bNewSignal_pitch == false))
    {
      pitch_pos_v = (int)(micros() - ulStartPeriod_pitch);
      ulStartPeriod_pitch = 0;
      bNewSignal_pitch = true;
}  }}
============================================================


That should do it, you should see the data on the serial display. Copy and paste code to add the other channels. Next up, using that data to then send a command to spin the motor.

Part 3: ESC and Motor (wiring and code)

List of parts to blog:
Part 1: Quadcopter Design
Part 2: Integrating rc Transmitter and Receiver with Arduino
Part 3: ESC and motors (wiring and code)
Part 4: Sensors (reading in raw data and applying calibration)
Part 5: IMU - Kalman Filter Orientation Estimator
Part 6: Discussion on Arduino Boards
Part 7: Designing a shield for the Due
Part 8: Control Laws!!!!!


Part 3: ESC and Motor (wiring and code)
Detailed in this section is flashing the ESC's, wiring them up, and sending commands from the Arduino to control the motors.

Part 3A: Flashing ESCs
Okay so in the video in Part 2, you can hear the high pitch noise from the motor. Yah thats annoying so to get rid of that you have to run the ESC at a higher rate. A guy by the name of SimonK has done this for us. He also has optimized the programming for quadcopters. His firmware is the standard for qaudcopters. Read about it here: http://www.rcgroups.com/forums/showthread.php?t=1513678

For the ESC's that I have I found a very nice page that details step by step on how to flash them:
http://www.rchacker.com/diy/simonk-esc-firmware-flashing
Following these steps from start to finish it took me maybe 45 minutes. Worked great. Now my motors spin nice and quiet and awesome.
Essentially what you do is you first have to buy an AVR programmer to connect your computer to the ESC via USB to the MOSI, MISO, etc pins on the ESC to reprogram the chip. Then use some software from here:
http://lazyzero.de/en/modellbau/kkmulticopterflashtool
which then flashes the ESC with the SimonK software.

Part 3B: Wiring ESC and Motors
Wiring for motors is simple.3 plugs on motor, 3 similar plugs on ESC  (or in my case no plugs and I had to buy and solder some bullet plugs onto the wires). Just make sure the middle plug from the ESC goes into middle cable of motor. The outer wires can be swaped. If you put the left one into right most on motor, the motor will spin on way. Do opposite and motor spins the other way.
Connect battery red and black to the ESC red and black wires (i soldered mine to the base plate of my frame and added some plugs).
Then the ESC has a servo plug (red brown orange cable. plug the brown into the ground on the arduino so that they have a common ground. Then the orange wire needs to be connected to a digital pin on the arduino:




Part 3C: Arduino Code for ESC and Motors
Okay so the ESC is looking for a signal exactly like the signal from the receiver, a PPM. So we need to send a pulse out the is x microseconds long where x is 1000 to 2000, with 1000 essentially being a 0 and 2000 meaning spin motor at full speed. Here is the code to do this, luckily Arduino has a library called servo that writes these pulses for us very easily.

The following code is integrated with the receiver code from the previous section, so now the throttle on the transmitter should control all four motors
====================================================================
#include <Servo.h> //standard arduino library to send signals to ESC


#define THROTTLE_PIN 53

#define MOTOR_1_PIN 36
#define MOTOR_2_PIN 38
#define MOTOR_3_PIN 40
#define MOTOR_4_PIN 42

//Receiver variables
volatile boolean bNewSignal_throttle = false; 
volatile unsigned long ulStartPeriod_throttle = 0;
volatile int throttle_pos_v = 1000;
static int throttle_pos = 1000;

// Motor Vars: FR = front right, FL = front left, BR = back right...
int motor_fr_cmd = 1000;
int motor_fl_cmd = 1000;
int motor_br_cmd = 1000;
int motor_bl_cmd = 1000;
Servo motor_FR;
Servo motor_FL;
Servo motor_BR;
Servo motor_BL;

void setup() {

 // Receiver interrupt routines
 attachInterrupt(THROTTLE_PIN,ISR_throttle,CHANGE)

  //Motor Servos
  motor_FR.attach(MOTOR_1_PIN);
  motor_FL.attach(MOTOR_2_PIN);
  motor_BR.attach(MOTOR_3_PIN);
  motor_BL.attach(MOTOR_4_PIN);
}

void loop() {

 noInterrupts();
  if(bNewSignal_throttle)
  {
    throttle_pos = throttle_pos_v;
    bNewSignal_throttle = false;
  }
 interrupts();

  // Send commands to ESCs
  motor_FR.writeMicroseconds(throttle_pos);
  motor_FL.writeMicroseconds(throttle_pos);
  motor_BR.writeMicroseconds(throttle_pos);
  motor_BL.writeMicroseconds(throttle_pos);

 delay(5);
}


// Interrupt Routines
void ISR_throttle() {
  if(digitalRead(THROTTLE_PIN) == HIGH) //means it went 0 to 1
    ulStartPeriod_throttle = micros();
  else // it went from 0 to 1, thus end of pulse
  {
    if(ulStartPeriod_throttle && (bNewSignal_throttle == false))
    {
      throttle_pos_v = (int)(micros() - ulStartPeriod_throttle);
      ulStartPeriod_throttle = 0;
      bNewSignal_throttle = true;
}  }}
====================================================================