Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
gyro.c
- Committer:
- gke
- Date:
- 2011-04-26
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
File content as of revision 2:90292f8bd179:
// =============================================================================================== // = 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/ = // =============================================================================================== // This is part of UAVXArm. // UAVXArm 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. // UAVXArm 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 http://www.gnu.org/licenses/ #include "UAVXArm.h" const real32 GyroToRadian[UnknownGyro] = { 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 }; void ReadGyros(void); void GetGyroRates(void); void CheckGyroFault(uint8, uint8, uint8); void ErectGyros(void); void InitGyros(void); void GyroTest(void); void ShowGyroType(void); 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++ ) Gyrop[g] = Gyro[g]; } // GetGyroRates void ReadGyros(void) { switch ( GyroType ) { case ITG3200Gyro: ReadITG3200Gyro(); break; case IRSensors: GetIRAttitude(); break; default : ReadAnalogGyros(); break; } // switch } // ReadGyros void ErectGyros(void) { static uint8 s, i, g; LEDRed_ON; for ( g = 0; g <(uint8)3; g++ ) GyroNeutral[g] = 0.0; for ( i = 0; i < 32 ; i++ ) { Delay1mS(10); ReadGyros(); for ( g = 0; g <(uint8)3; g++ ) GyroNeutral[g] += Gyro[g]; } for ( g = 0; g <(uint8)3; g++ ) { GyroNeutral[g] *= 0.03125; Gyro[g] = Gyrop[g] = 0.0; for ( s = 0; s < MaxAttitudeScheme; s++ ) EstAngle[g][s] = EstRate[g][s] = 0.0; } LEDRed_OFF; } // ErectGyros void GyroTest(void) { TxString("\r\nGyro Test - "); ShowGyroType(); ReadGyros(); TxString("\r\n\tRate and Max Delta(Deg./Sec.)\r\n"); TxString("\r\n\tRoll: \t"); TxVal32(Gyro[Roll] * MILLIANGLE, 3, HT); TxVal32(GyroNoise[Roll] * MILLIANGLE, 3, 0); TxNextLine(); TxString("\tPitch: \t"); TxVal32(Gyro[Pitch] * MILLIANGLE, 3, HT); TxVal32(GyroNoise[Pitch] * MILLIANGLE, 3, 0); TxNextLine(); TxString("\tYaw: \t"); TxVal32(Gyro[Yaw] * MILLIANGLE, 3, HT); TxVal32(GyroNoise[Yaw] * MILLIANGLE, 3, 0); TxNextLine(); switch ( GyroType ) { case ITG3200Gyro: ITG3200Test(); break; default: break; } // switch if ( F.GyroFailure ) TxString("\r\nFAILED\r\n"); } // GyroTest void InitGyros(void) { if ( ITG3200GyroActive() ) GyroType = ITG3200Gyro; else GyroType = P[GyroRollPitchType]; switch ( GyroType ) { case ITG3200Gyro: InitITG3200Gyro(); break; case IRSensors: InitIRSensors(); default: InitAnalogGyros(); break; } // switch Delay1mS(50); ErectGyros(); } // InitGyros void ShowGyroType(void) { switch ( GyroType ) { case MLX90609Gyro: TxString("MLX90609"); break; case ADXRS150Gyro: TxString("ADXRS613/150"); break; case IDG300Gyro: TxString("IDG300"); break; case LY530Gyro: TxString("ST-AY530"); break; case ADXRS300Gyro: TxString("ADXRS610/300"); break; case ITG3200Gyro: TxString("ITG3200"); break; case IRSensors: TxString("IR Sensors"); break; default: TxString("unknown"); break; } // switch } // ShowGyroType //________________________________________________________________________________________ // Analog Gyros void ReadAnalogGyros(void); void InitAnalogGyros(void); void CheckAnalogGyroFault(uint8, uint8, uint8); void AnalogGyroTest(void); void ReadAnalogGyros(void) { static uint8 g; GyroADC[Roll] = -RollADC.read(); GyroADC[Pitch] = -PitchADC.read(); GyroADC[Yaw] = YawADC.read(); for ( g = 0; g < (uint8)3; g++ ) Gyro[g] = GyroADC[g] * GyroToRadian[GyroType]; } // ReadAnalogGyros void InitAnalogGyros(void) { // nothing to do F.GyroFailure = false; } // InitAnalogGyros //________________________________________________________________________________________ // ITG3200 3-axis I2C Gyro void ReadITG3200(void); uint8 ReadByteITG3200(uint8); boolean WriteByteITG3200(uint8, uint8); void InitITG3200(void); void ITG3200Test(void); boolean ITG3200Active(void); real32 ITG3200Temperature; void ReadITG3200Gyro(void) { static char G[6]; static uint8 g; static i16u GX, GY, GZ; I2CGYRO.start(); 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) ) 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]; 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++ ) Gyro[g] = GyroADC[g] * GyroToRadian[ITG3200Gyro]; return; ITG3200Error: I2CGYRO.stop(); I2CError[ITG3200_ID]++; Stats[GyroFailS]++; // not in flight keep trying F.GyroFailure = true; } // ReadITG3200Gyro uint8 ReadByteITG3200(uint8 a) { static uint8 d; I2CGYRO.start(); 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 ITG3200Error; d = I2CGYRO.read(I2C_NACK); I2CGYRO.stop(); return ( d ); 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(true); } // WriteByteITG3200 void InitITG3200Gyro(void) { #define FS_SEL 3 //#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) { static uint8 MyID, SMPLRT_DIV, DLPF_FS, PWR_MGM; static int16 TEMP,GYRO_X, GYRO_Y, GYRO_Z; MyID = ReadByteITG3200(ITG3200_WHO); TxString("\tWHO_AM_I \t0x"); TxValH(MyID); TxNextLine(); Delay1mS(1); SMPLRT_DIV = ReadByteITG3200(ITG3200_SMPL); DLPF_FS = ReadByteITG3200(ITG3200_DLPF); TEMP = (int16)ReadByteITG3200(ITG3200_TMP_H)<<8 | ReadByteITG3200(ITG3200_TMP_L); GYRO_X = (int16)ReadByteITG3200(ITG3200_GX_H)<<8 | ReadByteITG3200(ITG3200_GX_L); GYRO_Y = (int16)ReadByteITG3200(ITG3200_GY_H)<<8 | ReadByteITG3200(ITG3200_GY_L); GYRO_Z = (int16)ReadByteITG3200(ITG3200_GZ_H)<<8 | ReadByteITG3200(ITG3200_GZ_L); PWR_MGM = ReadByteITG3200(ITG3200_PWR_M); ITG3200Temperature = 35.0 + ((TEMP + 13200.0 ) / 280.0); TxString("\tSMPLRT_DIV\t"); TxVal32( SMPLRT_DIV,0,0); TxNextLine(); TxString("\tDLPF \t"); TxVal32( DLPF_FS & 7,0,0 ); TxString(" FS \t"); TxVal32( (DLPF_FS>>3)&3, 0, 0); TxNextLine(); TxString("\tTEMP \t"); TxVal32( TEMP, 0, 0); TxNextLine(); TxString("\tGYRO_X \t"); TxVal32( GYRO_X, 0, 0); TxNextLine(); TxString("\tGYRO_Y \t"); TxVal32( GYRO_Y, 0, 0); TxNextLine(); TxString("\tGYRO_Z \t"); TxVal32( GYRO_Z, 0, 0); TxNextLine(); TxString("\tPWR_MGM \t0x"); TxValH( PWR_MGM ); TxNextLine(); TxNextLine(); } // ITG3200Test boolean ITG3200GyroActive(void) { F.GyroFailure = !I2CGYROAddressResponds( ITG3200_ID ); if ( !F.GyroFailure ) TrackMinI2CRate(400000); return ( !F.GyroFailure ); } // ITG3200GyroActive