Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

gyro.c@0:62a1c91a859a, 2011-02-18 (annotated)

Committer:
gke
Date:
Fri Feb 18 22:28:05 2011 +0000
Revision:
0:62a1c91a859a
Child:
1:1e3318a30ddd
First release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gke 0:62a1c91a859a 1 // ===============================================================================================
gke 0:62a1c91a859a 2 // = UAVXArm Quadrocopter Controller =
gke 0:62a1c91a859a 3 // = Copyright (c) 2008 by Prof. Greg Egan =
gke 0:62a1c91a859a 4 // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer =
gke 0:62a1c91a859a 5 // = http://code.google.com/p/uavp-mods/ http://uavp.ch =
gke 0:62a1c91a859a 6 // ===============================================================================================
gke 0:62a1c91a859a 7
gke 0:62a1c91a859a 8 // This is part of UAVXArm.
gke 0:62a1c91a859a 9
gke 0:62a1c91a859a 10 // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
gke 0:62a1c91a859a 11 // General Public License as published by the Free Software Foundation, either version 3 of the
gke 0:62a1c91a859a 12 // License, or (at your option) any later version.
gke 0:62a1c91a859a 13
gke 0:62a1c91a859a 14 // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
gke 0:62a1c91a859a 15 // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
gke 0:62a1c91a859a 16 // See the GNU General Public License for more details.
gke 0:62a1c91a859a 17
gke 0:62a1c91a859a 18 // You should have received a copy of the GNU General Public License along with this program.
gke 0:62a1c91a859a 19 // If not, see http://www.gnu.org/licenses/
gke 0:62a1c91a859a 20
gke 0:62a1c91a859a 21 #include "UAVXArm.h"
gke 0:62a1c91a859a 22 const real32 GyroToRadian[UnknownGyro] = {
gke 0:62a1c91a859a 23 0.023841, // MLX90609
gke 0:62a1c91a859a 24 0.044680, // ADXRS150
gke 0:62a1c91a859a 25 0.007149, // IDG300
gke 0:62a1c91a859a 26 0.011796, // LY530
gke 0:62a1c91a859a 27 0.017872, // ADXRS300
gke 0:62a1c91a859a 28 0.000607, // ITG3200
gke 0:62a1c91a859a 29 1.0 // Infrared Sensors
gke 0:62a1c91a859a 30 // add others as required
gke 0:62a1c91a859a 31 };
gke 0:62a1c91a859a 32
gke 0:62a1c91a859a 33 void ReadGyros(void);
gke 0:62a1c91a859a 34 void GetGyroRates(void);
gke 0:62a1c91a859a 35 void CheckGyroFault(uint8, uint8, uint8);
gke 0:62a1c91a859a 36 void ErectGyros(void);
gke 0:62a1c91a859a 37 void InitGyros(void);
gke 0:62a1c91a859a 38 void GyroTest(void);
gke 0:62a1c91a859a 39 void ShowGyroType(void);
gke 0:62a1c91a859a 40
gke 0:62a1c91a859a 41 real32 GyroADC[3], GyroNeutral[3], Gyro[3]; // Radians
gke 0:62a1c91a859a 42 uint8 GyroType;
gke 0:62a1c91a859a 43
gke 0:62a1c91a859a 44 void GetGyroRates(void) {
gke 0:62a1c91a859a 45 static uint8 g;
gke 0:62a1c91a859a 46
gke 0:62a1c91a859a 47 ReadGyros();
gke 0:62a1c91a859a 48 for ( g = 0; g < (uint8)3; g++ )
gke 0:62a1c91a859a 49 Gyro[g] = ( GyroADC[g] - GyroNeutral[g] ) ;
gke 0:62a1c91a859a 50 } // GetGyroRates
gke 0:62a1c91a859a 51
gke 0:62a1c91a859a 52 void ReadGyros(void) {
gke 0:62a1c91a859a 53 switch ( GyroType ) {
gke 0:62a1c91a859a 54 case ITG3200Gyro:
gke 0:62a1c91a859a 55 ReadITG3200Gyro();
gke 0:62a1c91a859a 56 break;
gke 0:62a1c91a859a 57 case IRSensors:
gke 0:62a1c91a859a 58 GetIRAttitude();
gke 0:62a1c91a859a 59 break;
gke 0:62a1c91a859a 60 default :
gke 0:62a1c91a859a 61 ReadAnalogGyros();
gke 0:62a1c91a859a 62 break;
gke 0:62a1c91a859a 63 } // switch
gke 0:62a1c91a859a 64 } // ReadGyros
gke 0:62a1c91a859a 65
gke 0:62a1c91a859a 66 void ErectGyros(void) {
gke 0:62a1c91a859a 67 static int16 i, g;
gke 0:62a1c91a859a 68
gke 0:62a1c91a859a 69 LEDRed_ON;
gke 0:62a1c91a859a 70
gke 0:62a1c91a859a 71 for ( g = 0; g <(int8)3; g++ )
gke 0:62a1c91a859a 72 GyroNeutral[g] = 0.0;
gke 0:62a1c91a859a 73
gke 0:62a1c91a859a 74 for ( i = 0; i < 32 ; i++ ) {
gke 0:62a1c91a859a 75 Delay1mS(10);
gke 0:62a1c91a859a 76
gke 0:62a1c91a859a 77 ReadGyros();
gke 0:62a1c91a859a 78 for ( g = 0; g <(int8)3; g++ )
gke 0:62a1c91a859a 79 GyroNeutral[g] += GyroADC[g];
gke 0:62a1c91a859a 80 }
gke 0:62a1c91a859a 81
gke 0:62a1c91a859a 82 for ( g = 0; g <(int8)3; g++ ) {
gke 0:62a1c91a859a 83 GyroNeutral[g] *= 0.03125;
gke 0:62a1c91a859a 84 Gyro[g] = 0.0;
gke 0:62a1c91a859a 85 }
gke 0:62a1c91a859a 86
gke 0:62a1c91a859a 87 LEDRed_OFF;
gke 0:62a1c91a859a 88
gke 0:62a1c91a859a 89 } // ErectGyros
gke 0:62a1c91a859a 90
gke 0:62a1c91a859a 91 void GyroTest(void) {
gke 0:62a1c91a859a 92 TxString("\r\nGyro Test - ");
gke 0:62a1c91a859a 93 ShowGyroType();
gke 0:62a1c91a859a 94
gke 0:62a1c91a859a 95 ReadGyros();
gke 0:62a1c91a859a 96
gke 0:62a1c91a859a 97 TxString("\r\n\tRoll: \t");
gke 0:62a1c91a859a 98 TxVal32(GyroADC[Roll] * 1000.0, 3, 0);
gke 0:62a1c91a859a 99 TxNextLine();
gke 0:62a1c91a859a 100 TxString("\tPitch: \t");
gke 0:62a1c91a859a 101 TxVal32(GyroADC[Pitch] * 1000.0, 3, 0);
gke 0:62a1c91a859a 102 TxNextLine();
gke 0:62a1c91a859a 103 TxString("\tYaw: \t");
gke 0:62a1c91a859a 104 TxVal32(GyroADC[Yaw] * 1000.0, 3, 0);
gke 0:62a1c91a859a 105 TxNextLine();
gke 0:62a1c91a859a 106
gke 0:62a1c91a859a 107 TxString("Expect ~0.0 ( ~0.5 for for analog gyros)\r\n");
gke 0:62a1c91a859a 108
gke 0:62a1c91a859a 109 switch ( GyroType ) {
gke 0:62a1c91a859a 110 case ITG3200Gyro:
gke 0:62a1c91a859a 111 ITG3200Test();
gke 0:62a1c91a859a 112 break;
gke 0:62a1c91a859a 113 default:
gke 0:62a1c91a859a 114 break;
gke 0:62a1c91a859a 115 } // switch
gke 0:62a1c91a859a 116
gke 0:62a1c91a859a 117 if ( F.GyroFailure )
gke 0:62a1c91a859a 118 TxString("\r\nFAILED\r\n");
gke 0:62a1c91a859a 119
gke 0:62a1c91a859a 120 } // GyroTest
gke 0:62a1c91a859a 121
gke 0:62a1c91a859a 122 void InitGyros(void) {
gke 0:62a1c91a859a 123 if ( ITG3200GyroActive() )
gke 0:62a1c91a859a 124 GyroType = ITG3200Gyro;
gke 0:62a1c91a859a 125 else
gke 0:62a1c91a859a 126 GyroType = P[GyroRollPitchType];
gke 0:62a1c91a859a 127
gke 0:62a1c91a859a 128 switch ( GyroType ) {
gke 0:62a1c91a859a 129 case ITG3200Gyro:
gke 0:62a1c91a859a 130 InitITG3200Gyro();
gke 0:62a1c91a859a 131 break;
gke 0:62a1c91a859a 132 case IRSensors:
gke 0:62a1c91a859a 133 InitIRSensors();
gke 0:62a1c91a859a 134 default:
gke 0:62a1c91a859a 135 InitAnalogGyros();
gke 0:62a1c91a859a 136 break;
gke 0:62a1c91a859a 137 } // switch
gke 0:62a1c91a859a 138
gke 0:62a1c91a859a 139 Delay1mS(50);
gke 0:62a1c91a859a 140 ErectGyros();
gke 0:62a1c91a859a 141
gke 0:62a1c91a859a 142 } // InitGyros
gke 0:62a1c91a859a 143
gke 0:62a1c91a859a 144 void ShowGyroType(void) {
gke 0:62a1c91a859a 145 switch ( GyroType ) {
gke 0:62a1c91a859a 146 case MLX90609Gyro:
gke 0:62a1c91a859a 147 TxString("MLX90609");
gke 0:62a1c91a859a 148 break;
gke 0:62a1c91a859a 149 case ADXRS150Gyro:
gke 0:62a1c91a859a 150 TxString("ADXRS613/150");
gke 0:62a1c91a859a 151 break;
gke 0:62a1c91a859a 152 case IDG300Gyro:
gke 0:62a1c91a859a 153 TxString("IDG300");
gke 0:62a1c91a859a 154 break;
gke 0:62a1c91a859a 155 case LY530Gyro:
gke 0:62a1c91a859a 156 TxString("ST-AY530");
gke 0:62a1c91a859a 157 break;
gke 0:62a1c91a859a 158 case ADXRS300Gyro:
gke 0:62a1c91a859a 159 TxString("ADXRS610/300");
gke 0:62a1c91a859a 160 break;
gke 0:62a1c91a859a 161 case ITG3200Gyro:
gke 0:62a1c91a859a 162 TxString("ITG3200");
gke 0:62a1c91a859a 163 break;
gke 0:62a1c91a859a 164 case IRSensors:
gke 0:62a1c91a859a 165 TxString("IR Sensors");
gke 0:62a1c91a859a 166 break;
gke 0:62a1c91a859a 167 default:
gke 0:62a1c91a859a 168 TxString("unknown");
gke 0:62a1c91a859a 169 break;
gke 0:62a1c91a859a 170 } // switch
gke 0:62a1c91a859a 171 } // ShowGyroType
gke 0:62a1c91a859a 172
gke 0:62a1c91a859a 173 //________________________________________________________________________________________
gke 0:62a1c91a859a 174
gke 0:62a1c91a859a 175 // Analog Gyros
gke 0:62a1c91a859a 176
gke 0:62a1c91a859a 177 void ReadAnalogGyros(void);
gke 0:62a1c91a859a 178 void InitAnalogGyros(void);
gke 0:62a1c91a859a 179 void CheckAnalogGyroFault(uint8, uint8, uint8);
gke 0:62a1c91a859a 180 void AnalogGyroTest(void);
gke 0:62a1c91a859a 181
gke 0:62a1c91a859a 182 void ReadAnalogGyros(void) {
gke 0:62a1c91a859a 183 static uint8 g;
gke 0:62a1c91a859a 184
gke 0:62a1c91a859a 185 GyroADC[Roll] = RollADC.read();
gke 0:62a1c91a859a 186 GyroADC[Pitch] = PitchADC.read();
gke 0:62a1c91a859a 187 GyroADC[Yaw] = YawADC.read();
gke 0:62a1c91a859a 188
gke 0:62a1c91a859a 189 for ( g = 0; g < (uint8)3; g++ )
gke 0:62a1c91a859a 190 GyroADC[g] *= GyroToRadian[GyroType];
gke 0:62a1c91a859a 191 } // ReadAnalogGyros
gke 0:62a1c91a859a 192
gke 0:62a1c91a859a 193 void InitAnalogGyros(void) {
gke 0:62a1c91a859a 194 // nothing to do
gke 0:62a1c91a859a 195 F.GyroFailure = false;
gke 0:62a1c91a859a 196 } // InitAnalogGyros
gke 0:62a1c91a859a 197
gke 0:62a1c91a859a 198 //________________________________________________________________________________________
gke 0:62a1c91a859a 199
gke 0:62a1c91a859a 200 // ITG3200 3-axis I2C Gyro
gke 0:62a1c91a859a 201
gke 0:62a1c91a859a 202 void ReadITG3200(void);
gke 0:62a1c91a859a 203 uint8 ReadByteITG3200(uint8);
gke 0:62a1c91a859a 204 void WriteByteITG3200(uint8, uint8);
gke 0:62a1c91a859a 205 void InitITG3200(void);
gke 0:62a1c91a859a 206 void ITG3200Test(void);
gke 0:62a1c91a859a 207 boolean ITG3200Active(void);
gke 0:62a1c91a859a 208
gke 0:62a1c91a859a 209 real32 ITG3200Temperature;
gke 0:62a1c91a859a 210
gke 0:62a1c91a859a 211 void ReadITG3200Gyro(void) {
gke 0:62a1c91a859a 212 static char G[6];
gke 0:62a1c91a859a 213 static uint8 g;
gke 0:62a1c91a859a 214 static i16u GX, GY, GZ;
gke 0:62a1c91a859a 215
gke 0:62a1c91a859a 216 I2CGYRO.start();
gke 0:62a1c91a859a 217 if ( I2CGYRO.write(ITG3200_W) != I2C_ACK ) goto SGerror;
gke 0:62a1c91a859a 218 if ( I2CGYRO.write(ITG3200_GX_H) != I2C_ACK ) goto SGerror;
gke 0:62a1c91a859a 219 I2CGYRO.stop();
gke 0:62a1c91a859a 220
gke 0:62a1c91a859a 221 I2CGYRO.read(ITG3200_R, G, 6);
gke 0:62a1c91a859a 222
gke 0:62a1c91a859a 223 GX.b0 = G[1];
gke 0:62a1c91a859a 224 GX.b1 = G[0];
gke 0:62a1c91a859a 225 GY.b0 = G[3];
gke 0:62a1c91a859a 226 GY.b1 = G[2];
gke 0:62a1c91a859a 227 GZ.b0 = G[5];
gke 0:62a1c91a859a 228 GZ.b1 = G[4];
gke 0:62a1c91a859a 229
gke 0:62a1c91a859a 230 if ( F.Using9DOF ) { // SparkFun/QuadroUFO breakout pins forward components up
gke 0:62a1c91a859a 231 GyroADC[Roll] = -(real32)GY.i16;
gke 0:62a1c91a859a 232 GyroADC[Pitch] = -(real32)GX.i16;
gke 0:62a1c91a859a 233 GyroADC[Yaw] = -(real32)GZ.i16;
gke 0:62a1c91a859a 234 } else { // SparkFun 6DOF breakout pins forward components down
gke 0:62a1c91a859a 235 GyroADC[Roll] = -(real32)GX.i16;
gke 0:62a1c91a859a 236 GyroADC[Pitch] = -(real32)GY.i16;
gke 0:62a1c91a859a 237 GyroADC[Yaw] = (real32)GZ.i16;
gke 0:62a1c91a859a 238 }
gke 0:62a1c91a859a 239
gke 0:62a1c91a859a 240 for ( g = 0; g < (uint8)3; g++ )
gke 0:62a1c91a859a 241 GyroADC[g] *= GyroToRadian[ITG3200Gyro];
gke 0:62a1c91a859a 242
gke 0:62a1c91a859a 243 return;
gke 0:62a1c91a859a 244
gke 0:62a1c91a859a 245 SGerror:
gke 0:62a1c91a859a 246 I2CGYRO.stop();
gke 0:62a1c91a859a 247 // GYRO FAILURE - FATAL
gke 0:62a1c91a859a 248 Stats[GyroFailS]++;
gke 0:62a1c91a859a 249 F.GyroFailure = true;
gke 0:62a1c91a859a 250 return;
gke 0:62a1c91a859a 251 } // ReadITG3200Gyro
gke 0:62a1c91a859a 252
gke 0:62a1c91a859a 253 uint8 ReadByteITG3200(uint8 a) {
gke 0:62a1c91a859a 254 static uint8 d;
gke 0:62a1c91a859a 255
gke 0:62a1c91a859a 256 I2CGYRO.start();
gke 0:62a1c91a859a 257 if ( I2CGYRO.write(ITG3200_W) != I2C_ACK ) goto SGerror;
gke 0:62a1c91a859a 258 if ( I2CGYRO.write(a) != I2C_ACK ) goto SGerror;
gke 0:62a1c91a859a 259
gke 0:62a1c91a859a 260 I2CGYRO.start();
gke 0:62a1c91a859a 261 if ( I2CGYRO.write(ITG3200_R) != I2C_ACK ) goto SGerror;
gke 0:62a1c91a859a 262 d = I2CGYRO.read(I2C_NACK);
gke 0:62a1c91a859a 263 I2CGYRO.stop();
gke 0:62a1c91a859a 264
gke 0:62a1c91a859a 265 return ( d );
gke 0:62a1c91a859a 266
gke 0:62a1c91a859a 267 SGerror:
gke 0:62a1c91a859a 268 I2CGYRO.stop();
gke 0:62a1c91a859a 269 // GYRO FAILURE - FATAL
gke 0:62a1c91a859a 270 Stats[GyroFailS]++;
gke 0:62a1c91a859a 271 F.GyroFailure = true;
gke 0:62a1c91a859a 272 return (I2C_NACK);
gke 0:62a1c91a859a 273 } // ReadByteITG3200
gke 0:62a1c91a859a 274
gke 0:62a1c91a859a 275 void WriteByteITG3200(uint8 a, uint8 d) {
gke 0:62a1c91a859a 276 I2CGYRO.start(); // restart
gke 0:62a1c91a859a 277 if ( I2CGYRO.write(ITG3200_W) != I2C_ACK ) goto SGerror;
gke 0:62a1c91a859a 278 if ( I2CGYRO.write(a) != I2C_ACK ) goto SGerror;
gke 0:62a1c91a859a 279 if ( I2CGYRO.write(d) != I2C_ACK ) goto SGerror;
gke 0:62a1c91a859a 280 I2CGYRO.stop();
gke 0:62a1c91a859a 281 return;
gke 0:62a1c91a859a 282
gke 0:62a1c91a859a 283 SGerror:
gke 0:62a1c91a859a 284 I2CGYRO.stop();
gke 0:62a1c91a859a 285 // GYRO FAILURE - FATAL
gke 0:62a1c91a859a 286 Stats[GyroFailS]++;
gke 0:62a1c91a859a 287 F.GyroFailure = true;
gke 0:62a1c91a859a 288 return;
gke 0:62a1c91a859a 289 } // WriteByteITG3200
gke 0:62a1c91a859a 290
gke 0:62a1c91a859a 291 void InitITG3200Gyro(void) {
gke 0:62a1c91a859a 292 F.GyroFailure = false; // reset optimistically!
gke 0:62a1c91a859a 293
gke 0:62a1c91a859a 294 WriteByteITG3200(ITG3200_PWR_M, 0x80); // Reset to defaults
gke 0:62a1c91a859a 295 WriteByteITG3200(ITG3200_SMPL, 0x00); // continuous update
gke 0:62a1c91a859a 296 WriteByteITG3200(ITG3200_DLPF, 0x19); // 188Hz, 2000deg/S
gke 0:62a1c91a859a 297 WriteByteITG3200(ITG3200_INT_C, 0x00); // no interrupts
gke 0:62a1c91a859a 298 WriteByteITG3200(ITG3200_PWR_M, 0x01); // X Gyro as Clock Ref.
gke 0:62a1c91a859a 299
gke 0:62a1c91a859a 300 Delay1mS(50);
gke 0:62a1c91a859a 301
gke 0:62a1c91a859a 302 ReadITG3200Gyro();
gke 0:62a1c91a859a 303
gke 0:62a1c91a859a 304 } // InitITG3200Gyro
gke 0:62a1c91a859a 305
gke 0:62a1c91a859a 306 void ITG3200Test(void) {
gke 0:62a1c91a859a 307 static uint8 MyID, SMPLRT_DIV, DLPF_FS, PWR_MGM;
gke 0:62a1c91a859a 308 static int16 TEMP,GYRO_X, GYRO_Y, GYRO_Z;
gke 0:62a1c91a859a 309
gke 0:62a1c91a859a 310 MyID = ReadByteITG3200(ITG3200_WHO);
gke 0:62a1c91a859a 311
gke 0:62a1c91a859a 312 TxString("\tWHO_AM_I \t0x");
gke 0:62a1c91a859a 313 TxValH(MyID);
gke 0:62a1c91a859a 314 TxNextLine();
gke 0:62a1c91a859a 315 Delay1mS(1);
gke 0:62a1c91a859a 316 SMPLRT_DIV = ReadByteITG3200(ITG3200_SMPL);
gke 0:62a1c91a859a 317 DLPF_FS = ReadByteITG3200(ITG3200_DLPF);
gke 0:62a1c91a859a 318 TEMP = (int16)ReadByteITG3200(ITG3200_TMP_H)<<8 | ReadByteITG3200(ITG3200_TMP_L);
gke 0:62a1c91a859a 319 GYRO_X = (int16)ReadByteITG3200(ITG3200_GX_H)<<8 | ReadByteITG3200(ITG3200_GX_L);
gke 0:62a1c91a859a 320 GYRO_Y = (int16)ReadByteITG3200(ITG3200_GY_H)<<8 | ReadByteITG3200(ITG3200_GY_L);
gke 0:62a1c91a859a 321 GYRO_Z = (int16)ReadByteITG3200(ITG3200_GZ_H)<<8 | ReadByteITG3200(ITG3200_GZ_L);
gke 0:62a1c91a859a 322 PWR_MGM = ReadByteITG3200(ITG3200_PWR_M);
gke 0:62a1c91a859a 323
gke 0:62a1c91a859a 324 ITG3200Temperature = 35.0 + ((TEMP + 13200.0 ) / 280.0);
gke 0:62a1c91a859a 325
gke 0:62a1c91a859a 326 TxString("\tSMPLRT_DIV\t");
gke 0:62a1c91a859a 327 TxVal32( SMPLRT_DIV,0,0);
gke 0:62a1c91a859a 328 TxNextLine();
gke 0:62a1c91a859a 329 TxString("\tDLPF \t");
gke 0:62a1c91a859a 330 TxVal32( DLPF_FS & 7,0,0 );
gke 0:62a1c91a859a 331 TxString(" FS \t");
gke 0:62a1c91a859a 332 TxVal32( (DLPF_FS>>3)&3, 0, 0);
gke 0:62a1c91a859a 333 TxNextLine();
gke 0:62a1c91a859a 334 TxString("\tTEMP \t");
gke 0:62a1c91a859a 335 TxVal32( TEMP, 0, 0);
gke 0:62a1c91a859a 336 TxNextLine();
gke 0:62a1c91a859a 337 TxString("\tGYRO_X \t");
gke 0:62a1c91a859a 338 TxVal32( GYRO_X, 0, 0);
gke 0:62a1c91a859a 339 TxNextLine();
gke 0:62a1c91a859a 340 TxString("\tGYRO_Y \t");
gke 0:62a1c91a859a 341 TxVal32( GYRO_Y, 0, 0);
gke 0:62a1c91a859a 342 TxNextLine();
gke 0:62a1c91a859a 343 TxString("\tGYRO_Z \t");
gke 0:62a1c91a859a 344 TxVal32( GYRO_Z, 0, 0);
gke 0:62a1c91a859a 345 TxNextLine();
gke 0:62a1c91a859a 346 TxString("\tPWR_MGM \t0x");
gke 0:62a1c91a859a 347 TxValH( PWR_MGM );
gke 0:62a1c91a859a 348 TxNextLine();
gke 0:62a1c91a859a 349
gke 0:62a1c91a859a 350 TxNextLine();
gke 0:62a1c91a859a 351
gke 0:62a1c91a859a 352 } // ITG3200Test
gke 0:62a1c91a859a 353
gke 0:62a1c91a859a 354 boolean ITG3200GyroActive(void) {
gke 0:62a1c91a859a 355 I2CGYRO.start();
gke 0:62a1c91a859a 356 F.GyroFailure = I2CGYRO.write(ITG3200_ID) != I2C_ACK;
gke 0:62a1c91a859a 357 I2CGYRO.stop();
gke 0:62a1c91a859a 358
gke 0:62a1c91a859a 359 if ( !F.GyroFailure )
gke 0:62a1c91a859a 360 TrackMinI2CRate(400000);
gke 0:62a1c91a859a 361
gke 0:62a1c91a859a 362 return ( !F.GyroFailure );
gke 0:62a1c91a859a 363
gke 0:62a1c91a859a 364 } // ITG3200GyroActive