gyro.c

00001 // ===============================================================================================
00002 // =                              UAVXArm Quadrocopter Controller                                =
00003 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
00004 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
00005 // =                           http://code.google.com/p/uavp-mods/                               =
00006 // ===============================================================================================
00007 
00008 //    This is part of UAVXArm.
00009 
00010 //    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
00011 //    General Public License as published by the Free Software Foundation, either version 3 of the
00012 //    License, or (at your option) any later version.
00013 
00014 //    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
00015 //    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016 //    See the GNU General Public License for more details.
00017 
00018 //    You should have received a copy of the GNU General Public License along with this program.
00019 //    If not, see http://www.gnu.org/licenses/
00020 
00021 #include "UAVXArm.h"
00022 const real32 GyroToRadian[UnknownGyro] = {
00023     8.635062,       // MLX90609
00024     4.607669,       // ADXRS150
00025     28.797933,      // IDG300
00026     17.453293,      // LY530
00027     11.519173,      // ADXRS300
00028     0.000438704 * 2.0 * 1.31,    // ITG3200 16bit 2's complement
00029     1.0             // Infrared Sensors
00030     // add others as required
00031 };
00032 
00033 void ReadGyros(void);
00034 void GetGyroRates(void);
00035 void CheckGyroFault(uint8, uint8, uint8);
00036 void ErectGyros(void);
00037 void InitGyros(void);
00038 void GyroTest(void);
00039 void ShowGyroType(void);
00040 
00041 real32 GyroADC[3], GyroNoise[3], GyroNeutral[3], Gyro[3], Gyrop[3]; // Radians
00042 uint8 GyroType;
00043 
00044 void GetGyroRates(void) {
00045     static uint8 g;
00046     static real32 d, GyroA;
00047 
00048     ReadGyros();
00049 
00050     for ( g = 0; g < (uint8)3; g++ ) {
00051         d = fabs(Gyro[g]-Gyrop[g]);
00052         if ( d > GyroNoise[g] ) GyroNoise[g] = d;
00053     }
00054 
00055 #ifndef SUPPRESS_ROLL_PITCH_GYRO_FILTERS
00056     // dT is almost unchanged so this could be optimised
00057     GyroA = dT / ( 1.0 / ( TWOPI * ROLL_PITCH_FREQ ) + dT );
00058     Gyro[Roll] = LPFilter( Gyro[Roll] - GyroNeutral[Roll], Gyrop[Roll], GyroA );
00059     Gyro[Pitch] = LPFilter( Gyro[Pitch] - GyroNeutral[Pitch], Gyrop[Pitch], GyroA );
00060 #endif // !SUPPRESS_ROLL_PITCH_GYRO_FILTERS 
00061 
00062 #ifndef SUPPRESS_YAW_GYRO_FILTERS
00063 
00064 #ifdef USE_FIXED_YAW_FILTER
00065     GyroA = dT / ( 1.0 / ( TWOPI * MAX_YAW_FREQ ) + dT );
00066 #else
00067     GyroA = dT / ( 1.0 / ( TWOPI * YawFilterLPFreq ) + dT );
00068 #endif // USE_FIXED_YAW_FILTER
00069 
00070     Gyro[Yaw] = LPFilter( Gyro[Yaw] - GyroNeutral[Yaw], Gyrop[Yaw], GyroA );
00071 #endif // !SUPPRESS_GYRO_FILTERS
00072 
00073     for ( g = 0; g < (uint8)3; g++ )
00074         Gyrop[g] = Gyro[g];
00075 
00076 } // GetGyroRates
00077 
00078 void ReadGyros(void) {
00079     switch ( GyroType ) {
00080         case ITG3200Gyro:
00081             ReadITG3200Gyro();
00082             break;
00083         case IRSensors:
00084             GetIRAttitude();
00085             break;
00086         default :
00087             ReadAnalogGyros();
00088             break;
00089     } // switch
00090 } // ReadGyros
00091 
00092 void ErectGyros(void) {
00093     static uint8 s, i, g;
00094 
00095     LEDRed_ON;
00096 
00097     for ( g = 0; g <(uint8)3; g++ )
00098         GyroNeutral[g] = 0.0;
00099 
00100     for ( i = 0; i < 32 ; i++ ) {
00101         Delay1mS(10);
00102 
00103         ReadGyros();
00104         for ( g = 0; g <(uint8)3; g++ )
00105             GyroNeutral[g] += Gyro[g];
00106     }
00107 
00108     for ( g = 0; g <(uint8)3; g++ ) {
00109         GyroNeutral[g] *= 0.03125;
00110         Gyro[g] = Gyrop[g] = 0.0;
00111         for ( s = 0; s < MaxAttitudeScheme; s++ )
00112             EstAngle[g][s] = EstRate[g][s] = 0.0;
00113     }
00114 
00115     LEDRed_OFF;
00116 
00117 } // ErectGyros
00118 
00119 void GyroTest(void) {
00120     TxString("\r\nGyro Test - ");
00121     ShowGyroType();
00122 
00123     ReadGyros();
00124 
00125     TxString("\r\n\tRate and Max Delta(Deg./Sec.)\r\n");
00126 
00127     TxString("\r\n\tRoll:   \t");
00128     TxVal32(Gyro[Roll] * MILLIANGLE, 3, HT);
00129     TxVal32(GyroNoise[Roll] * MILLIANGLE, 3, 0);
00130     TxNextLine();
00131     TxString("\tPitch:  \t");
00132     TxVal32(Gyro[Pitch] * MILLIANGLE, 3, HT);
00133     TxVal32(GyroNoise[Pitch] * MILLIANGLE, 3, 0);
00134     TxNextLine();
00135     TxString("\tYaw:    \t");
00136     TxVal32(Gyro[Yaw] * MILLIANGLE, 3, HT);
00137     TxVal32(GyroNoise[Yaw] * MILLIANGLE, 3, 0);
00138     TxNextLine();
00139 
00140     switch ( GyroType ) {
00141         case ITG3200Gyro:
00142             ITG3200Test();
00143             break;
00144         default:
00145             break;
00146     } // switch
00147 
00148     if ( F.GyroFailure )
00149         TxString("\r\nFAILED\r\n");
00150 
00151 } // GyroTest
00152 
00153 void InitGyros(void) {
00154 
00155     if ( ITG3200GyroActive() )
00156         GyroType = ITG3200Gyro;
00157     else
00158         GyroType = P[GyroRollPitchType];
00159 
00160     switch ( GyroType ) {
00161         case ITG3200Gyro:
00162             InitITG3200Gyro();
00163             break;
00164         case IRSensors:
00165             InitIRSensors();
00166         default:
00167             InitAnalogGyros();
00168             break;
00169     } // switch
00170 
00171     Delay1mS(50);
00172     ErectGyros();
00173 
00174 } // InitGyros
00175 
00176 void ShowGyroType(void) {
00177     switch ( GyroType ) {
00178         case MLX90609Gyro:
00179             TxString("MLX90609");
00180             break;
00181         case ADXRS150Gyro:
00182             TxString("ADXRS613/150");
00183             break;
00184         case IDG300Gyro:
00185             TxString("IDG300");
00186             break;
00187         case LY530Gyro:
00188             TxString("ST-AY530");
00189             break;
00190         case ADXRS300Gyro:
00191             TxString("ADXRS610/300");
00192             break;
00193         case ITG3200Gyro:
00194             TxString("ITG3200");
00195             break;
00196         case IRSensors:
00197             TxString("IR Sensors");
00198             break;
00199         default:
00200             TxString("unknown");
00201             break;
00202     } // switch
00203 } // ShowGyroType
00204 
00205 //________________________________________________________________________________________
00206 
00207 // Analog Gyros
00208 
00209 void ReadAnalogGyros(void);
00210 void InitAnalogGyros(void);
00211 void CheckAnalogGyroFault(uint8, uint8, uint8);
00212 void AnalogGyroTest(void);
00213 
00214 void ReadAnalogGyros(void) {
00215     static uint8 g;
00216 
00217     GyroADC[Roll] = -RollADC.read();
00218     GyroADC[Pitch] = -PitchADC.read();
00219     GyroADC[Yaw] = YawADC.read();
00220 
00221     for ( g = 0; g < (uint8)3; g++ )
00222         Gyro[g] = GyroADC[g] * GyroToRadian[GyroType];
00223 
00224 } // ReadAnalogGyros
00225 
00226 void InitAnalogGyros(void) {
00227     // nothing to do
00228     F.GyroFailure = false;
00229 } // InitAnalogGyros
00230 
00231 //________________________________________________________________________________________
00232 
00233 // ITG3200 3-axis I2C Gyro
00234 
00235 void ReadITG3200(void);
00236 uint8 ReadByteITG3200(uint8);
00237 boolean WriteByteITG3200(uint8, uint8);
00238 void InitITG3200(void);
00239 void ITG3200Test(void);
00240 boolean ITG3200Active(void);
00241 
00242 real32 ITG3200Temperature;
00243 
00244 void ReadITG3200Gyro(void) {
00245     static char G[6];
00246     static uint8 g;
00247     static i16u GX, GY, GZ;
00248 
00249     I2CGYRO.start();
00250     if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto ITG3200Error;
00251     if ( I2CGYRO.write(ITG3200_GX_H) != I2C_ACK ) goto ITG3200Error;
00252     I2CGYRO.stop();
00253 
00254     if ( I2CGYRO.blockread(ITG3200_ID, G, 6) ) goto ITG3200Error;
00255 
00256     GX.b0 = G[1];
00257     GX.b1 = G[0];
00258     GY.b0 = G[3];
00259     GY.b1 = G[2];
00260     GZ.b0 = G[5];
00261     GZ.b1 = G[4];
00262 
00263     if ( F.Using9DOF ) { // SparkFun/QuadroUFO breakout pins forward components up
00264         GyroADC[Roll] = -(real32)GY.i16;
00265         GyroADC[Pitch] = -(real32)GX.i16;
00266         GyroADC[Yaw] = -(real32)GZ.i16;
00267     } else { // SparkFun 6DOF breakout pins forward components down
00268         GyroADC[Roll] = -(real32)GX.i16;
00269         GyroADC[Pitch] = -(real32)GY.i16;
00270         GyroADC[Yaw] = (real32)GZ.i16;
00271     }
00272 
00273     for ( g = 0; g < (uint8)3; g++ )
00274         Gyro[g] = GyroADC[g] * GyroToRadian[ITG3200Gyro];
00275 
00276     return;
00277 
00278 ITG3200Error:
00279     I2CGYRO.stop();
00280 
00281     I2CError[ITG3200_ID]++;
00282 
00283     Stats[GyroFailS]++; // not in flight keep trying
00284     F.GyroFailure = true;
00285 
00286 } // ReadITG3200Gyro
00287 
00288 uint8 ReadByteITG3200(uint8 a) {
00289     static uint8 d;
00290 
00291     I2CGYRO.start();
00292     if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto ITG3200Error;
00293     if ( I2CGYRO.write(a) != I2C_ACK ) goto ITG3200Error;
00294     I2CGYRO.start();
00295     if ( I2CGYRO.write(ITG3200_RD) != I2C_ACK ) goto ITG3200Error;
00296     d = I2CGYRO.read(I2C_NACK);
00297     I2CGYRO.stop();
00298 
00299     return ( d );
00300 
00301 ITG3200Error:
00302     I2CGYRO.stop();
00303 
00304     I2CError[ITG3200_ID]++;
00305     // GYRO FAILURE - FATAL
00306     Stats[GyroFailS]++;
00307 
00308     //F.GyroFailure = true;
00309 
00310     return ( 0 );
00311 
00312 } // ReadByteITG3200
00313 
00314 boolean WriteByteITG3200(uint8 a, uint8 d) {
00315 
00316     I2CGYRO.start();    // restart
00317     if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto ITG3200Error;
00318     if ( I2CGYRO.write(a) != I2C_ACK ) goto ITG3200Error;
00319     if ( I2CGYRO.write(d) != I2C_ACK ) goto ITG3200Error;
00320     I2CGYRO.stop();
00321 
00322     return(false);
00323 
00324 ITG3200Error:
00325     I2CGYRO.stop();
00326 
00327     I2CError[ITG3200_ID]++;
00328     // GYRO FAILURE - FATAL
00329     Stats[GyroFailS]++;
00330     F.GyroFailure = true;
00331 
00332     return(true);
00333 
00334 } // WriteByteITG3200
00335 
00336 void InitITG3200Gyro(void) {
00337 
00338 #define FS_SEL 3
00339 
00340 //#define DLPF_CFG 1 // 188HZ
00341 #define DLPF_CFG 2 // 98HZ
00342 //#define DLPF_CFG 3 // 42HZ
00343 
00344     if ( WriteByteITG3200(ITG3200_PWR_M, 0x80) ) goto ITG3200Error; // Reset to defaults
00345     if ( WriteByteITG3200(ITG3200_SMPL, 0x00) ) goto ITG3200Error; // continuous update
00346     if ( WriteByteITG3200(ITG3200_DLPF, (FS_SEL << 3) | DLPF_CFG ) ) goto ITG3200Error; // 188Hz, 2000deg/S
00347     if ( WriteByteITG3200(ITG3200_INT_C, 0x00) ) goto ITG3200Error; // no interrupts
00348     if ( WriteByteITG3200(ITG3200_PWR_M, 0x01) ) goto ITG3200Error; // X Gyro as Clock Ref.
00349 
00350     Delay1mS(50);
00351 
00352     F.GyroFailure = false;
00353 
00354     ReadITG3200Gyro();
00355 
00356     return;
00357 
00358 ITG3200Error:
00359 
00360     F.GyroFailure = true;
00361 
00362 } // InitITG3200Gyro
00363 
00364 void ITG3200Test(void) {
00365     static uint8 MyID, SMPLRT_DIV, DLPF_FS, PWR_MGM;
00366     static int16 TEMP,GYRO_X, GYRO_Y, GYRO_Z;
00367 
00368     MyID = ReadByteITG3200(ITG3200_WHO);
00369 
00370     TxString("\tWHO_AM_I  \t0x");
00371     TxValH(MyID);
00372     TxNextLine();
00373     Delay1mS(1);
00374     SMPLRT_DIV = ReadByteITG3200(ITG3200_SMPL);
00375     DLPF_FS = ReadByteITG3200(ITG3200_DLPF);
00376     TEMP = (int16)ReadByteITG3200(ITG3200_TMP_H)<<8 | ReadByteITG3200(ITG3200_TMP_L);
00377     GYRO_X = (int16)ReadByteITG3200(ITG3200_GX_H)<<8 | ReadByteITG3200(ITG3200_GX_L);
00378     GYRO_Y = (int16)ReadByteITG3200(ITG3200_GY_H)<<8 | ReadByteITG3200(ITG3200_GY_L);
00379     GYRO_Z = (int16)ReadByteITG3200(ITG3200_GZ_H)<<8 | ReadByteITG3200(ITG3200_GZ_L);
00380     PWR_MGM = ReadByteITG3200(ITG3200_PWR_M);
00381 
00382     ITG3200Temperature = 35.0 + ((TEMP + 13200.0 ) / 280.0);
00383 
00384     TxString("\tSMPLRT_DIV\t");
00385     TxVal32( SMPLRT_DIV,0,0);
00386     TxNextLine();
00387     TxString("\tDLPF      \t");
00388     TxVal32( DLPF_FS & 7,0,0 );
00389     TxString(" FS         \t");
00390     TxVal32( (DLPF_FS>>3)&3, 0, 0);
00391     TxNextLine();
00392     TxString("\tTEMP      \t");
00393     TxVal32( TEMP, 0, 0);
00394     TxNextLine();
00395     TxString("\tGYRO_X    \t");
00396     TxVal32( GYRO_X, 0, 0);
00397     TxNextLine();
00398     TxString("\tGYRO_Y    \t");
00399     TxVal32( GYRO_Y, 0, 0);
00400     TxNextLine();
00401     TxString("\tGYRO_Z    \t");
00402     TxVal32( GYRO_Z, 0, 0);
00403     TxNextLine();
00404     TxString("\tPWR_MGM   \t0x");
00405     TxValH( PWR_MGM );
00406     TxNextLine();
00407 
00408     TxNextLine();
00409 
00410 } // ITG3200Test
00411 
00412 boolean ITG3200GyroActive(void) {
00413 
00414     F.GyroFailure = !I2CGYROAddressResponds( ITG3200_ID );
00415 
00416     if ( !F.GyroFailure )
00417         TrackMinI2CRate(400000);
00418 
00419     return ( !F.GyroFailure );
00420 
00421 } // ITG3200GyroActive