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!

Blu-Baby 42" RC Plane Build Log

This post will go through the parts of the plane, how they work, and my RC build.

Note: this post is not yet complete.

Basics

What is a plane? Wikipedia says: "A fixed wing aircraft is an aircraft capable of flight using wings that generate lift due to the vehicle's forward airspeed and the shape of the wings. A powered fixed-wing aircraft that is propelled forward by thrust from a jet engine or propeller is typically called an aeroplane, airplane, or simply a plane." Here is a basic picture:

Fuselage - This is the base of the entire aircraft as it holds everything from the motor to the control surfaces.  typically made out of foam or wood

Propeller - This is turned by the motor and because of its shape, it generates thrust as it spins. It could be described as "scooping" the air and pushing it back. to provide horizontal thrust for the plane, pulling it forward.

Motor - This can either be electric(brushed or brushless) or a gas/nitro/whatever motor. This sits right behind the propeller and most likely directly(no gears) drives the propeller. I use a brushless motor (more efficient, less wear), but gas is still pretty popular.

ESC - This pertains to electric motors only (brushed/brushless). This is connected to the battery and motor so that a signal (later) can change the speed of the motor and therefor the amount of speed/thrust of the plane.

Batery - This provides electricity for the servos, RX, motor and other stuff. Most commonly a LiPo because  of its high power to weight ratio.

RX(aka the receiver) - This receives signals from the TX(transmitter) and changes the motor speed, servo positions, and other stuff so that the plane can be controlled by the user.

TX - It has sticks/switches and other controls so the the user can control the plane.

Landing Gear - These are simply wheels connected to the bottom of the fuselage and allow for the plaen to safely land and take off with out had tossing, grass etc.

Wing - This generates lift because the path on top of the wing is longer than the path of air below the wing.
      -Ailerons - This surface rolls the plane.

Horizontal Stabilizer - This stabilizes the plane in the horizontal axis.
      -Elevator - This surface moves the plane up and down.

Vertical Stabilizer - This stabilizes the plane in the vertical axis.
      -Rudder - This surface moves the plane left and right

Theory

Basic needed parts:

Motor

ESC

Servos

Linkages

control horns

building material

plans (im not to the "think of a plane design off of the top of my head" stage).

LiPo

LiPo charger

TX

RX

Prop

Misc: wire, plugs, zipties, nuts/bolts, sodlering iron, general tools, and hot glue.]

The main piece is the wing; a device for generating lift. Below you can see a typical KFm3 type wing in both a top and side view. Since the path for air to follow is shorter on the bottom than the top, it generates lift. in this wing it is done with the FKm3 50% and 75% strips. The 75% strip is a 2in strip of FFF that sits at the 75% of the wing. Then, the longer 50% strip goes from the leading edge of the wing to folded down onto the 7% strip thus creating the airfoil. This wing, since it isn't two pieces(left and right wing), it uses a straight KFm3 strip from tip to tip unlike a regular BB wing which the styrip goes from 50/75% on the tip to the 50/75% on the root chord. But, it still works great!

But if you throw a wing on a long chunk of rectangular foam(fuselage), it wont fly. It wouldn't be stable horizontally or vertically, and that is why the vstab and hstab are there: they make the plane stable and sort of "counteract" the lift wing and prevent it from going crazy.

That would give you a glider, but it wouldn't be controllable and so you'd throw it and then it would have to be retrieved. This is where RC comes in. Adding surfaces to the hstap, vstap, and wing with the addition of a motor gives a controllable aircraft.

The Build

After building my quad and beginning to fly it, I've started to explore other RC areas such as panes which is where this post comes into play. I've done a decent amount of simulator time, but I didn't want go get going to fast into something that i wasn't ready for so this is a build log of a Blu-Baby 42" trainer plane with a more aerobatic wing. This is actually my second build (the first was a standard a 33" Blu-Baby but it was too heavy). Here is the main topic at RCGroups: ** Blu-Baby Primary Trainer ** Plans, Pics and Fun! You should also look at the ** Blu Baby 33" KFm3P Polyhedral Tip Wing Build ** and BB 33 With Full Length Monoblock. Those are for a 33" BB, but i simply took the ideas and scaled them up for a BB 42"

You'll need:
-At least 2 sheets (4x3") FFF 1/4
-2in pink foam
-Knife (those snap off ones work nice)
-Quick PU gorilla glue
-Packaging tape
-1/8in plywood
-Wheels
-Servos (9g or bigger)
-Brushless Motor
-11in prop (or so)
-ESC
-RX
-UBEC (possibly if ESC BEC cant give enough current for servos)


The first thing that i needed was 1/4in FFF (Fan Fold Foam) as this is the basis for the entire plane. You can either get it in huge fan folded pieces at Menards/Home Depot etc for 40$ (outrageous) or you can just get a few sheets from someone (like a contractor for a few bucks). The only thing that isnt foam are things such as the electronics, motor mount, landing gear, surface hinges etc. Most FFF is pink and has a thin plastic film on each side, but mine was a bit different: it had a plastic film on one side and a plastic film with a metal film on the other. Metal is great at blocking RF so the metal obviously has to come off. I left the other side of the FFF on(only plsatic film) because that adds a nice smoothh finish and a more sturdy foam with little weight. below you can see the orioginal foam on both sides and then it being pulled off.

Metal film side.

Plastic film side (some was peeled off).

Bare foam under metal film.

The easiest way to pull it off is to pick at a bit on the corner (very easy), and then pull it off at a 90deg angle (this way it doesn't rip like the 4th picture).

All sheets with metal film peeled off and stacked up.

Next, I downlaoded and printed off the BB42 tiled plans from here, and figured out the dimensions of the wing. I did this because: tracing isnt very accurate, im making a one piece wing, and third i wanted to learn more abount root/tip chords etc stuff about the wing. Below is a simple diagram that gives all dimensions to make the base of the wing.

Marked the foam with a pen, tape measure, and a big "T square" to go 90deg off the sides and draw the long straight lines.

Done cutting, started to mark the lines for the kmF3 pieces. I was still trying to figure out how to do the KFm3 stuff (50 and 75% strips) so that's why there are a quite a few lines. All you need is a line at the 75% and 50% of the tip chord that goes to the opposite tip. Note that from now on, the wing must have its skin (plastic film) side down so that when it is done, the rougher, but glueable foam is inside.

Then I cut a 43in wide by 4in deep piece of FFF for the 50% strip and a 43in by 3in piece for the 75% strip.

I put a 45deg chamfer on the leading edge of the wing and on the long edge of the 50% strip. 

Pretty Straight!

Then i put them together to make sure they make a nice 90deg angle.

I glued on the 75% strip so that the trailing edge of the 75% piece was at the 75% mark of the wing tip with gorilla quick PU glue.

This is where it gets more complicated. As i was doing a more aerobatic wing with polyhedral, the dowel( or CF rod etc) that supports the leading edge of the wing couldn't go the whole way or else i couldn't change the wing later for the tips of the wing that angle up (polyhedral). Since I needed 7in polyhedral tips on each side of the wing, i made two 7in long dowels and one 29in dowel (centered) which is 43 all the way across and i can still make the changes for the poly later.

I put a ~43in piece of packaging tape on the work area and then set the leading edge of the wing on about 1/3 of it. That gave me enough room to put the dowels in there. Then i set the 50% strip on the rest of the tape after the dowels, made sure it was firm. In this pic i have flipped it over and was smoothing out the tape.

and then folded it over and glued it to the 75% strip with gorilla quick PU glue.

TeDa! a 43" KFm3 wing.

I measured 7in from the tip of the wing, made a line all the way from the leading to trailing edge with a T square, ant then marked 1/8in from the 7in line on both sides. Then i took a fine toothed hand saw and made so that the two lines 1.4in apart intersected at the top of the bottom layer of foam. ( make a tiny little v in the bottom sheet of foam  from the 75 to 100% chord, and then put one piece of 20in+ duck tape on the back of the wing so that most of its sticks out.

Measure from the tip of the bottom of the wing to the surface the wing is sitting on. for a 42: with 7in polys, it should be 2 1/4in (actually 2 1/8in but we go over so that when it settles the poly will not be too low). Scrape off the plastic film where the duct tape will go so that it has a good bond to the foam.

Get the inside of the v wet and squeeze some PU in there(don't be skimpy) and also a bit on the outside of the v. The wrap the duck tape back around and over the top of the wing. As it dries, the PU will bubble up a bit, so, check it every few minutes and push on the tape to get the glue back down in there. I used the same roll of duct tape to hold the wing at 2 1/4 in.

Do the same for the other side and your'e all done with a KFm3 polyhedral wing!

Printed out the sheets for the elevator from the PDF and cut them from FFF. The chamfered them to 45deg like the wing on the non-skin side.

This is how to do packaging tape hinges! Put the elevator surface on top of the hstab so that it forms a 30deg(?) angle. Put a few sheets of paper between them for space.

Then, put some tape from the surface to the hstab so that it creates a hinge (around the 30deg angle). Make sure that the two pieces are flat against each other. 

Then, lay the hstab flat and put some tape across there too. you should now have a fairly easy to move control surface.

Obviously, this is the fuselage. The big thing is that i went with a full monoblock design which is hevier but way stronger thatn using a standard monoblock and FFF for the rest of the fuselage. I simply used the 3 pages spliced together from the BB43in tiled pdf of the side panels for the fuelage to cut the shape using a band saw. I stacked a three rectangles of foam on top of each other(one 2in, and two FFF pecies) that could all fit the side panel on them and cut them all at once.

 Made a little motor mount cutout at the front. 

Used 1 in wood bit to cut out battery, servo and RX compartments.

This is VERY VERY important for any plane newb to understand! Guess how i know! You can see the washers on the x mount of the motor, right? Those washers point the motor down and to the right 2 deg each from the cockpit view. that is so that the motor counteracts its own spinning torque. Note: I have the wrong prop on in this picture (a CCW); just haven't changed it yet.

Back view. Simple 1/8in ply with holes in it.

Covered up x mount bolts to protect them from glue. Shows how dowels go into motor mount and fuselage.

Glued all the dowels into the motor mount plywood and then glued them into the foam.

hstab and vstab done.

Hot glued onto the fuselage.

I PU glued the FFF fuselage side panels onto the 2in full fuselage (i should have done this before the hstab and vstab were on! hah). I put in the 18A SS ESC, 20-22L w/10x4.5 prop, RX and battery in there to test it.

You can also see here that i put in the dowels for mounting the wing. i put them ~<1in back and down from the leading edge (and opposite for the trailing).

Got tired of waiting so i bought a control rod kit from my LHS (total ripoff). Still using my HK 9g servos.

And also a ruder/elevator control rod kit.

I mounted the rudder/elevator servos ~1in above the hstab and in the servo "pocket" towards the back of the 1-piece-fuse so that the servo arm can never go lower than the hstab (would get caught if it did).

The servo pocket. Didn't really know how i was going to mount the servos when i started, so this may be filled or something.

 Control Surface Install how-to:

1. Put a control horn on the surface so that the hole in which the control rod goes into is parallel to the center of the hinge, and it is near the center of the surface (be careful for two surfaces colliding).

2. Attach the Micro E/Z Link (aka clevis) on to the end of the Control Rod.

3. The Control Rod should be at least the distance to the servo so that you can make adjustments and have a plastic sleeve on it to route the wire to the surface from the servo.

4. Connect the servos to the RX and turn on your TX. Make sure that all channels are centered with no trim etc.

5. This should be done while the TX sticks are centered so that the servo is centered. Move the servo arm on the servo (dont just turn the servo, move the arm from the gears) so that it is either pointed straight up or straight down (or close as possible). Tighten down the arm with the little screw to the servo after powering down so you don't strip the servo.

6. Hold the surface parallel to its stabilizer (ex rudder to vstab) and mark the wire about 1in past the servo. Cut it and also cut the sleeve ~3in shorter than this.

7. Install the E/Z connector onto the servo arm. place the little aluminum piece on the outside of the arm and then push the rubber grip onto the aluminum piece on the inside of the arm. 

8. Make sure the servo is centered, hold the surface parallel to the stabilizer slide the wire into the E/Z Connector and tighten it down.

9. Use hot glue or something to hold down the sleeve to something sturdy.

10. Test! should be pretty close to perfect. may have to change trim some.

The elevator surface.

I put the battery in there (not actual flight position) and tested the ruder and elevator surfaces. 

Total crap landing gear made from .078in music wire, pink foam, duct tape and pen tube for a bearing. I need to bridge the gap between the wire and the tube somehow and it would be better. This thing definitely has enough power for takeoff so ill see how it does. If it works well then ill leave it (i dont care about looks on this build).

This is the finished wing except for the control horns. This can't be seen in any of the images really, but there is a dowel embedded at the center, trailing edge of the wing so that the rubber bands don't gouge the wing.

To make the custom ailerons:

1. I found the shortest 75->100% area on the wing and then made the aileron less than that (~1.5in). 

2. In terms of length, i made the ailerons go 2in from the polyhedral and 2.5in from the center of the wing (so i have room for the rubber bands). 

3. Then I used a tape measure to mark all of these points and then used a T-square to connect the dots.

4. I cut them out with an exacto and cut the 45 deg angles on the edge like i did for the rudder and elevator.

5. I used the same tape method to create a hinge.  

I used bigger servos here because i didnt have any other ones and because these surfaces are bigger than the 32in BB. I thought that keeping them toward the center of the wing would be the best, so they are 4in from the center and 4in from the aileron surface hinge. Also, i made sure that they are inside the 50-75% step of foam so that they wont stick out of the top of the wing as they are ~.375in thick. To do the actual mount i just traced it onto the foam, cut it out, put some hot glue at the bottom, and put a piece of packaging tape over the top.

 I put the servos to center, made sure the ailerons were parallel with the wing, and then tightened down the screw on the servo arm.

Crappy, DIY ailerons horns from CD case. seem to work pretty well.

Full shot.


I have been doing test flights, but the first few haven't worked too well:
http://www.youtube.com/watch?v=ddBOsy0hlJ0
http://www.youtube.com/watch?v=XG63NVOVU7c

i have been talking with the people on RCG, so i hope i can get it  sorted out soon!!