My Very Own Quadcopter X Acro Mode Firmware

As I have gotten into multirotors the one and only firmware that i have used is Alex's awesome MultiWiiCopter for Arduino's. For a challenge/fun, i decided to make my own basic firmware so that i can learn more about how MR firmware works. It is by no means advance and will only do basic acro flight in the beginning (i dont think it will ever do more?), so it will only be useful for reference and not wide spread usage (it would only split the community even more if it ever managed to be good). The previous posts you may have seen about creating IMU code for my Pololu IMU, failed two wheel balancing robots (that may work now...), and the RC car mods all lead up to this. If i opened them all up side by side i could easily see where things are spliced in and modified for the quad firmware. Instead of doing one huge project i split them up into easy parts so now the only part is blending in quad theory. The code below is NOT finished.

Check out the MulltiWii rev 1.0 firmware. It is very basic compared to the latest rev 2.1 and helped me a lot in understanding how it worked.

will expand more later...

Code:

//include interrupt,eeprom, and wrie libraries
#include <PinChangeInt.h>
#include <EEPROM.h>
#include <Wire.h>

//I2C addressses for IMU
#define GYR_ADDRESS (0xD2 >> 1)
#define MAG_ADDRESS (0x3C >> 1)
#define ACC_ADDRESS (0x30 >> 1)

//Gyro, ACC, Mag enable registers
#define L3G4200D_CTRL_REG1 0x20
#define LSM303_CTRL_REG1_A 0x20
#define LSM303_MR_REG_M 0x02

//acc settings
#define CTRL_REG4_A 0x23

//gyrto settings
#define CTRL_REG4 0x23

//Gyro, Acc, Mag output registers
#define L3G4200D_OUT_X_L 0x28
#define LSM303_OUT_X_L_A 0x28
#define LSM303_OUT_X_H_M 0x03

//ACC Reg data
int ACCx;
int ACCy;
int ACCz;
//Bias
int bACCx;
int bACCy;
int bACCz;

//Gyro Reg data
int GYROx;
int GYROy;
int GYROz;
//Bias
int bGYROx;
int bGYROy;
int bGYROz;

//MAG REG data
int MAGx;
int MAGy;
int MAGz;

//YAW calc
int xMAGMax;
int yMAGMax;
int zMAGMax;
int xMAGMin;
int yMAGMin;
int zMAGMin;
float xMAGMap;
float yMAGMap;
float zMAGMap;


//gyro/acc gain-converts raw values, gyro deg/s, acc to Gs
#define AccGain 3.9      //8g
#define GyroGain 70     //2000dps

//--kalman------------------------------------------------------------
float giroVar = 0.1;
float deltaGiroVar = 0.1;
float accelVar = 5;
float Pxx = 0.1; // angle variance
float Pvv = 0.1; // angle change rate variance
float Pxv = 0.1; // angle and angle change rate covariance
float kx, kv;

//time, used in kalman filtering to keep a constant loop time nad therefor to have a good gyro deg calc from deg/s
unsigned long timer = 0;
unsigned long timer1 = 0;
float timeStep = 0.02;          //20ms. Need a time step value for integration of gyro angle from angle/sec

//calculations
int pitchAccel;
int pitchGyro;
int rollAccel;
int rollGyro;

//final xyz values
float pitchPrediction = 0; //Output of Kalman filter, final pitch value
float rollPrediction = 0;  //Output of Kalman filter, final roll value
float yawRaw=0; //output of yaw calculator

//--PID------------------------------------------------------------
//awesome explanation: http://www.x-firm.com/?page_id=193

//change for stability
#define Kp 3   //proportional
#define Ki 10   //Integral
#define Kd -15   //derivative

//for storing calculated values later on
//       {roll,pitch,yaw}
int ePID[3] = {
  0,0,0}; //-error-
int waPID[3] = {
  0,0,0}; //-the wanted angle for each axis
int caPID[3] = {
  0,0,0}; //-   current
int pPID[3] = {
  0,0,0}; //holds proportional term for each axis
int iPID[3] = {
  0,0,0}; //-integral-
int dPID[3] = {
  0,0,0}; //-derivitive-
int pePID[3] = {
  0,0,0}; //-previous error-
int oPID = {
  0,0,0}; //final ouput of PID calculations

//RX READING STUFF--------------------------------------------------

#define jitterThresh 6 //jitter protection/deadband

//RX input pins
#define ailPin 2 //ch1
#define thrPin 4 //ch2
#define elePin 5 //ch3
#define rudPin 6 //ch4
#define aux1Pin 7 //ch5
#define aux2Pin 8 //ch6

// These bit flags are set in bUpdateFlagsShared to indicate which
// channels have new signals
#define ailFlag 1
#define thrFlag 2
#define eleFlag 3
#define rudFlag 4
#define aux1Flag 5
#define aux2Flag 6

// holds the update flags defined above
volatile uint8_t bUpdateFlagsShared;

//values of RX ch used between ISR and loop
volatile uint16_t ailShared;
volatile uint16_t thrShared;
volatile uint16_t eleShared;
volatile uint16_t rudShared;
volatile uint16_t aux1Shared;
volatile uint16_t aux2Shared;

//vals used only in loop so not volatile
uint32_t ailStart;
uint32_t thrStart;
uint32_t eleStart;
uint32_t rudStart;
uint32_t aux1Start;
uint32_t aux2Start;

//EEPROM addresses for holding calib values.
#define ailMinIndex 0
#define ailMidIndex 1
#define ailMaxIndex 2

#define thrMinIndex 3
#define thrMidIndex 4
#define thrMaxIndex 5

#define eleMinIndex 6
#define eleMidIndex 7
#define eleMaxIndex 8

#define rudMinIndex 9
#define rudMidIndex 10
#define rudMaxIndex 11

#define aux1MinIndex 12
#define aux1MidIndex 13
#define aux1MaxIndex 14

#define aux2MinIndex 15
#define aux2MidIndex 16
#define aux2MaxIndex 17

boolean needCalib = false;

//holds values for min/max of each channel
int calibValues[17] = {
  1500,0,1500,1500,0,1500,1500,0,1500,1500,0,1500}; //ail min (0), mid (1), max (2) //thr min (3), mid (4), max (5) //ele min (6), mid (7), max (8)
//rud min (9), mid (10), max (11) //aux1 min (12), mid (13), max (14) //aux2 min (15), mid (16), max (17)

//--MOTOR SETUP----------------------------------------------------------------------------
#include <Servo.h>

//make servo objects
Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;


//motor pins
#deine motor1Pin 3
#deine motor2Pin 9
#deine motor3Pin 10
#deine motor4Pin 11

//motor values: 1,2,3,4
int motorVals[4] = {0,0,0,0};


void setup()
{
  Serial.begin(115200); //begin serial comm.

  Wire.begin(); //start Wire for IMU

  writeGyroReg(L3G4200D_CTRL_REG1, 0b10101111); //enable gyro, 0b10101111, 1010- 400hz ODR/50hz cutoff, 1111- default(enable all axis/normal mode)
  writeAccReg(LSM303_CTRL_REG1_A, 0b00110111); //enable acc, 0b00110111, 001- normal mode, 10- 400hz ODR, 111- default(enable all axis)
  writeMagReg(LSM303_MR_REG_M, 0x00); //enable mag

  //gyro settings
  writeGyroReg(CTRL_REG4, 0b00110001); //0-continous update, 0- little endian, 11 2000dps, 0- blank,  00- self test disabled, 0 spi 4 wire(if used)

  //acc settings
  writeAccReg(CTRL_REG4_A, 0b00110000); //0- continuous update, 0- little endian, 11- 8g scale, 00 default, 0- blank, 0- self test disabled

  CalibrateIMU(); //calibrate the IMU for level starting pos.

  //set RX input pins
  pinMode(thrPin, INPUT);
  pinMode(ailPin, INPUT);
  pinMode(elePin, INPUT);
  pinMode(rudPin, INPUT);
  pinMode(aux1Pin, INPUT);
  pinMode(aux2Pin, INPUT);

  //RX input interrupts
  PCintPort::attachInterrupt(ailPin, ailCalc,CHANGE);
  PCintPort::attachInterrupt(thrPin, thrCalc,CHANGE);
  PCintPort::attachInterrupt(elePin, eleCalc,CHANGE);
  PCintPort::attachInterrupt(rudPin, rudCalc,CHANGE);
  PCintPort::attachInterrupt(aux1Pin, aux1Calc,CHANGE);
  PCintPort::attachInterrupt(aux2Pin, aux2Calc,CHANGE);

  //atach the motors
  motor1.attach(motor1Pin);
  motor2.attach(motor2Pin);
  motor3.attach(motor3Pin);
  motor4.attach(motor4Pin);


  if(needCalib)
  {
    CalibSticks();
  }
  else
  {
    readEEPROM();
  }
}

void loop() {
  timer = millis(); //loop begin time

  //read sensors
  readGyro();
  readAcc();
  readMag();

  //calcualte pitch, roll, yaw, kalman etc
  Calculations();

  // create local variables to hold a local copies of the channel inputs
  // these are declared static so that their values will be retained
  // between calls to loop.
  static uint16_t ailIn;
  static uint16_t thrIn;
  static uint16_t eleIn;
  static uint16_t rudIn;
  static uint16_t aux1In;
  static uint16_t aux2In;
  // local copy of update flags
  static uint8_t bUpdateFlags;

  // check shared update flags to see if any channels have a new signal
  if(bUpdateFlagsShared)
  {
    noInterrupts(); // turn interrupts off quickly while we take local copies of the shared variables

      // take a local copy of which channels were updated in case we need to use this in the rest of loop
    bUpdateFlags = bUpdateFlagsShared;

    // in the current code, the shared values are always populated
    // so we could copy them without testing the flags
    // however in the future this could change, so lets
    // only copy when the flags tell us we can.

    if(bUpdateFlags & ailFlag)
    {
      ailIn = ailShared;
    }

    if(bUpdateFlags & thrFlag)
    {
      thrIn = thrShared;
    }

    if(bUpdateFlags & eleFlag)
    {
      eleIn = eleShared;
    }

    if(bUpdateFlags & rudFlag)
    {
      rudIn = rudShared;
    }

    if(bUpdateFlags & aux1Flag)
    {
      aux1In = aux1Shared;
    }

    if(bUpdateFlags & aux2Flag)
    {
      aux2In = aux2Shared;
    }

    // clear shared copy of updated flags as we have already taken the updates
    // we still have a local copy if we need to use it in bUpdateFlags
    bUpdateFlagsShared = 0;

    interrupts(); // we have local copies of the inputs, so now we can turn interrupts back on
    // as soon as interrupts are back on, we can no longer use the shared copies, the interrupt
    // service routines own these and could update them at any time. During the update, the
    // shared copies may contain junk. Luckily we have our local copies to work with :-)
  }

  // we are checking to see if the channel value has changed, this is indicated
  // by the flags. For the simple pass through we don't really need this check,
  // but for a more complex project where a new signal requires significant processing
  // this allows us to only calculate new values when we have new inputs, rather than
  // on every cycle.

  if(bUpdateFlags & ailFlag)
  {
    //calculate wanted angle based off of rx input here, FIXME!
    //ailIn
    ailPID[0] = PIDCalc(0);
  }

  if(bUpdateFlags & thrFlag)
  {

    //thrIn
  }

  if(bUpdateFlags & eleFlag)
  {
    //eleIn
    elePID[0] = PIDCalc(1);
  }

  if(bUpdateFlags & rudFlag)
  {
    //rudIn
    rudPID[0] = PIDCalc(2);
  }

  if(bUpdateFlags & aux1Flag)
  {
    //aux1In
  }

  if(bUpdateFlags & aux2Flag)
  {
    //aux2In
  }

  bUpdateFlags = 0;

  if(aux1In) //if arm switch is on
  {
    MixMotors(); //take rx and gyro data to get values for motors.

    WriteMotor(); //send values to motors
  }
  else
  {
    StopMotors(); //send value less than minCommand to make sure motors are off  
  }
  //print values
  PrintVals();

  timer1 = millis(); //loop end time
  delay(((timeStep * 1000)-(timer1-timer))); //delay so loop lasts 20msec, (timestep(.02) * 1000 = msec) - how long loop took
}

void MixMotors()
{
motorVals[0] = thrIn + axisPID[ROLL] - axisPID[PITCH] - axisPID[YAW];
motorVals[1] = thrIn - axisPID[ROLL] - axisPID[PITCH] + axisPID[YAW];
motorVals[2] = thrIn + axisPID[ROLL] + axisPID[PITCH] + axisPID[YAW];
motorVals[3] = thrIn - axisPID[ROLL] + axisPID[PITCH] - axisPID[YAW];
}

void StopMotors()
{
motor1.WriteMicroseconds(minCommand);
motor2.WriteMicroseconds(minCommand);
motor4.WriteMicroseconds(minCommand);
motor3.WriteMicroseconds(minCommand);
}

void WriteMotors()
{
motor1.WriteMicroseconds(motorVals[0]);
motor2.WriteMicroseconds(motorVals[1]);
motor4.WriteMicroseconds(motorVals[2]);
motor3.WriteMicroseconds(motorVals[3]);
}

/*
int pPID[3] = {0,0,0}; //holds proportional term for each axis
 int iPID[3] = {0,0,0}; //-integral-
 int dPID[3] = {0,0,0}; //-derivitive-
 int ePID[3] = {0,0,0}; //-error-
 int pePID[3] = {0,0,0}; //-previous error-
 int waPID[3] = {0,0,0}; //-the wanted angle for each axis
 int caPID[3] = {0,0,0}; //-   current
 */

int PIDCalc(byte axis)
{
  ePID[axis] = waPID[axis] - caPID[axis]; //finds error between the wanted value and what it currently is.
  pPID[axis] = ePID[axis]; //"makes a change to the output that is proportional to the current error value"
  iPID[axis] += pError; //"if you aren't quite getting to target, keep increasing over a period of time until you get there"
  dPID[axis] = (ePID[axis] - pePID[axis]); //"how fast a change in angle occurs"

  pePID[axis] = ePID[axis]; //puts current error in another value for next loop to use.

  //calculates the final adjusted output based on Ki,Kp,Kd set in the beginning of
  //the program and the resulting values from Proportion, Integral, and Derivative calculations
  // Output = (Kp * Proportion) + (Ki * Integral) + (Kd * Derivative);
  return((Kp * pPID[axis]) + (Ki * iPID[axis]) + (Kd * dPID[axis]));
}

void Calculations() //calculate roll/pitch for acc/gyro, remove level bias. kalman filtering for pitch/roll, calc yaw
{
  /*
  Gyro in deg/s
   pitchGyro = (GYROx - bGYROx) / GyroGain;
   rollGyro = (GYROy - bGYROy) / GyroGain;
   */

  pitchGyro = (pitchGyro + ((GYROx - bGYROx) / GyroGain)) * timeStep; //gyro pitch in deg
  pitchAccel = (atan2((ACCy - bACCy) / AccGain, (ACCz - bACCz) / AccGain) * 180.0) / PI;
  pitchPrediction = pitchPrediction + ((GYROx - bGYROx) / GyroGain) * timeStep;

  rollGyro = (rollGyro + ((GYROy - bGYROy) / GyroGain)) * timeStep; //gyro roll in deg
  rollAccel = (atan2((ACCx - bACCx) / AccGain, (ACCz - bACCz) / AccGain) * 180.0) / PI;
  rollPrediction = rollPrediction - ((GYROy - bGYROy) / GyroGain) * timeStep;

  YawCalc();  //calc yaw with mag!

  Kalman(); //predict pitch, roll
}

void YawCalc() // calculate yaw from mag
{
  //YAW!
  //coonvert raw acc to g.
  float newACCx = ACCx - bACCx;
  float newACCy = ACCy - bACCy;

  newACCx = newACCx / pow(2, 15) * 8;
  newACCy = newACCy / pow(2, 15) * 8;

  float pitch = asin(newACCx);
  float roll = asin(newACCy/cos(pitch));

  //this part is required to normalize the magnetic vector
  if (MAGx>xMAGMax) {
    xMAGMax = MAGx;
  }
  if (MAGy>yMAGMax) {
    yMAGMax = MAGy;
  }
  if (MAGz>zMAGMax) {
    zMAGMax = MAGz;
  }

  if (MAGx<xMAGMin) {
    xMAGMin = MAGx;
  }
  if (MAGy<yMAGMin) {
    yMAGMin = MAGy;
  }
  if (MAGz<zMAGMin) {
    zMAGMin = MAGz;
  }

  //Map the incoming Data from -1 to 1
  xMAGMap = float(map(MAGx, xMAGMin, xMAGMax, -30000, 30000))/30000.0;
  yMAGMap = float(map(MAGy, yMAGMin, yMAGMax, -30000, 30000))/30000.0;
  zMAGMap = float(map(MAGz, zMAGMin, zMAGMax, -30000, 30000))/30000.0;

  //normalize the magnetic vector
  float norm = sqrt( sq(xMAGMap) + sq(yMAGMap) + sq(zMAGMap));
  xMAGMap /=norm;
  yMAGMap /=norm;
  zMAGMap /=norm;

  //new calcs:
  float xh = xMAGMap * cos(pitch) + zMAGMap * sin(pitch);
  float yh = xMAGMap * sin(roll) * sin(pitch) + yMAGMap * cos(roll) - zMAGMap * sin(roll) * cos(pitch);
  float zh = -xMAGMap * cos(roll) * sin(pitch) + yMAGMap * sin(roll) + zMAGMap * cos(roll) * cos(pitch);

  yawRaw = 180 * atan2(yh, xh)/PI;
  if (yh >= 0)
  {
    //do nothing, yaw value is ok
  }
  else
  {
    yawRaw += 360;
  }
}

void Kalman() //kalman filter for pitch / roll
{
  Pxx += timeStep * (2 * Pxv + timeStep * Pvv);
  Pxv += timeStep * Pvv;
  Pxx += timeStep * giroVar;
  Pvv += timeStep * deltaGiroVar;
  kx = Pxx * (1 / (Pxx + accelVar));
  kv = Pxv * (1 / (Pxx + accelVar));

  pitchPrediction += (pitchAccel - pitchPrediction) * kx;
  rollPrediction += (rollAccel - rollPrediction) * kx;

  Pxx *= (1 - kx);
  Pxv *= (1 - kx);
  Pvv -= kv * Pxv;
}

void PrintVals()
{
  //IMU grapher processing
  Serial.print(map(pitchPrediction, -180,180,0,500));
  Serial.print(",");
  Serial.print(map(rollPrediction, -180,180,0,500));
  Serial.print(",");
  Serial.print(yawRaw);
  Serial.println();
}

void ailCalc()
{
  if(digitalRead(ailPin) == HIGH)
  {
    ailStart = micros();
  }
  else
  {
    ailShared = (uint16_t)(micros() - ailStart);
    bUpdateFlagsShared |= ailFlag;
  }
}

// simple interrupt service routines
void thrCalc()
{
  // if the pin is high, its a rising edge of the signal pulse, so lets record its value
  if(digitalRead(thrPin) == HIGH)
  {
    thrStart = micros();
  }
  else
  {
    // else it must be a falling edge, so lets get the time and subtract the time of the rising edge
    // this gives use the time between the rising and falling edges i.e. the pulse duration.
    thrShared = (uint16_t)(micros() - thrStart);
    // use set the throttle flag to indicate that a new throttle signal has been received
    bUpdateFlagsShared |= thrFlag;
  }
}

void eleCalc()
{
  if(digitalRead(elePin) == HIGH)
  {
    eleStart = micros();
  }
  else
  {
    eleShared = (uint16_t)(micros() - eleStart);
    bUpdateFlagsShared |= eleFlag;
  }
}

void rudCalc()
{
  if(digitalRead(rudPin) == HIGH)
  {
    rudStart = micros();
  }
  else
  {
    rudShared = (uint16_t)(micros() - rudStart);
    bUpdateFlagsShared |= rudFlag;
  }
}

void aux1Calc()
{
  if(digitalRead(aux1Pin) == HIGH)
  {
    aux1Start = micros();
  }
  else
  {
    aux1Shared = (uint16_t)(micros() - aux1Start);
    bUpdateFlagsShared |= aux1Flag;
  }
}

void aux2Calc()
{
  if(digitalRead(aux2Pin) == HIGH)
  {
    aux2Start = micros();
  }
  else
  {
    aux2Shared = (uint16_t)(micros() - aux2Start);
    bUpdateFlagsShared |= aux2Flag;
  }
}

void readEEPROM()
{
  calibValues[0] = readChannel(ailMinIndex);
  calibValues[1] = readChannel(ailMidIndex);
  calibValues[2] = readChannel(ailMaxIndex);

  calibValues[3] = readChannel(thrMinIndex);
  calibValues[4] = readChannel(thrMidIndex);
  calibValues[5] = readChannel(thrMaxIndex);

  calibValues[6] = readChannel(eleMinIndex);
  calibValues[7] = readChannel(eleMidIndex);
  calibValues[8] = readChannel(eleMaxIndex);

  calibValues[9] = readChannel(rudMinIndex);
  calibValues[10] = readChannel(rudMidIndex);
  calibValues[11] = readChannel(rudMaxIndex);

  calibValues[12] = readChannel(aux1MinIndex);
  calibValues[13] = readChannel(aux1MidIndex);
  calibValues[14] = readChannel(aux1MaxIndex);

  calibValues[15] = readChannel(aux2MinIndex);
  calibValues[16] = readChannel(aux2MidIndex);
  calibValues[17] = readChannel(aux2MaxIndex);
}

void writeEEPROM()
{
  writeChannel(ailMinIndex,calibValues[0]);
  writeChannel(ailMidIndex,calibValues[1]);
  writeChannel(ailMaxIndex,calibValues[2]);

  writeChannel(thrMinIndex,calibValues[3]);
  writeChannel(thrMidIndex,calibValues[4]);
  writeChannel(thrMaxIndex,calibValues[5]);

  writeChannel(eleMinIndex,calibValues[6]);
  writeChannel(eleMidIndex,calibValues[7]);
  writeChannel(eleMaxIndex,calibValues[8]);

  writeChannel(rudMinIndex,calibValues[9]);
  writeChannel(rudMidIndex,calibValues[10]);
  writeChannel(rudMaxIndex,calibValues[11]);

  writeChannel(aux1MinIndex,calibValues[12]);
  writeChannel(aux1MidIndex,calibValues[13]);
  writeChannel(aux1MaxIndex,calibValues[14]);

  writeChannel(aux2MinIndex,calibValues[15]);
  writeChannel(aux2MidIndex,calibValues[16]);
  writeChannel(aux2MaxIndex,calibValues[17]);
}

uint16_t readChannel(uint8_t nStart)
{
  uint16_t unSetting = (EEPROM.read((nStart*sizeof(uint16_t))+1)<<8);
  unSetting += EEPROM.read(nStart*sizeof(uint16_t));

  return unSetting;
}

void writeChannel(uint8_t nIndex,uint16_t unSetting)
{
  EEPROM.write(nIndex*sizeof(uint16_t),lowByte(unSetting));
  EEPROM.write((nIndex*sizeof(uint16_t))+1,highByte(unSetting));
}

void CalibSticks()
{
  static uint16_t thrIn;
  static uint16_t ailIn;
  static uint16_t eleIn;
  static uint16_t rudIn;
  static uint16_t aux1In;
  static uint16_t aux2In;
  // local copy of update flags
  static uint8_t bUpdateFlags;

  long time = (millis() + 8000); //8 sec to move sticks in full range

  //FIXME 1sec high sound then 1 sec low sound to begin calib

  while(millis() < time)
  {
    if(bUpdateFlagsShared)
    {
      noInterrupts(); // turn interrupts off quickly while we take local copies of the shared variables

        // take a local copy of which channels were updated in case we need to use this in the rest of loop
      bUpdateFlags = bUpdateFlagsShared;

      // in the current code, the shared values are always populated
      // so we could copy them without testing the flags
      // however in the future this could change, so lets
      // only copy when the flags tell us we can.

      if(bUpdateFlags & ailFlag)
      {
        ailIn = ailShared;
      }

      if(bUpdateFlags & thrFlag)
      {
        thrIn = thrShared;
      }

      if(bUpdateFlags & eleFlag)
      {
        eleIn = eleShared;
      }

      if(bUpdateFlags & rudFlag)
      {
        rudIn = rudShared;
      }

      if(bUpdateFlags & aux1Flag)
      {
        aux1In = aux1Shared;
      }

      if(bUpdateFlags & aux2Flag)
      {
        aux2In = aux2Shared;
      }

      // clear shared copy of updated flags as we have already taken the updates
      // we still have a local copy if we need to use it in bUpdateFlags
      bUpdateFlagsShared = 0;

      interrupts(); // we have local copies of the inputs, so now we can turn interrupts back on
      // as soon as interrupts are back on, we can no longer use the shared copies, the interrupt
      // service routines own these and could update them at any time. During the update, the
      // shared copies may contain junk. Luckily we have our local copies to work with :-)
    }

    if(ailIn < calibValues[0] && ailIn > 900) //if stick val is less than min previously recorded
    {
      calibValues[0] = ailIn;
    }
    else if(ailIn > calibValues[2] && ailIn < 2500) //if stick val is greater than max previously recorded
    {
      calibValues[2] = ailIn;
    }

    if(thrIn < calibValues[3] && thrIn > 900) //if stick val is less than min previously recorded
    {
      calibValues[3] = thrIn;
    }
    else if(thrIn > calibValues[5] && thrIn < 2500) //if stick val is greater than max previously recorded
    {
      calibValues[5] = thrIn;
    }

    if(eleIn < calibValues[3] && eleIn > 900) //if stick val is less than min previously recorded
    {
      calibValues[6] = eleIn;
    }
    else if(eleIn > calibValues[5] && eleIn < 2500) //if stick val is greater than max previously recorded
    {
      calibValues[8] = eleIn;
    }

    if(rudIn < calibValues[3] && rudIn > 900) //if stick val is less than min previously recorded
    {
      calibValues[9] = rudIn;
    }
    else if(rudIn > calibValues[5] && rudIn < 2500) //if stick val is greater than max previously recorded
    {
      calibValues[11] = rudIn;
    }

    if(aux1In < calibValues[12] && aux1In > 900) //if stick val is less than min previously recorded
    {
      calibValues[12] = aux1In;
    }
    else if(aux1In > calibValues[14] && aux1In < 2500) //if stick val is greater than max previously recorded
    {
      calibValues[14] = aux1In;
    }

    if(aux2In < calibValues[15] && aux2In > 900) //if stick val is less than min previously recorded
    {
      calibValues[15] = aux2In;
    }
    else if(aux2In > calibValues[17] && aux2In < 2500) //if stick val is greater than max previously recorded
    {
      calibValues[17] = aux2In;
    }

    if(time - millis() < 1000)
    {
      //FIXME short .5sec Low then .5sec high sound
    }

    bUpdateFlags = 0;
  }

  //get center pos
  calibValues[1] = ailIn;
  calibValues[4] = thrIn;
  //calibValues[13] = aux1In;
  calibValues[16] = aux2In;
  /*
  for(int i = 0; i <12; i++)
   {
   Serial.print(calibValues[i]);
   Serial.print(",");
   }
   Serial.println();
   */
  writeEEPROM();

}

void readGyro() // get x, y, z values from gyro
{
  Wire.beginTransmission(GYR_ADDRESS);
  Wire.write(L3G4200D_OUT_X_L | (1 << 7));
  Wire.endTransmission();
  Wire.requestFrom(GYR_ADDRESS, 6);

  while (Wire.available() < 6);

  uint8_t xla = Wire.read();
  uint8_t xha = Wire.read();
  uint8_t yla = Wire.read();
  uint8_t yha = Wire.read();
  uint8_t zla = Wire.read();
  uint8_t zha = Wire.read();

  GYROy = xha << 8 | xla;
  GYROx = yha << 8 | yla;
  GYROz = zha << 8 | zla;
}

void readAcc() // get x, y, z values from accelerometer
{
  Wire.beginTransmission(ACC_ADDRESS);
  Wire.write(LSM303_OUT_X_L_A | (1 << 7));
  Wire.endTransmission();
  Wire.requestFrom(ACC_ADDRESS, 6);

  while (Wire.available() < 6);

  byte xla = Wire.read();
  byte xha = Wire.read();
  byte yla = Wire.read();
  byte yha = Wire.read();
  byte zla = Wire.read();
  byte zha = Wire.read();

  ACCy = -((xha << 8 | xla) >> 4); //reversed y axis
  ACCx = -((yha << 8 | yla) >> 4); //reversed x
  ACCz = (zha << 8 | zla) >> 4;
}

void readMag() //get mag x, y, z values
{
  Wire.beginTransmission(MAG_ADDRESS);
  Wire.write(LSM303_OUT_X_H_M);
  Wire.endTransmission();
  Wire.requestFrom(MAG_ADDRESS, 6);

  while (Wire.available() < 6);

  byte xhm = Wire.read();
  byte xlm = Wire.read();

  byte yhm, ylm, zhm, zlm;

  zhm = Wire.read();
  zlm = Wire.read();
  yhm = Wire.read();
  ylm = Wire.read();

  MAGx = (xhm << 8 | xlm);
  MAGy = (yhm << 8 | ylm);
  MAGz = (zhm << 8 | zlm);
}

void CalibrateIMU() //get level value bias of IMU sensors
{
  //temporary total holders
  int tGYROx;
  int tGYROy;
  int tGYROz;

  int tACCx;
  int tACCy;
  int tACCz;

  delay(100); //wait for stable values
  for(int i = 0; i<50; i++) //get values fifty times for acc + gyro
  {
    readGyro();
    readAcc();
    readMag();

    tGYROx += GYROx; //total gyrox value += current reading
    tGYROy += GYROy;
    tGYROz += GYROz;

    tACCx += ACCx;
    tACCy += ACCy;
    tACCz += ACCz;
    delay(4);
  }

  bGYROx = tGYROx / 50; //bias in gyro x = total gyro x/50
  bGYROy = tGYROy / 50;
  bGYROz = tGYROz / 50;

  bACCx = tACCx / 50;
  bACCy = tACCy / 50;
  bACCz = (tACCz / 50) - 256; //Don't compensate gravity away! We would all (float)!
}

//write stuff to the snsor registers
void writeGyroReg(byte reg, byte value)
{
  Wire.beginTransmission(GYR_ADDRESS);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

void writeAccReg(byte reg, byte value)
{
  Wire.beginTransmission(ACC_ADDRESS);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

void writeMagReg(byte reg, byte value)
{
  Wire.beginTransmission(MAG_ADDRESS);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

Arduino Based MWC Field Programmer

Yes, the name is horrible (haven't worked that part out quite yet), but this is my latest project. Basically, it uses an Arduino, character LCD, user controls, and some sort of serial connection (Bluetooth, hard-wired, Xbee, etc) to the MultiiWiiCopter FC so that it can change the settings as seen on the computer GUI in the field without a laptop or phone. This is important to me since i dont have a phone or laptop with bluetooth or serial port and i don't want to lug around a laptop, anyways. It will navigate via buttons or buttons and an Encoder (quicker, will act like a scroll wheel on  your mouse), plus a few short cuts to make things easier (read, write, root). It has the same "comment" and "uncomment" style that MWC code has to enable/disable settings.

First Rev

Currently, the project is still in its beginning stage, but in the first revision i plan to at least have:
  -LCDs supported:
      -Sparkfun Serial
      -Parallax Serial
      -General 4bit interface LCD (HD44780
      -16x4 or bigger (any smaller would be a pain, right?)
  -MWC Functions:
      -PIDs
      -Sensor values
      -Channel Values
      -Read/Write all of these (where applicable).
  -User Control Methods
      -7 buttons (select, back, left, right, up, down)
      -Root Addon Button - Go back to main screen any time.
      -Read Addon - Re-read values from MWC FC.
      -Write Addon - Write values to MWC FC at any time.
      -Encoder - Uses encoder to scroll through the menu faster.

BreadBoarding

Features

For all features(now + later) i am thinking of:
  -LCDs supported:
      -Sparkfun Serial
      -Parallax Serial
      -General 4bit interface LCD (HD44780
      -16x4 or bigger (any smaller would be a pain, right?)
      -More upon request + plausibility

  -MWC Functions:
      -PIDs
      -Sensor Data
         -MWC direction (heading, pitch, roll)
         -Enabled Sensors
         -Calibrate Them
         -See any debug errors

    -Channel Values
    -Rates /expo

    -Aux Channel Setup
    -Presets for all settings
    -Reset Defualts

So basically the whole computer based GUI.

Menu Layout on the LCD

The menu on the LCD will look like so:
  -PID
     -Roll
     -Pitch
     -Yaw
     -Alt
     -Etc
  -Channel Values
  -Rates + Expo
  -Sensors
     -Enabled Sensors
     -Calibrate Sensors
        -Gyro
        -Acc
        -Mag
     -Values
        -Gyro
        -Acc
        -Mag
        -Heading
        -Alt
        -GPS
        -Opto
  -Aux Setup
  -Settings Presets
     -New
     -Save
     -Load
  -Read Values
  -Write Values
  -Reset Defaults

Arduino ISR (Interrupt) Code Tutorial

I know a lot people look for something like this, so here is an example of how to debounce a button in an interrupt routine. this allows you to simply use the value in the main loop and not have to worry about the debouncing in the loop wasting time.

//----------------------------------------------------------------------------------------------

volatile long lastDebounceTime = 0;   // the last time the interrupt was triggered
#define debounceDelay 50    // the debounce time in ms; decrease if quick button presses are ignored, increase
                             //if you get noise (multipule button clicks detected by the code when you only pressed it once)

#define BtnPin 0 //digital pin 2, interrupt 0

void setup()
{
  // put your setup code here, to run once:
  attachInterrupt(BtnPin,FireEvent, RISING);
  //this works on a switch that is pulled down to ground and sends V+ to the arduino pin when "closed" (pushed down)
  //See here for the scheme: http://arduino.cc/en/uploads/Tutorial/button_schem.png the resistor is 10k.
}

void loop()
{
  // put your main code here, to run repeatedly:
 
}

void FireEvent() //triggers when the switch is pressed. each time you press the switch,
{                //there are hundreds of little tiny conenctions as you press it down, so the delay filters those out.

  if ((millis() - lastDebounceTime) > debounceDelay) //if current time minus the last trigger time is greater than
  {                                                  //the delay (debounce) time, button is completley closed.
    lastDebounceTime = millis();
   
   //switch was pressed, do stuff like turn off your motor/led etc
  }
}
//------------------------------------------------------------------------------------------------------------

Bitcoin Mining Tutorial!

Believe it or not, this is true. You can make money simply by using your GPU (graphics card) in your computer calculate hashes. Most times people work together in "pools" so that they can find "blocks" faster (Its like the hashes is the hay, and the block is the needle in a haystack). These blocks are what gives you're main money; when the pool finds these, it gives each user some ratio of money. This money is sent as bitcoins which is basically electronic money that can be used on many websites and converted in to hard cash. A program called Bitcoin generates "keys" that are given to the pool server so that it knows where to send the bitcoins. This programs stores the money in a "wallet" on your computer, so if someone breaks into your computer, they could steal them, or if you forget the wallet password (if you encrypt it).  I'm not sure exactly, but i think each of these hashes is encrypting the bitcoins to keep it safe in some way.

More after the break!