Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
Diff: gyro.c
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
diff -r 1e3318a30ddd -r 90292f8bd179 gyro.c --- a/gyro.c Fri Feb 25 01:35:24 2011 +0000 +++ b/gyro.c Tue Apr 26 12:12:29 2011 +0000 @@ -2,7 +2,7 @@ // = UAVXArm Quadrocopter Controller = // = Copyright (c) 2008 by Prof. Greg Egan = // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer = -// = http://code.google.com/p/uavp-mods/ http://uavp.ch = +// = http://code.google.com/p/uavp-mods/ = // =============================================================================================== // This is part of UAVXArm. @@ -20,12 +20,12 @@ #include "UAVXArm.h" const real32 GyroToRadian[UnknownGyro] = { - 0.023841, // MLX90609 - 0.044680, // ADXRS150 - 0.007149, // IDG300 - 0.011796, // LY530 - 0.017872, // ADXRS300 - 0.000607, // ITG3200 + 8.635062, // MLX90609 + 4.607669, // ADXRS150 + 28.797933, // IDG300 + 17.453293, // LY530 + 11.519173, // ADXRS300 + 0.000438704 * 2.0 * 1.31, // ITG3200 16bit 2's complement 1.0 // Infrared Sensors // add others as required }; @@ -38,15 +38,41 @@ void GyroTest(void); void ShowGyroType(void); -real32 GyroADC[3], GyroNeutral[3], Gyro[3]; // Radians +real32 GyroADC[3], GyroNoise[3], GyroNeutral[3], Gyro[3], Gyrop[3]; // Radians uint8 GyroType; void GetGyroRates(void) { static uint8 g; + static real32 d, GyroA; ReadGyros(); + + for ( g = 0; g < (uint8)3; g++ ) { + d = fabs(Gyro[g]-Gyrop[g]); + if ( d > GyroNoise[g] ) GyroNoise[g] = d; + } + +#ifndef SUPPRESS_ROLL_PITCH_GYRO_FILTERS + // dT is almost unchanged so this could be optimised + GyroA = dT / ( 1.0 / ( TWOPI * ROLL_PITCH_FREQ ) + dT ); + Gyro[Roll] = LPFilter( Gyro[Roll] - GyroNeutral[Roll], Gyrop[Roll], GyroA ); + Gyro[Pitch] = LPFilter( Gyro[Pitch] - GyroNeutral[Pitch], Gyrop[Pitch], GyroA ); +#endif // !SUPPRESS_ROLL_PITCH_GYRO_FILTERS + +#ifndef SUPPRESS_YAW_GYRO_FILTERS + +#ifdef USE_FIXED_YAW_FILTER + GyroA = dT / ( 1.0 / ( TWOPI * MAX_YAW_FREQ ) + dT ); +#else + GyroA = dT / ( 1.0 / ( TWOPI * YawFilterLPFreq ) + dT ); +#endif // USE_FIXED_YAW_FILTER + + Gyro[Yaw] = LPFilter( Gyro[Yaw] - GyroNeutral[Yaw], Gyrop[Yaw], GyroA ); +#endif // !SUPPRESS_GYRO_FILTERS + for ( g = 0; g < (uint8)3; g++ ) - Gyro[g] = ( GyroADC[g] - GyroNeutral[g] ) ; + Gyrop[g] = Gyro[g]; + } // GetGyroRates void ReadGyros(void) { @@ -64,24 +90,26 @@ } // ReadGyros void ErectGyros(void) { - static int16 i, g; + static uint8 s, i, g; LEDRed_ON; - for ( g = 0; g <(int8)3; g++ ) + for ( g = 0; g <(uint8)3; g++ ) GyroNeutral[g] = 0.0; for ( i = 0; i < 32 ; i++ ) { Delay1mS(10); ReadGyros(); - for ( g = 0; g <(int8)3; g++ ) - GyroNeutral[g] += GyroADC[g]; + for ( g = 0; g <(uint8)3; g++ ) + GyroNeutral[g] += Gyro[g]; } - for ( g = 0; g <(int8)3; g++ ) { + for ( g = 0; g <(uint8)3; g++ ) { GyroNeutral[g] *= 0.03125; - Gyro[g] = 0.0; + Gyro[g] = Gyrop[g] = 0.0; + for ( s = 0; s < MaxAttitudeScheme; s++ ) + EstAngle[g][s] = EstRate[g][s] = 0.0; } LEDRed_OFF; @@ -94,18 +122,21 @@ ReadGyros(); + TxString("\r\n\tRate and Max Delta(Deg./Sec.)\r\n"); + TxString("\r\n\tRoll: \t"); - TxVal32(GyroADC[Roll] * 1000.0, 3, 0); + TxVal32(Gyro[Roll] * MILLIANGLE, 3, HT); + TxVal32(GyroNoise[Roll] * MILLIANGLE, 3, 0); TxNextLine(); TxString("\tPitch: \t"); - TxVal32(GyroADC[Pitch] * 1000.0, 3, 0); + TxVal32(Gyro[Pitch] * MILLIANGLE, 3, HT); + TxVal32(GyroNoise[Pitch] * MILLIANGLE, 3, 0); TxNextLine(); TxString("\tYaw: \t"); - TxVal32(GyroADC[Yaw] * 1000.0, 3, 0); + TxVal32(Gyro[Yaw] * MILLIANGLE, 3, HT); + TxVal32(GyroNoise[Yaw] * MILLIANGLE, 3, 0); TxNextLine(); - TxString("Expect ~0.0 ( ~0.5 for for analog gyros)\r\n"); - switch ( GyroType ) { case ITG3200Gyro: ITG3200Test(); @@ -120,6 +151,7 @@ } // GyroTest void InitGyros(void) { + if ( ITG3200GyroActive() ) GyroType = ITG3200Gyro; else @@ -182,12 +214,13 @@ void ReadAnalogGyros(void) { static uint8 g; - GyroADC[Roll] = RollADC.read(); - GyroADC[Pitch] = PitchADC.read(); + GyroADC[Roll] = -RollADC.read(); + GyroADC[Pitch] = -PitchADC.read(); GyroADC[Yaw] = YawADC.read(); for ( g = 0; g < (uint8)3; g++ ) - GyroADC[g] *= GyroToRadian[GyroType]; + Gyro[g] = GyroADC[g] * GyroToRadian[GyroType]; + } // ReadAnalogGyros void InitAnalogGyros(void) { @@ -201,7 +234,7 @@ void ReadITG3200(void); uint8 ReadByteITG3200(uint8); -void WriteByteITG3200(uint8, uint8); +boolean WriteByteITG3200(uint8, uint8); void InitITG3200(void); void ITG3200Test(void); boolean ITG3200Active(void); @@ -210,41 +243,45 @@ void ReadITG3200Gyro(void) { static char G[6]; - static uint8 g, r; + static uint8 g; static i16u GX, GY, GZ; I2CGYRO.start(); - r = ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ); - r = ( I2CGYRO.write(ITG3200_GX_H) != I2C_ACK ); + if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto ITG3200Error; + if ( I2CGYRO.write(ITG3200_GX_H) != I2C_ACK ) goto ITG3200Error; I2CGYRO.stop(); - if ( I2CGYRO.blockread(ITG3200_ID, G, 6) == 0 ) { + if ( I2CGYRO.blockread(ITG3200_ID, G, 6) ) goto ITG3200Error; - GX.b0 = G[1]; - GX.b1 = G[0]; - GY.b0 = G[3]; - GY.b1 = G[2]; - GZ.b0 = G[5]; - GZ.b1 = G[4]; + GX.b0 = G[1]; + GX.b1 = G[0]; + GY.b0 = G[3]; + GY.b1 = G[2]; + GZ.b0 = G[5]; + GZ.b1 = G[4]; - if ( F.Using9DOF ) { // SparkFun/QuadroUFO breakout pins forward components up - GyroADC[Roll] = -(real32)GY.i16; - GyroADC[Pitch] = -(real32)GX.i16; - GyroADC[Yaw] = -(real32)GZ.i16; - } else { // SparkFun 6DOF breakout pins forward components down - GyroADC[Roll] = -(real32)GX.i16; - GyroADC[Pitch] = -(real32)GY.i16; - GyroADC[Yaw] = (real32)GZ.i16; - } + if ( F.Using9DOF ) { // SparkFun/QuadroUFO breakout pins forward components up + GyroADC[Roll] = -(real32)GY.i16; + GyroADC[Pitch] = -(real32)GX.i16; + GyroADC[Yaw] = -(real32)GZ.i16; + } else { // SparkFun 6DOF breakout pins forward components down + GyroADC[Roll] = -(real32)GX.i16; + GyroADC[Pitch] = -(real32)GY.i16; + GyroADC[Yaw] = (real32)GZ.i16; + } - for ( g = 0; g < (uint8)3; g++ ) - GyroADC[g] *= GyroToRadian[ITG3200Gyro]; + for ( g = 0; g < (uint8)3; g++ ) + Gyro[g] = GyroADC[g] * GyroToRadian[ITG3200Gyro]; + + return; - } else { - // GYRO FAILURE - FATAL - Stats[GyroFailS]++; - // not in flight keep trying F.GyroFailure = true; - } +ITG3200Error: + I2CGYRO.stop(); + + I2CError[ITG3200_ID]++; + + Stats[GyroFailS]++; // not in flight keep trying + F.GyroFailure = true; } // ReadITG3200Gyro @@ -252,53 +289,76 @@ static uint8 d; I2CGYRO.start(); - if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto SGerror; - if ( I2CGYRO.write(a) != I2C_ACK ) goto SGerror; - + if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto ITG3200Error; + if ( I2CGYRO.write(a) != I2C_ACK ) goto ITG3200Error; I2CGYRO.start(); - if ( I2CGYRO.write(ITG3200_RD) != I2C_ACK ) goto SGerror; + if ( I2CGYRO.write(ITG3200_RD) != I2C_ACK ) goto ITG3200Error; d = I2CGYRO.read(I2C_NACK); I2CGYRO.stop(); return ( d ); -SGerror: +ITG3200Error: I2CGYRO.stop(); + + I2CError[ITG3200_ID]++; + // GYRO FAILURE - FATAL + Stats[GyroFailS]++; + + //F.GyroFailure = true; + + return ( 0 ); + +} // ReadByteITG3200 + +boolean WriteByteITG3200(uint8 a, uint8 d) { + + I2CGYRO.start(); // restart + if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto ITG3200Error; + if ( I2CGYRO.write(a) != I2C_ACK ) goto ITG3200Error; + if ( I2CGYRO.write(d) != I2C_ACK ) goto ITG3200Error; + I2CGYRO.stop(); + + return(false); + +ITG3200Error: + I2CGYRO.stop(); + + I2CError[ITG3200_ID]++; // GYRO FAILURE - FATAL Stats[GyroFailS]++; F.GyroFailure = true; - return (I2C_NACK); -} // ReadByteITG3200 -void WriteByteITG3200(uint8 a, uint8 d) { - I2CGYRO.start(); // restart - if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto SGerror; - if ( I2CGYRO.write(a) != I2C_ACK ) goto SGerror; - if ( I2CGYRO.write(d) != I2C_ACK ) goto SGerror; - I2CGYRO.stop(); - return; + return(true); -SGerror: - I2CGYRO.stop(); - // GYRO FAILURE - FATAL - Stats[GyroFailS]++; - F.GyroFailure = true; - return; } // WriteByteITG3200 void InitITG3200Gyro(void) { - F.GyroFailure = false; // reset optimistically! + +#define FS_SEL 3 - WriteByteITG3200(ITG3200_PWR_M, 0x80); // Reset to defaults - WriteByteITG3200(ITG3200_SMPL, 0x00); // continuous update - WriteByteITG3200(ITG3200_DLPF, 0x19); // 188Hz, 2000deg/S - WriteByteITG3200(ITG3200_INT_C, 0x00); // no interrupts - WriteByteITG3200(ITG3200_PWR_M, 0x01); // X Gyro as Clock Ref. +//#define DLPF_CFG 1 // 188HZ +#define DLPF_CFG 2 // 98HZ +//#define DLPF_CFG 3 // 42HZ + + if ( WriteByteITG3200(ITG3200_PWR_M, 0x80) ) goto ITG3200Error; // Reset to defaults + if ( WriteByteITG3200(ITG3200_SMPL, 0x00) ) goto ITG3200Error; // continuous update + if ( WriteByteITG3200(ITG3200_DLPF, (FS_SEL << 3) | DLPF_CFG ) ) goto ITG3200Error; // 188Hz, 2000deg/S + if ( WriteByteITG3200(ITG3200_INT_C, 0x00) ) goto ITG3200Error; // no interrupts + if ( WriteByteITG3200(ITG3200_PWR_M, 0x01) ) goto ITG3200Error; // X Gyro as Clock Ref. Delay1mS(50); + F.GyroFailure = false; + ReadITG3200Gyro(); + return; + +ITG3200Error: + + F.GyroFailure = true; + } // InitITG3200Gyro void ITG3200Test(void) {