Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
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?
User | Revision | Line number | New 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 |