ITG3200::ITG3200() {
spn("Init ITG-3200 starting...");
readI2C(GYRADDR, 0x00, 1, buffer); // Who am I?
sp("ITG-3200 ID = ");
spln((int) buffer[0]);
readI2C(GYRADDR, 0x15, 2, buffer);
// Sample rate divider is 1 + 1 = 2, so 1000 Hz / 2 = 500 Hz
buffer[0] = 1;
// Set FS_SEL = 3 as recommended on DS p. 24.
// Set DLPF_CFG = 3. This signifies a 1 kHz internal sample rate with a 42
// Hz LPF bandwidth, which should be low enough to filter out the motor
// vibrations.
buffer[1] = (3 << 3) | 3; // FS_SEL is on bits 4 and 3.
writeI2C(GYRADDR, 0x15, 2, buffer);
// Set to use X gyro as clock reference as recommended on DS p. 27.
buffer[0] = 1;
writeI2C(GYRADDR, 0x3e, 1, buffer);
spln("ITG-3200 configured!");
// Zero buffer.
for (int i=0; i<6; i++) {
buffer[i] = 0;
}
// Low-pass filter.
#ifdef GYRO_LPF_DEPTH
lpfIndex = 0;
for (int i=0; i<3; i++)
for (int j=0; j<GYRO_LPF_DEPTH; j++)
lpfVal[i][j] = 0;
#endif // GYRO_LPF_DEPTH
for (int i=0; i<3; i++) {
gZero[i] = 0;
gVec[i] = 0;
angle[i] = 0;
}
calibrated = false;
}
void IMU::init() {
IMU::reset();
spln("IMU here!");
gyro.calibrate(500);
// Set initial DCM as the identity matrix.
for (int i=0; i<3; i++)
for (int j=0; j<3; j++)
gyroDCM[i][j] = (i==j) ? 1.0 : 0.0;
/*! Calculate DCM offset.
*
* We use an accelerometer to correct for gyro drift. This means that the
* IMU will adjust gyroDCM according to whatever the accelerometer thinks
* is gravity. However, since it is nearly impossible to mount the
* accelerometer perfectly horizontal to the chassis, this also means that
* gyroDCM will represent the orientation of the accelerometer, not the
* chassis, along the X and Y rotational axes.
*
* The rotational difference between the orientations of the accelerometer
* (gyroDCM) and the chassis (bodyDCM) is constant and can be approximated
* by another rotation matrix we will call trimDCM, which we populate with
* trim angles obtained during hover calibration. We rotate gyroDCM by
* trimDCM to obtain bodyDCM.
*
* Keep in mind, however, that we still update the IMU based on gyroDCM
* and not bodyDCM. This is because it is gyroDCM we are correcting with
* the accelerometer's measurement of the gravity vector, and bodyDCM is
* only a transformation of that DCM based on trimDCM. We use bodyDCM
* for flight calculations, but gyroDCM is what we keep track of within
* the IMU.
*
* As a final note, this is a small-angle approximation! We could do
* something fancy with trigonometry, but if the hardware is so poorly
* built (or poorly designed) that small-angle approximations become
* insufficient, we can't expect software to fix everything.
*/
#ifdef ACC_WEIGHT
trimAngle[0] = TRIM_ANGLE_X;
trimAngle[1] = TRIM_ANGLE_Y;
aVec[0] = 0;
aVec[1] = 0;
aVec[2] = -1;
for (int i=0; i<3; i++) {
aVecLast[i] = aVec[i];
}
//accVar = 100.;
#endif // ACC_WEIGHT
}
void ITG3200::calibrate(int sampleNum) {
int32_t tmp[3];
for (int i=0; i<3; i++) {
tmp[i] = 0;
}
for (int i=0; i<sampleNum; i++) {
ITG3200::poll();
for (int j=0; j<3; j++) {
tmp[j] = tmp[j] + gRaw[j];
}
delayMicroseconds(20);
}
for (int i=0; i<3; i++) {
gZero[i] = tmp[i]/sampleNum;
}
spln("Gyro calibration complete!");
calibrated = true;
}
ChebyshevFilter::ChebyshevFilter(int type,int order,double ripple,double samRate,double cutF1,double cutF2)
{
dbfac = 10.0/log(10.0);
kind=2;
this->type=type;
rn=order;
n = rn;
rn = n; /* ensure it is an integer */
if( kind > 1 ) /* not Butterworth */
{
dbr=ripple;
if( kind == 2 )
{
/* For Chebyshev filter, ripples go from 1.0 to 1/sqrt(1+eps^2) */
phi = exp( 0.5*dbr/dbfac );
if( (n & 1) == 0 )
scale = phi;
else
scale = 1.0;
}
}
fs=samRate;
fnyq = 0.5 * fs;
f2=cutF1;
if( (type & 1) == 0 )
{
f1=cutF2;
if( (f1 <= 0.0) || (f1 >= fnyq) )
f1=0;
}
else
{
f1 = 0.0;
}
if( f2 < f1 )
{
a = f2;
f2 = f1;
f1 = a;
}
if( type == 3 ) /* high pass */
{
bw = f2;
a = fnyq;
}
else
{
bw = f2 - f1;
a = f2;
}
/* Frequency correspondence for bilinear transformation
*
* Wanalog = tan( 2 pi Fdigital T / 2 )
*
* where T = 1/fs
*/
ang = bw * PI / fs;
cang = cos( ang );
c = sin(ang) / cang; /* Wanalog */
if( kind != 3 )
{
wc = c;
}
/* Transformation from low-pass to band-pass critical frequencies
*
* Center frequency
* cos( 1/2 (Whigh+Wlow) T )
* cos( Wcenter T ) = ----------------------
* cos( 1/2 (Whigh-Wlow) T )
*
*
* Band edges
* cos( Wcenter T) - cos( Wdigital T )
* Wanalog = -----------------------------------
* sin( Wdigital T )
*/
if( kind == 2 )
{ /* Chebyshev */
a = PI * (a+f1) / fs ;
cgam = cos(a) / cang;
a = 2.0 * PI * f2 / fs;
cbp = (cgam - cos(a))/sin(a);
}
if( kind == 1 )
{ /* Butterworth */
a = PI * (a+f1) / fs ;
cgam = cos(a) / cang;
a = 2.0 * PI * f2 / fs;
cbp = (cgam - cos(a))/sin(a);
scale = 1.0;
}
spln(); /* find s plane poles and zeros */
zplna(); /* convert s plane to z plane */
zplnb();
zplnc();
}
void IMU::init() {
spln("IMU here!");
update();
}
BMA180::BMA180() {
readI2C(ACCADDR, 0x00, 1, buffer);
sp("BMA180 ID = ");
spln((int) buffer[0]);
// Set ee_w bit
readI2C(ACCADDR, CTRLREG0, 1, buffer);
buffer[0] |= 0x10; // Bitwise OR operator to set ee_w bit.
writeI2C(ACCADDR, CTRLREG0, 1, buffer); // Have to set ee_w to write any other registers.
// Set range.
readI2C(ACCADDR, OLSB1, 1, buffer);
buffer[0] &= (~0x0e); // Clear old ACC_RANGE bits.
buffer[0] |= (ACC_RANGE << 1); // Need to shift left one bit; refer to DS p. 21.
writeI2C(ACCADDR, OLSB1, 1, buffer); // Write new ACC_RANGE data, keep other bits the same.
// Set ADC resolution (DS p. 8).
res = 8000; // == 1/0.000125 // [ -1, 1] g
if (ACC_RANGE == 1) res /= 1.5; // [ -1.5, 1.5] g
else if (ACC_RANGE == 2) res /= 2; // [ -2, 2] g
else if (ACC_RANGE == 3) res /= 3; // [ -3, 3] g
else if (ACC_RANGE == 4) res /= 4; // [ -4, 4] g
else if (ACC_RANGE == 5) res /= 8; // [ -8, 8] g
else if (ACC_RANGE == 6) res /= 16; // [ -16, 16] g
// Set bandwidth.
// ACC_BW bandwidth (Hz)
// 0 10
// 1 20
// 2 40
// 3 75
// 4 150
// 5 300
// 6 600
// 7 1200
readI2C(ACCADDR, BWTCS, 1, buffer);
buffer[0] &= (~0xf0); // Clear bandwidth bits <7:4>.
buffer[0] |= (ACC_BW << 4); // Need to shift left four bits; refer to DS p. 21.
writeI2C(ACCADDR, BWTCS, 1, buffer); // Keep tcs<3:0> in BWTCS, but write new ACC_BW.
// Set mode_config to 0x01 (ultra low noise mode, DS p. 28).
//readI2C(ACCADDR, 0x30, 1, buffer);
//buffer[0] &= (~0x03); // Clear mode_config bits <1:0>.
//buffer[0] |= 0x01;
//writeI2C(ACCADDR, 0x30, 1, buffer);
spln("BMA180 configured!");
for (int i=0; i<3; i++) {
aRaw[i] = 0;
aVec[i] = 0;
}
// Zero buffer.
for (int i=0; i<6; i++) {
buffer[i] = 0;
}
// Low-pass filter.
#ifdef ACC_LPF_DEPTH
lpfIndex = 0;
for (int i=0; i<3; i++)
for (int j=0; j<ACC_LPF_DEPTH; j++)
lpfVal[i][j] = 0;
#endif // ACC_LPF_DEPTH
}
