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 }