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.
. 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...
//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();
}
.jpg)
.jpg)
