/*
AeroQuad v2.1 - October 2010
www.AeroQuad.com
Copyright (c) 2010 Ted Carancho. All rights reserved.
An Open Source Arduino based multicopter.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see .
*/
class Accel {
public:
float accelScaleFactor;
float smoothFactor;
float rawAltitude;
int accelChannel[3];
int accelZero[3];
int accelData[3];
int accelADC[3];
int sign[3];
int accelOneG, zAxis;
byte rollChannel, pitchChannel, zAxisChannel;
unsigned long currentTime, previousTime;
Accel(void) {
sign[ROLL] = 1;
sign[PITCH] = 1;
sign[YAW] = 1;
zAxis = 0;
}
// ******************************************************************
// The following function calls must be defined in any new subclasses
// ******************************************************************
virtual void initialize(void) {
this->_initialize(rollChannel, pitchChannel, zAxisChannel);
smoothFactor = readFloat(ACCSMOOTH_ADR);
}
virtual void measure(void);
virtual void calibrate(void);
virtual const int getFlightData(byte);
// **************************************************************
// The following functions are common between all Gyro subclasses
// **************************************************************
void _initialize(byte rollChannel, byte pitchChannel, byte zAxisChannel) {
accelChannel[ROLL] = rollChannel;
accelChannel[PITCH] = pitchChannel;
accelChannel[ZAXIS] = zAxisChannel;
accelZero[ROLL] = readFloat(LEVELROLLCAL_ADR);
accelZero[PITCH] = readFloat(LEVELPITCHCAL_ADR);
accelZero[ZAXIS] = readFloat(LEVELZCAL_ADR);
accelOneG = readFloat(ACCEL1G_ADR);
}
const int getRaw(byte axis) {
return accelADC[axis] * sign[axis];
}
const int getData(byte axis) {
return accelData[axis] * sign[axis];
}
const int invert(byte axis) {
sign[axis] = -sign[axis];
return sign[axis];
}
const int getZero(byte axis) {
return accelZero[axis];
}
void setZero(byte axis, int value) {
accelZero[axis] = value;
}
const float getScaleFactor(void) {
return accelScaleFactor;
}
const float getSmoothFactor() {
return smoothFactor;
}
void setSmoothFactor(float value) {
smoothFactor = value;
}
const float angleRad(byte axis) {
if (axis == PITCH) return arctan2(accelData[PITCH] * sign[PITCH], sqrt((long(accelData[ROLL]) * accelData[ROLL]) + (long(accelData[ZAXIS]) * accelData[ZAXIS])));
if (axis == ROLL) return arctan2(accelData[ROLL] * sign[ROLL], sqrt((long(accelData[PITCH]) * accelData[PITCH]) + (long(accelData[ZAXIS]) * accelData[ZAXIS])));
}
const float angleDeg(byte axis) {
return degrees(angleRad(axis));
}
void setOneG(int value) {
accelOneG = value;
}
const int getOneG(void) {
return accelOneG;
}
const int getZaxis() {
zAxis = smooth(getFlightData(ZAXIS) - accelOneG, zAxis, 0.25);
return zAxis;
}
void calculateAltitude() {
currentTime = millis();
if ((abs(getData(ROLL)) < 1800) || (abs(getData(PITCH)) < 1800))
rawAltitude += (getData(ZAXIS) - accelOneG) * accelScaleFactor * ((currentTime - previousTime) / 1000.0);
previousTime = currentTime;
}
const float getAltitude(void) {
return rawAltitude;
}
};
/******************************************************/
/************ AeroQuad v1 Accelerometer ***************/
/******************************************************/
class Accel_AeroQuad_v1 : public Accel {
private:
int findZero[FINDZERO];
public:
Accel_AeroQuad_v1() : Accel(){
// Accelerometer Values
// Update these variables if using a different accel
// Output is ratiometric for ADXL 335
// Note: Vs is not AREF voltage
// If Vs = 3.6V, then output sensitivity is 360mV/g
// If Vs = 2V, then it's 195 mV/g
// Then if Vs = 3.3V, then it's 329.062 mV/g
accelScaleFactor = 0.000329062;
}
void initialize(void) {
// rollChannel = 1
// pitchChannel = 0
// zAxisChannel = 2
this->_initialize(1, 0, 2);
smoothFactor = readFloat(ACCSMOOTH_ADR);
}
void measure(void) {
for (axis = ROLL; axis < LASTAXIS; axis++) {
accelADC[axis] = analogRead(accelChannel[axis]) - accelZero[axis];
accelData[axis] = smooth(accelADC[axis], accelData[axis], smoothFactor);
}
}
const int getFlightData(byte axis) {
return getRaw(axis);
}
// Allows user to zero accelerometers on command
void calibrate(void) {
for (byte calAxis = ROLL; calAxis < LASTAXIS; calAxis++) {
for (int i=0; i> 2; // last 2 bits are not part of measurement
accelADC[select] = rawData[select] - accelZero[select]; // center accel data around zero
accelData[select] = smooth(accelADC[select], accelData[select], smoothFactor);
if (select == ZAXIS) calculateAltitude();
if (++select == LASTAXIS) select = ROLL; // go to next axis, reset to ROLL if past ZAXIS
}
const int getFlightData(byte axis) {
return getRaw(axis) >> 4;
}
// Allows user to zero accelerometers on command
void calibrate(void) {
int dataAddress;
for (byte calAxis = ROLL; calAxis < ZAXIS; calAxis++) {
if (calAxis == ROLL) dataAddress = 0x04;
if (calAxis == PITCH) dataAddress = 0x02;
if (calAxis == ZAXIS) dataAddress = 0x06;
for (int i=0; i> 2; // last two bits are not part of measurement
delay(1);
}
accelZero[calAxis] = findMode(findZero, FINDZERO);
}
// replace with estimated Z axis 0g value
accelZero[ZAXIS] = (accelZero[ROLL] + accelZero[PITCH]) / 2;
// store accel value that represents 1g
measure();
accelOneG = getRaw(ZAXIS);
writeFloat(accelOneG, ACCEL1G_ADR);
writeFloat(accelZero[ROLL], LEVELROLLCAL_ADR);
writeFloat(accelZero[PITCH], LEVELPITCHCAL_ADR);
writeFloat(accelZero[ZAXIS], LEVELZCAL_ADR);
}
};
/******************************************************/
/*********** ArduCopter ADC Accelerometer *************/
/******************************************************/
#ifdef ArduCopter
class Accel_ArduCopter : public Accel {
private:
int findZero[FINDZERO];
int rawADC;
public:
Accel_ArduCopter() : Accel(){
// ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step
// ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412
// Tested value : 414
// #define GRAVITY 414 //this equivalent to 1G in the raw data coming from the accelerometer
// #define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
accelScaleFactor = 414.0 / 9.81;
}
void initialize(void) {
// rollChannel = 5
// pitchChannel = 4
// zAxisChannel = 6
this->_initialize(5, 4, 6);
}
void measure(void) {
for (axis = ROLL; axis < LASTAXIS; axis++) {
rawADC = analogRead_ArduCopter_ADC(accelChannel[axis]);
if (rawADC > 500) // Check if measurement good
accelADC[axis] = rawADC - accelZero[axis];
accelData[axis] = accelADC[axis]; // no smoothing needed
}
}
const int getFlightData(byte axis) {
return getRaw(axis);
}
// Allows user to zero accelerometers on command
void calibrate(void) {
for(byte calAxis = 0; calAxis < LASTAXIS; calAxis++) {
for (int i=0; i_initialize(6, 7, 5);
smoothFactor = readFloat(ACCSMOOTH_ADR);
}
void measure(void) {
for (axis = ROLL; axis < LASTAXIS; axis++) {
accelADC[axis] = analogRead(accelChannel[axis]) - accelZero[axis];
accelData[axis] = smooth(accelADC[axis], accelData[axis], smoothFactor);
}
}
const int getFlightData(byte axis) {
return getRaw(axis);
}
// Allows user to zero accelerometers on command
void calibrate(void) {
for (byte calAxis = ROLL; calAxis < LASTAXIS; calAxis++) {
for (int i=0; i