Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

gyro.c@2:90292f8bd179, 2011-04-26 (annotated)

Committer:
gke
Date:
Tue Apr 26 12:12:29 2011 +0000
Revision:
2:90292f8bd179
Parent:
1:1e3318a30ddd 
Not flightworthy. Posted for others to make use of the I2C SW code.

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 2:90292f8bd179 5 // = http://code.google.com/p/uavp-mods/ =
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 2:90292f8bd179 23 8.635062, // MLX90609
gke 2:90292f8bd179 24 4.607669, // ADXRS150
gke 2:90292f8bd179 25 28.797933, // IDG300
gke 2:90292f8bd179 26 17.453293, // LY530
gke 2:90292f8bd179 27 11.519173, // ADXRS300
gke 2:90292f8bd179 28 0.000438704 * 2.0 * 1.31, // ITG3200 16bit 2's complement
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 2:90292f8bd179 41 real32 GyroADC[3], GyroNoise[3], GyroNeutral[3], Gyro[3], Gyrop[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 2:90292f8bd179 46 static real32 d, GyroA;
gke 0:62a1c91a859a 47
gke 0:62a1c91a859a 48 ReadGyros();
gke 2:90292f8bd179 49
gke 2:90292f8bd179 50 for ( g = 0; g < (uint8)3; g++ ) {
gke 2:90292f8bd179 51 d = fabs(Gyro[g]-Gyrop[g]);
gke 2:90292f8bd179 52 if ( d > GyroNoise[g] ) GyroNoise[g] = d;
gke 2:90292f8bd179 53 }
gke 2:90292f8bd179 54
gke 2:90292f8bd179 55 #ifndef SUPPRESS_ROLL_PITCH_GYRO_FILTERS
gke 2:90292f8bd179 56 // dT is almost unchanged so this could be optimised
gke 2:90292f8bd179 57 GyroA = dT / ( 1.0 / ( TWOPI * ROLL_PITCH_FREQ ) + dT );
gke 2:90292f8bd179 58 Gyro[Roll] = LPFilter( Gyro[Roll] - GyroNeutral[Roll], Gyrop[Roll], GyroA );
gke 2:90292f8bd179 59 Gyro[Pitch] = LPFilter( Gyro[Pitch] - GyroNeutral[Pitch], Gyrop[Pitch], GyroA );
gke 2:90292f8bd179 60 #endif // !SUPPRESS_ROLL_PITCH_GYRO_FILTERS
gke 2:90292f8bd179 61
gke 2:90292f8bd179 62 #ifndef SUPPRESS_YAW_GYRO_FILTERS
gke 2:90292f8bd179 63
gke 2:90292f8bd179 64 #ifdef USE_FIXED_YAW_FILTER
gke 2:90292f8bd179 65 GyroA = dT / ( 1.0 / ( TWOPI * MAX_YAW_FREQ ) + dT );
gke 2:90292f8bd179 66 #else
gke 2:90292f8bd179 67 GyroA = dT / ( 1.0 / ( TWOPI * YawFilterLPFreq ) + dT );
gke 2:90292f8bd179 68 #endif // USE_FIXED_YAW_FILTER
gke 2:90292f8bd179 69
gke 2:90292f8bd179 70 Gyro[Yaw] = LPFilter( Gyro[Yaw] - GyroNeutral[Yaw], Gyrop[Yaw], GyroA );
gke 2:90292f8bd179 71 #endif // !SUPPRESS_GYRO_FILTERS
gke 2:90292f8bd179 72
gke 0:62a1c91a859a 73 for ( g = 0; g < (uint8)3; g++ )
gke 2:90292f8bd179 74 Gyrop[g] = Gyro[g];
gke 2:90292f8bd179 75
gke 0:62a1c91a859a 76 } // GetGyroRates
gke 0:62a1c91a859a 77
gke 0:62a1c91a859a 78 void ReadGyros(void) {
gke 0:62a1c91a859a 79 switch ( GyroType ) {
gke 0:62a1c91a859a 80 case ITG3200Gyro:
gke 0:62a1c91a859a 81 ReadITG3200Gyro();
gke 0:62a1c91a859a 82 break;
gke 0:62a1c91a859a 83 case IRSensors:
gke 0:62a1c91a859a 84 GetIRAttitude();
gke 0:62a1c91a859a 85 break;
gke 0:62a1c91a859a 86 default :
gke 0:62a1c91a859a 87 ReadAnalogGyros();
gke 0:62a1c91a859a 88 break;
gke 0:62a1c91a859a 89 } // switch
gke 0:62a1c91a859a 90 } // ReadGyros
gke 0:62a1c91a859a 91
gke 0:62a1c91a859a 92 void ErectGyros(void) {
gke 2:90292f8bd179 93 static uint8 s, i, g;
gke 0:62a1c91a859a 94
gke 0:62a1c91a859a 95 LEDRed_ON;
gke 0:62a1c91a859a 96
gke 2:90292f8bd179 97 for ( g = 0; g <(uint8)3; g++ )
gke 0:62a1c91a859a 98 GyroNeutral[g] = 0.0;
gke 0:62a1c91a859a 99
gke 0:62a1c91a859a 100 for ( i = 0; i < 32 ; i++ ) {
gke 0:62a1c91a859a 101 Delay1mS(10);
gke 0:62a1c91a859a 102
gke 0:62a1c91a859a 103 ReadGyros();
gke 2:90292f8bd179 104 for ( g = 0; g <(uint8)3; g++ )
gke 2:90292f8bd179 105 GyroNeutral[g] += Gyro[g];
gke 0:62a1c91a859a 106 }
gke 0:62a1c91a859a 107
gke 2:90292f8bd179 108 for ( g = 0; g <(uint8)3; g++ ) {
gke 0:62a1c91a859a 109 GyroNeutral[g] *= 0.03125;
gke 2:90292f8bd179 110 Gyro[g] = Gyrop[g] = 0.0;
gke 2:90292f8bd179 111 for ( s = 0; s < MaxAttitudeScheme; s++ )
gke 2:90292f8bd179 112 EstAngle[g][s] = EstRate[g][s] = 0.0;
gke 0:62a1c91a859a 113 }
gke 0:62a1c91a859a 114
gke 0:62a1c91a859a 115 LEDRed_OFF;
gke 0:62a1c91a859a 116
gke 0:62a1c91a859a 117 } // ErectGyros
gke 0:62a1c91a859a 118
gke 0:62a1c91a859a 119 void GyroTest(void) {
gke 0:62a1c91a859a 120 TxString("\r\nGyro Test - ");
gke 0:62a1c91a859a 121 ShowGyroType();
gke 0:62a1c91a859a 122
gke 0:62a1c91a859a 123 ReadGyros();
gke 0:62a1c91a859a 124
gke 2:90292f8bd179 125 TxString("\r\n\tRate and Max Delta(Deg./Sec.)\r\n");
gke 2:90292f8bd179 126
gke 0:62a1c91a859a 127 TxString("\r\n\tRoll: \t");
gke 2:90292f8bd179 128 TxVal32(Gyro[Roll] * MILLIANGLE, 3, HT);
gke 2:90292f8bd179 129 TxVal32(GyroNoise[Roll] * MILLIANGLE, 3, 0);
gke 0:62a1c91a859a 130 TxNextLine();
gke 0:62a1c91a859a 131 TxString("\tPitch: \t");
gke 2:90292f8bd179 132 TxVal32(Gyro[Pitch] * MILLIANGLE, 3, HT);
gke 2:90292f8bd179 133 TxVal32(GyroNoise[Pitch] * MILLIANGLE, 3, 0);
gke 0:62a1c91a859a 134 TxNextLine();
gke 0:62a1c91a859a 135 TxString("\tYaw: \t");
gke 2:90292f8bd179 136 TxVal32(Gyro[Yaw] * MILLIANGLE, 3, HT);
gke 2:90292f8bd179 137 TxVal32(GyroNoise[Yaw] * MILLIANGLE, 3, 0);
gke 0:62a1c91a859a 138 TxNextLine();
gke 0:62a1c91a859a 139
gke 0:62a1c91a859a 140 switch ( GyroType ) {
gke 0:62a1c91a859a 141 case ITG3200Gyro:
gke 0:62a1c91a859a 142 ITG3200Test();
gke 0:62a1c91a859a 143 break;
gke 0:62a1c91a859a 144 default:
gke 0:62a1c91a859a 145 break;
gke 0:62a1c91a859a 146 } // switch
gke 0:62a1c91a859a 147
gke 0:62a1c91a859a 148 if ( F.GyroFailure )
gke 0:62a1c91a859a 149 TxString("\r\nFAILED\r\n");
gke 0:62a1c91a859a 150
gke 0:62a1c91a859a 151 } // GyroTest
gke 0:62a1c91a859a 152
gke 0:62a1c91a859a 153 void InitGyros(void) {
gke 2:90292f8bd179 154
gke 0:62a1c91a859a 155 if ( ITG3200GyroActive() )
gke 0:62a1c91a859a 156 GyroType = ITG3200Gyro;
gke 0:62a1c91a859a 157 else
gke 0:62a1c91a859a 158 GyroType = P[GyroRollPitchType];
gke 0:62a1c91a859a 159
gke 0:62a1c91a859a 160 switch ( GyroType ) {
gke 0:62a1c91a859a 161 case ITG3200Gyro:
gke 0:62a1c91a859a 162 InitITG3200Gyro();
gke 0:62a1c91a859a 163 break;
gke 0:62a1c91a859a 164 case IRSensors:
gke 0:62a1c91a859a 165 InitIRSensors();
gke 0:62a1c91a859a 166 default:
gke 0:62a1c91a859a 167 InitAnalogGyros();
gke 0:62a1c91a859a 168 break;
gke 0:62a1c91a859a 169 } // switch
gke 0:62a1c91a859a 170
gke 0:62a1c91a859a 171 Delay1mS(50);
gke 0:62a1c91a859a 172 ErectGyros();
gke 0:62a1c91a859a 173
gke 0:62a1c91a859a 174 } // InitGyros
gke 0:62a1c91a859a 175
gke 0:62a1c91a859a 176 void ShowGyroType(void) {
gke 0:62a1c91a859a 177 switch ( GyroType ) {
gke 0:62a1c91a859a 178 case MLX90609Gyro:
gke 0:62a1c91a859a 179 TxString("MLX90609");
gke 0:62a1c91a859a 180 break;
gke 0:62a1c91a859a 181 case ADXRS150Gyro:
gke 0:62a1c91a859a 182 TxString("ADXRS613/150");
gke 0:62a1c91a859a 183 break;
gke 0:62a1c91a859a 184 case IDG300Gyro:
gke 0:62a1c91a859a 185 TxString("IDG300");
gke 0:62a1c91a859a 186 break;
gke 0:62a1c91a859a 187 case LY530Gyro:
gke 0:62a1c91a859a 188 TxString("ST-AY530");
gke 0:62a1c91a859a 189 break;
gke 0:62a1c91a859a 190 case ADXRS300Gyro:
gke 0:62a1c91a859a 191 TxString("ADXRS610/300");
gke 0:62a1c91a859a 192 break;
gke 0:62a1c91a859a 193 case ITG3200Gyro:
gke 0:62a1c91a859a 194 TxString("ITG3200");
gke 0:62a1c91a859a 195 break;
gke 0:62a1c91a859a 196 case IRSensors:
gke 0:62a1c91a859a 197 TxString("IR Sensors");
gke 0:62a1c91a859a 198 break;
gke 0:62a1c91a859a 199 default:
gke 0:62a1c91a859a 200 TxString("unknown");
gke 0:62a1c91a859a 201 break;
gke 0:62a1c91a859a 202 } // switch
gke 0:62a1c91a859a 203 } // ShowGyroType
gke 0:62a1c91a859a 204
gke 0:62a1c91a859a 205 //________________________________________________________________________________________
gke 0:62a1c91a859a 206
gke 0:62a1c91a859a 207 // Analog Gyros
gke 0:62a1c91a859a 208
gke 0:62a1c91a859a 209 void ReadAnalogGyros(void);
gke 0:62a1c91a859a 210 void InitAnalogGyros(void);
gke 0:62a1c91a859a 211 void CheckAnalogGyroFault(uint8, uint8, uint8);
gke 0:62a1c91a859a 212 void AnalogGyroTest(void);
gke 0:62a1c91a859a 213
gke 0:62a1c91a859a 214 void ReadAnalogGyros(void) {
gke 0:62a1c91a859a 215 static uint8 g;
gke 0:62a1c91a859a 216
gke 2:90292f8bd179 217 GyroADC[Roll] = -RollADC.read();
gke 2:90292f8bd179 218 GyroADC[Pitch] = -PitchADC.read();
gke 0:62a1c91a859a 219 GyroADC[Yaw] = YawADC.read();
gke 0:62a1c91a859a 220
gke 0:62a1c91a859a 221 for ( g = 0; g < (uint8)3; g++ )
gke 2:90292f8bd179 222 Gyro[g] = GyroADC[g] * GyroToRadian[GyroType];
gke 2:90292f8bd179 223
gke 0:62a1c91a859a 224 } // ReadAnalogGyros
gke 0:62a1c91a859a 225
gke 0:62a1c91a859a 226 void InitAnalogGyros(void) {
gke 0:62a1c91a859a 227 // nothing to do
gke 0:62a1c91a859a 228 F.GyroFailure = false;
gke 0:62a1c91a859a 229 } // InitAnalogGyros
gke 0:62a1c91a859a 230
gke 0:62a1c91a859a 231 //________________________________________________________________________________________
gke 0:62a1c91a859a 232
gke 0:62a1c91a859a 233 // ITG3200 3-axis I2C Gyro
gke 0:62a1c91a859a 234
gke 0:62a1c91a859a 235 void ReadITG3200(void);
gke 0:62a1c91a859a 236 uint8 ReadByteITG3200(uint8);
gke 2:90292f8bd179 237 boolean WriteByteITG3200(uint8, uint8);
gke 0:62a1c91a859a 238 void InitITG3200(void);
gke 0:62a1c91a859a 239 void ITG3200Test(void);
gke 0:62a1c91a859a 240 boolean ITG3200Active(void);
gke 0:62a1c91a859a 241
gke 0:62a1c91a859a 242 real32 ITG3200Temperature;
gke 0:62a1c91a859a 243
gke 0:62a1c91a859a 244 void ReadITG3200Gyro(void) {
gke 0:62a1c91a859a 245 static char G[6];
gke 2:90292f8bd179 246 static uint8 g;
gke 0:62a1c91a859a 247 static i16u GX, GY, GZ;
gke 0:62a1c91a859a 248
gke 0:62a1c91a859a 249 I2CGYRO.start();
gke 2:90292f8bd179 250 if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto ITG3200Error;
gke 2:90292f8bd179 251 if ( I2CGYRO.write(ITG3200_GX_H) != I2C_ACK ) goto ITG3200Error;
gke 0:62a1c91a859a 252 I2CGYRO.stop();
gke 0:62a1c91a859a 253
gke 2:90292f8bd179 254 if ( I2CGYRO.blockread(ITG3200_ID, G, 6) ) goto ITG3200Error;
gke 0:62a1c91a859a 255
gke 2:90292f8bd179 256 GX.b0 = G[1];
gke 2:90292f8bd179 257 GX.b1 = G[0];
gke 2:90292f8bd179 258 GY.b0 = G[3];
gke 2:90292f8bd179 259 GY.b1 = G[2];
gke 2:90292f8bd179 260 GZ.b0 = G[5];
gke 2:90292f8bd179 261 GZ.b1 = G[4];
gke 0:62a1c91a859a 262
gke 2:90292f8bd179 263 if ( F.Using9DOF ) { // SparkFun/QuadroUFO breakout pins forward components up
gke 2:90292f8bd179 264 GyroADC[Roll] = -(real32)GY.i16;
gke 2:90292f8bd179 265 GyroADC[Pitch] = -(real32)GX.i16;
gke 2:90292f8bd179 266 GyroADC[Yaw] = -(real32)GZ.i16;
gke 2:90292f8bd179 267 } else { // SparkFun 6DOF breakout pins forward components down
gke 2:90292f8bd179 268 GyroADC[Roll] = -(real32)GX.i16;
gke 2:90292f8bd179 269 GyroADC[Pitch] = -(real32)GY.i16;
gke 2:90292f8bd179 270 GyroADC[Yaw] = (real32)GZ.i16;
gke 2:90292f8bd179 271 }
gke 1:1e3318a30ddd 272
gke 2:90292f8bd179 273 for ( g = 0; g < (uint8)3; g++ )
gke 2:90292f8bd179 274 Gyro[g] = GyroADC[g] * GyroToRadian[ITG3200Gyro];
gke 2:90292f8bd179 275
gke 2:90292f8bd179 276 return;
gke 1:1e3318a30ddd 277
gke 2:90292f8bd179 278 ITG3200Error:
gke 2:90292f8bd179 279 I2CGYRO.stop();
gke 2:90292f8bd179 280
gke 2:90292f8bd179 281 I2CError[ITG3200_ID]++;
gke 2:90292f8bd179 282
gke 2:90292f8bd179 283 Stats[GyroFailS]++; // not in flight keep trying
gke 2:90292f8bd179 284 F.GyroFailure = true;
gke 0:62a1c91a859a 285
gke 0:62a1c91a859a 286 } // ReadITG3200Gyro
gke 0:62a1c91a859a 287
gke 0:62a1c91a859a 288 uint8 ReadByteITG3200(uint8 a) {
gke 0:62a1c91a859a 289 static uint8 d;
gke 0:62a1c91a859a 290
gke 0:62a1c91a859a 291 I2CGYRO.start();
gke 2:90292f8bd179 292 if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto ITG3200Error;
gke 2:90292f8bd179 293 if ( I2CGYRO.write(a) != I2C_ACK ) goto ITG3200Error;
gke 0:62a1c91a859a 294 I2CGYRO.start();
gke 2:90292f8bd179 295 if ( I2CGYRO.write(ITG3200_RD) != I2C_ACK ) goto ITG3200Error;
gke 0:62a1c91a859a 296 d = I2CGYRO.read(I2C_NACK);
gke 0:62a1c91a859a 297 I2CGYRO.stop();
gke 0:62a1c91a859a 298
gke 0:62a1c91a859a 299 return ( d );
gke 0:62a1c91a859a 300
gke 2:90292f8bd179 301 ITG3200Error:
gke 0:62a1c91a859a 302 I2CGYRO.stop();
gke 2:90292f8bd179 303
gke 2:90292f8bd179 304 I2CError[ITG3200_ID]++;
gke 2:90292f8bd179 305 // GYRO FAILURE - FATAL
gke 2:90292f8bd179 306 Stats[GyroFailS]++;
gke 2:90292f8bd179 307
gke 2:90292f8bd179 308 //F.GyroFailure = true;
gke 2:90292f8bd179 309
gke 2:90292f8bd179 310 return ( 0 );
gke 2:90292f8bd179 311
gke 2:90292f8bd179 312 } // ReadByteITG3200
gke 2:90292f8bd179 313
gke 2:90292f8bd179 314 boolean WriteByteITG3200(uint8 a, uint8 d) {
gke 2:90292f8bd179 315
gke 2:90292f8bd179 316 I2CGYRO.start(); // restart
gke 2:90292f8bd179 317 if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto ITG3200Error;
gke 2:90292f8bd179 318 if ( I2CGYRO.write(a) != I2C_ACK ) goto ITG3200Error;
gke 2:90292f8bd179 319 if ( I2CGYRO.write(d) != I2C_ACK ) goto ITG3200Error;
gke 2:90292f8bd179 320 I2CGYRO.stop();
gke 2:90292f8bd179 321
gke 2:90292f8bd179 322 return(false);
gke 2:90292f8bd179 323
gke 2:90292f8bd179 324 ITG3200Error:
gke 2:90292f8bd179 325 I2CGYRO.stop();
gke 2:90292f8bd179 326
gke 2:90292f8bd179 327 I2CError[ITG3200_ID]++;
gke 0:62a1c91a859a 328 // GYRO FAILURE - FATAL
gke 0:62a1c91a859a 329 Stats[GyroFailS]++;
gke 0:62a1c91a859a 330 F.GyroFailure = true;
gke 0:62a1c91a859a 331
gke 2:90292f8bd179 332 return(true);
gke 0:62a1c91a859a 333
gke 0:62a1c91a859a 334 } // WriteByteITG3200
gke 0:62a1c91a859a 335
gke 0:62a1c91a859a 336 void InitITG3200Gyro(void) {
gke 2:90292f8bd179 337
gke 2:90292f8bd179 338 #define FS_SEL 3
gke 0:62a1c91a859a 339
gke 2:90292f8bd179 340 //#define DLPF_CFG 1 // 188HZ
gke 2:90292f8bd179 341 #define DLPF_CFG 2 // 98HZ
gke 2:90292f8bd179 342 //#define DLPF_CFG 3 // 42HZ
gke 2:90292f8bd179 343
gke 2:90292f8bd179 344 if ( WriteByteITG3200(ITG3200_PWR_M, 0x80) ) goto ITG3200Error; // Reset to defaults
gke 2:90292f8bd179 345 if ( WriteByteITG3200(ITG3200_SMPL, 0x00) ) goto ITG3200Error; // continuous update
gke 2:90292f8bd179 346 if ( WriteByteITG3200(ITG3200_DLPF, (FS_SEL << 3) | DLPF_CFG ) ) goto ITG3200Error; // 188Hz, 2000deg/S
gke 2:90292f8bd179 347 if ( WriteByteITG3200(ITG3200_INT_C, 0x00) ) goto ITG3200Error; // no interrupts
gke 2:90292f8bd179 348 if ( WriteByteITG3200(ITG3200_PWR_M, 0x01) ) goto ITG3200Error; // X Gyro as Clock Ref.
gke 0:62a1c91a859a 349
gke 0:62a1c91a859a 350 Delay1mS(50);
gke 0:62a1c91a859a 351
gke 2:90292f8bd179 352 F.GyroFailure = false;
gke 2:90292f8bd179 353
gke 0:62a1c91a859a 354 ReadITG3200Gyro();
gke 0:62a1c91a859a 355
gke 2:90292f8bd179 356 return;
gke 2:90292f8bd179 357
gke 2:90292f8bd179 358 ITG3200Error:
gke 2:90292f8bd179 359
gke 2:90292f8bd179 360 F.GyroFailure = true;
gke 2:90292f8bd179 361
gke 0:62a1c91a859a 362 } // InitITG3200Gyro
gke 0:62a1c91a859a 363
gke 0:62a1c91a859a 364 void ITG3200Test(void) {
gke 0:62a1c91a859a 365 static uint8 MyID, SMPLRT_DIV, DLPF_FS, PWR_MGM;
gke 0:62a1c91a859a 366 static int16 TEMP,GYRO_X, GYRO_Y, GYRO_Z;
gke 0:62a1c91a859a 367
gke 0:62a1c91a859a 368 MyID = ReadByteITG3200(ITG3200_WHO);
gke 0:62a1c91a859a 369
gke 0:62a1c91a859a 370 TxString("\tWHO_AM_I \t0x");
gke 0:62a1c91a859a 371 TxValH(MyID);
gke 0:62a1c91a859a 372 TxNextLine();
gke 0:62a1c91a859a 373 Delay1mS(1);
gke 0:62a1c91a859a 374 SMPLRT_DIV = ReadByteITG3200(ITG3200_SMPL);
gke 0:62a1c91a859a 375 DLPF_FS = ReadByteITG3200(ITG3200_DLPF);
gke 0:62a1c91a859a 376 TEMP = (int16)ReadByteITG3200(ITG3200_TMP_H)<<8 | ReadByteITG3200(ITG3200_TMP_L);
gke 0:62a1c91a859a 377 GYRO_X = (int16)ReadByteITG3200(ITG3200_GX_H)<<8 | ReadByteITG3200(ITG3200_GX_L);
gke 0:62a1c91a859a 378 GYRO_Y = (int16)ReadByteITG3200(ITG3200_GY_H)<<8 | ReadByteITG3200(ITG3200_GY_L);
gke 0:62a1c91a859a 379 GYRO_Z = (int16)ReadByteITG3200(ITG3200_GZ_H)<<8 | ReadByteITG3200(ITG3200_GZ_L);
gke 0:62a1c91a859a 380 PWR_MGM = ReadByteITG3200(ITG3200_PWR_M);
gke 0:62a1c91a859a 381
gke 0:62a1c91a859a 382 ITG3200Temperature = 35.0 + ((TEMP + 13200.0 ) / 280.0);
gke 0:62a1c91a859a 383
gke 0:62a1c91a859a 384 TxString("\tSMPLRT_DIV\t");
gke 0:62a1c91a859a 385 TxVal32( SMPLRT_DIV,0,0);
gke 0:62a1c91a859a 386 TxNextLine();
gke 0:62a1c91a859a 387 TxString("\tDLPF \t");
gke 0:62a1c91a859a 388 TxVal32( DLPF_FS & 7,0,0 );
gke 0:62a1c91a859a 389 TxString(" FS \t");
gke 0:62a1c91a859a 390 TxVal32( (DLPF_FS>>3)&3, 0, 0);
gke 0:62a1c91a859a 391 TxNextLine();
gke 0:62a1c91a859a 392 TxString("\tTEMP \t");
gke 0:62a1c91a859a 393 TxVal32( TEMP, 0, 0);
gke 0:62a1c91a859a 394 TxNextLine();
gke 0:62a1c91a859a 395 TxString("\tGYRO_X \t");
gke 0:62a1c91a859a 396 TxVal32( GYRO_X, 0, 0);
gke 0:62a1c91a859a 397 TxNextLine();
gke 0:62a1c91a859a 398 TxString("\tGYRO_Y \t");
gke 0:62a1c91a859a 399 TxVal32( GYRO_Y, 0, 0);
gke 0:62a1c91a859a 400 TxNextLine();
gke 0:62a1c91a859a 401 TxString("\tGYRO_Z \t");
gke 0:62a1c91a859a 402 TxVal32( GYRO_Z, 0, 0);
gke 0:62a1c91a859a 403 TxNextLine();
gke 0:62a1c91a859a 404 TxString("\tPWR_MGM \t0x");
gke 0:62a1c91a859a 405 TxValH( PWR_MGM );
gke 0:62a1c91a859a 406 TxNextLine();
gke 0:62a1c91a859a 407
gke 0:62a1c91a859a 408 TxNextLine();
gke 0:62a1c91a859a 409
gke 0:62a1c91a859a 410 } // ITG3200Test
gke 0:62a1c91a859a 411
gke 0:62a1c91a859a 412 boolean ITG3200GyroActive(void) {
gke 1:1e3318a30ddd 413
gke 1:1e3318a30ddd 414 F.GyroFailure = !I2CGYROAddressResponds( ITG3200_ID );
gke 1:1e3318a30ddd 415
gke 0:62a1c91a859a 416 if ( !F.GyroFailure )
gke 0:62a1c91a859a 417 TrackMinI2CRate(400000);
gke 0:62a1c91a859a 418
gke 0:62a1c91a859a 419 return ( !F.GyroFailure );
gke 0:62a1c91a859a 420
gke 0:62a1c91a859a 421 } // ITG3200GyroActive