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();
}
No comments:
Post a Comment