Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
accel.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?
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 | 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 | |
gke | 0:62a1c91a859a | 23 | void ShowAccType(void); |
gke | 0:62a1c91a859a | 24 | void ReadAccelerometers(void); |
gke | 0:62a1c91a859a | 25 | void GetAccelerations(void); |
gke | 0:62a1c91a859a | 26 | void GetNeutralAccelerations(void); |
gke | 0:62a1c91a859a | 27 | void AccelerometerTest(void); |
gke | 0:62a1c91a859a | 28 | void InitAccelerometers(void); |
gke | 0:62a1c91a859a | 29 | |
gke | 0:62a1c91a859a | 30 | real32 Vel[3], Acc[3], AccNeutral[3], Accp[3]; |
gke | 0:62a1c91a859a | 31 | int16 NewAccNeutral[3]; |
gke | 0:62a1c91a859a | 32 | uint8 AccelerometerType; |
gke | 0:62a1c91a859a | 33 | |
gke | 0:62a1c91a859a | 34 | void ShowAccType(void) { |
gke | 0:62a1c91a859a | 35 | switch ( AccelerometerType ) { |
gke | 0:62a1c91a859a | 36 | case LISLAcc: |
gke | 0:62a1c91a859a | 37 | TxString("LIS3L"); |
gke | 0:62a1c91a859a | 38 | break; |
gke | 0:62a1c91a859a | 39 | case ADXL345Acc: |
gke | 0:62a1c91a859a | 40 | TxString("ADXL345"); |
gke | 0:62a1c91a859a | 41 | break; |
gke | 0:62a1c91a859a | 42 | case AccUnknown: |
gke | 0:62a1c91a859a | 43 | TxString("unknown"); |
gke | 0:62a1c91a859a | 44 | break; |
gke | 0:62a1c91a859a | 45 | default: |
gke | 0:62a1c91a859a | 46 | ; |
gke | 0:62a1c91a859a | 47 | } // switch |
gke | 0:62a1c91a859a | 48 | } // ShowAccType |
gke | 0:62a1c91a859a | 49 | |
gke | 0:62a1c91a859a | 50 | void ReadAccelerometers(void) { |
gke | 0:62a1c91a859a | 51 | switch ( AccelerometerType ) { |
gke | 0:62a1c91a859a | 52 | case LISLAcc: |
gke | 0:62a1c91a859a | 53 | ReadLISLAcc(); |
gke | 0:62a1c91a859a | 54 | break; |
gke | 0:62a1c91a859a | 55 | case ADXL345Acc: |
gke | 0:62a1c91a859a | 56 | ReadADXL345Acc(); |
gke | 0:62a1c91a859a | 57 | break; |
gke | 0:62a1c91a859a | 58 | // other accelerometers |
gke | 0:62a1c91a859a | 59 | default: |
gke | 0:62a1c91a859a | 60 | Acc[BF] = Acc[LR] = 0.0; |
gke | 0:62a1c91a859a | 61 | Acc[UD] = 1.0; |
gke | 0:62a1c91a859a | 62 | break; |
gke | 0:62a1c91a859a | 63 | } // switch |
gke | 0:62a1c91a859a | 64 | } // ReadAccelerometers |
gke | 0:62a1c91a859a | 65 | |
gke | 0:62a1c91a859a | 66 | void GetNeutralAccelerations(void) { |
gke | 0:62a1c91a859a | 67 | static uint8 i, a; |
gke | 0:62a1c91a859a | 68 | static real32 Temp[3] = {0.0, 0.0, 0.0}; |
gke | 0:62a1c91a859a | 69 | |
gke | 0:62a1c91a859a | 70 | if ( F.AccelerationsValid ) { |
gke | 0:62a1c91a859a | 71 | for ( i = 16; i; i--) { |
gke | 0:62a1c91a859a | 72 | ReadAccelerometers(); |
gke | 0:62a1c91a859a | 73 | for ( a = 0; a <(uint8)3; a++ ) |
gke | 0:62a1c91a859a | 74 | Temp[a] += Acc[a]; |
gke | 0:62a1c91a859a | 75 | } |
gke | 0:62a1c91a859a | 76 | |
gke | 0:62a1c91a859a | 77 | for ( a = 0; a <(uint8)3; a++ ) |
gke | 0:62a1c91a859a | 78 | Temp[a] = Temp[a] * 0.0625; |
gke | 0:62a1c91a859a | 79 | |
gke | 0:62a1c91a859a | 80 | NewAccNeutral[BF] = Limit((int16)(Temp[BF] * 1000.0 ), -99, 99); |
gke | 0:62a1c91a859a | 81 | NewAccNeutral[LR] = Limit( (int16)(Temp[LR] * 1000.0 ), -99, 99); |
gke | 0:62a1c91a859a | 82 | NewAccNeutral[UD] = Limit( (int16)(( Temp[UD] - 1.0 ) * 1000.0) , -99, 99); |
gke | 0:62a1c91a859a | 83 | |
gke | 0:62a1c91a859a | 84 | } else |
gke | 0:62a1c91a859a | 85 | for ( a = 0; a <(uint8)3; a++ ) |
gke | 0:62a1c91a859a | 86 | AccNeutral[a] = 0.0; |
gke | 0:62a1c91a859a | 87 | |
gke | 0:62a1c91a859a | 88 | } // GetNeutralAccelerations |
gke | 0:62a1c91a859a | 89 | |
gke | 0:62a1c91a859a | 90 | void GetAccelerations(void) { |
gke | 0:62a1c91a859a | 91 | |
gke | 0:62a1c91a859a | 92 | static real32 AccA; |
gke | 0:62a1c91a859a | 93 | static uint8 a; |
gke | 0:62a1c91a859a | 94 | |
gke | 0:62a1c91a859a | 95 | if ( F.AccelerationsValid ) { |
gke | 0:62a1c91a859a | 96 | ReadAccelerometers(); |
gke | 0:62a1c91a859a | 97 | |
gke | 0:62a1c91a859a | 98 | // Neutral[ {LR, BF, UD} ] pass through UAVPSet |
gke | 0:62a1c91a859a | 99 | // and come back as AccMiddle[LR] etc. |
gke | 0:62a1c91a859a | 100 | |
gke | 0:62a1c91a859a | 101 | Acc[BF] -= K[MiddleBF]; |
gke | 0:62a1c91a859a | 102 | Acc[LR] -= K[MiddleLR]; |
gke | 0:62a1c91a859a | 103 | Acc[UD] -= K[MiddleUD]; |
gke | 0:62a1c91a859a | 104 | |
gke | 0:62a1c91a859a | 105 | AccA = dT / ( OneOnTwoPiAccF + dT ); |
gke | 0:62a1c91a859a | 106 | for ( a = 0; a < (uint8)3; a++ ) { |
gke | 0:62a1c91a859a | 107 | Acc[a] = Accp[a] + (Acc[a] - Accp[a]) * AccA; |
gke | 0:62a1c91a859a | 108 | Accp[a] = Acc[a]; |
gke | 0:62a1c91a859a | 109 | } |
gke | 0:62a1c91a859a | 110 | |
gke | 0:62a1c91a859a | 111 | } else { |
gke | 0:62a1c91a859a | 112 | Acc[LR] = Acc[BF] = 0; |
gke | 0:62a1c91a859a | 113 | Acc[UD] = 1.0; |
gke | 0:62a1c91a859a | 114 | } |
gke | 0:62a1c91a859a | 115 | } // GetAccelerations |
gke | 0:62a1c91a859a | 116 | |
gke | 0:62a1c91a859a | 117 | void AccelerometerTest(void) { |
gke | 0:62a1c91a859a | 118 | TxString("\r\nAccelerometer test -"); |
gke | 0:62a1c91a859a | 119 | ShowAccType(); |
gke | 0:62a1c91a859a | 120 | TxString("\r\nRead once - no averaging\r\n"); |
gke | 0:62a1c91a859a | 121 | |
gke | 0:62a1c91a859a | 122 | InitAccelerometers(); |
gke | 0:62a1c91a859a | 123 | |
gke | 0:62a1c91a859a | 124 | if ( F.AccelerationsValid ) { |
gke | 0:62a1c91a859a | 125 | ReadAccelerometers(); |
gke | 0:62a1c91a859a | 126 | |
gke | 0:62a1c91a859a | 127 | TxString("\tL->R: \t"); |
gke | 0:62a1c91a859a | 128 | TxVal32( Acc[LR] * 1000.0, 3, 'G'); |
gke | 0:62a1c91a859a | 129 | if ( fabs(Acc[LR]) > 0.2 ) |
gke | 0:62a1c91a859a | 130 | TxString(" fault?"); |
gke | 0:62a1c91a859a | 131 | TxNextLine(); |
gke | 0:62a1c91a859a | 132 | |
gke | 0:62a1c91a859a | 133 | TxString("\tB->F: \t"); |
gke | 0:62a1c91a859a | 134 | TxVal32( Acc[BF] * 1000.0, 3, 'G'); |
gke | 0:62a1c91a859a | 135 | if ( fabs(Acc[BF]) > 0.2 ) |
gke | 0:62a1c91a859a | 136 | TxString(" fault?"); |
gke | 0:62a1c91a859a | 137 | TxNextLine(); |
gke | 0:62a1c91a859a | 138 | |
gke | 0:62a1c91a859a | 139 | |
gke | 0:62a1c91a859a | 140 | TxString("\tU->D: \t"); |
gke | 0:62a1c91a859a | 141 | TxVal32( Acc[UD] * 1000.0, 3, 'G'); |
gke | 0:62a1c91a859a | 142 | if ( fabs(Acc[UD]) > 1.2 ) |
gke | 0:62a1c91a859a | 143 | TxString(" fault?"); |
gke | 0:62a1c91a859a | 144 | TxNextLine(); |
gke | 0:62a1c91a859a | 145 | } |
gke | 0:62a1c91a859a | 146 | } // AccelerometerTest |
gke | 0:62a1c91a859a | 147 | |
gke | 0:62a1c91a859a | 148 | void InitAccelerometers(void) { |
gke | 0:62a1c91a859a | 149 | static uint8 a; |
gke | 0:62a1c91a859a | 150 | |
gke | 0:62a1c91a859a | 151 | F.AccelerationsValid = true; // optimistic |
gke | 0:62a1c91a859a | 152 | |
gke | 0:62a1c91a859a | 153 | for ( a = 0; a < (uint8)3; a++ ) { |
gke | 0:62a1c91a859a | 154 | NewAccNeutral[a] = Acc[a] = Accp[a] = Vel[a] = 0.0; |
gke | 0:62a1c91a859a | 155 | Comp[a] = 0; |
gke | 0:62a1c91a859a | 156 | } |
gke | 0:62a1c91a859a | 157 | Acc[2] = Accp[2] = 1.0; |
gke | 0:62a1c91a859a | 158 | |
gke | 0:62a1c91a859a | 159 | if ( ADXL345AccActive() ) { |
gke | 0:62a1c91a859a | 160 | InitADXL345Acc(); |
gke | 0:62a1c91a859a | 161 | AccelerometerType = ADXL345Acc; |
gke | 0:62a1c91a859a | 162 | |
gke | 0:62a1c91a859a | 163 | } else |
gke | 0:62a1c91a859a | 164 | if ( LISLAccActive() ) |
gke | 0:62a1c91a859a | 165 | AccelerometerType = LISLAcc; |
gke | 0:62a1c91a859a | 166 | else |
gke | 0:62a1c91a859a | 167 | // check for other accs in preferred order |
gke | 0:62a1c91a859a | 168 | { |
gke | 0:62a1c91a859a | 169 | AccelerometerType = AccUnknown; |
gke | 0:62a1c91a859a | 170 | F.AccelerationsValid = false; |
gke | 0:62a1c91a859a | 171 | } |
gke | 0:62a1c91a859a | 172 | |
gke | 0:62a1c91a859a | 173 | if ( F.AccelerationsValid ) { |
gke | 0:62a1c91a859a | 174 | LEDYellow_ON; |
gke | 0:62a1c91a859a | 175 | GetNeutralAccelerations(); |
gke | 0:62a1c91a859a | 176 | LEDYellow_OFF; |
gke | 0:62a1c91a859a | 177 | } else |
gke | 0:62a1c91a859a | 178 | F.AccFailure = true; |
gke | 0:62a1c91a859a | 179 | } // InitAccelerometers |
gke | 0:62a1c91a859a | 180 | |
gke | 0:62a1c91a859a | 181 | //________________________________________________________________________________________ |
gke | 0:62a1c91a859a | 182 | |
gke | 0:62a1c91a859a | 183 | // ADXL345 3 Axis Accelerometer |
gke | 0:62a1c91a859a | 184 | |
gke | 0:62a1c91a859a | 185 | void ReadADXL345Acc(void); |
gke | 0:62a1c91a859a | 186 | void InitADXL345Acc(void); |
gke | 0:62a1c91a859a | 187 | boolean ADXL345AccActive(void); |
gke | 0:62a1c91a859a | 188 | |
gke | 0:62a1c91a859a | 189 | const float GRAVITY_ADXL345 = 250.0; // ~4mG/LSB |
gke | 0:62a1c91a859a | 190 | |
gke | 0:62a1c91a859a | 191 | void ReadADXL345Acc(void) { |
gke | 0:62a1c91a859a | 192 | |
gke | 0:62a1c91a859a | 193 | static uint8 a, r; |
gke | 0:62a1c91a859a | 194 | static char b[6]; |
gke | 0:62a1c91a859a | 195 | static i16u X, Y, Z; |
gke | 0:62a1c91a859a | 196 | |
gke | 0:62a1c91a859a | 197 | /* |
gke | 0:62a1c91a859a | 198 | r = 0; |
gke | 0:62a1c91a859a | 199 | while ( r == 0 ) { |
gke | 0:62a1c91a859a | 200 | I2CACC.start(); |
gke | 0:62a1c91a859a | 201 | r = I2CACC.write(ADXL345_ID); |
gke | 0:62a1c91a859a | 202 | r = I2CACC.write(0x30); |
gke | 0:62a1c91a859a | 203 | r = I2CACC.read(true) & 0x80; |
gke | 0:62a1c91a859a | 204 | I2CACC.stop(); |
gke | 0:62a1c91a859a | 205 | } |
gke | 0:62a1c91a859a | 206 | */ |
gke | 0:62a1c91a859a | 207 | |
gke | 0:62a1c91a859a | 208 | I2CACC.start(); |
gke | 0:62a1c91a859a | 209 | r = I2CACC.write(ADXL345_ID); |
gke | 0:62a1c91a859a | 210 | r = I2CACC.write(0x32); // point to acc data |
gke | 0:62a1c91a859a | 211 | I2CACC.stop(); |
gke | 0:62a1c91a859a | 212 | |
gke | 0:62a1c91a859a | 213 | I2CACC.read(ADXL345_ID, b, 6); |
gke | 0:62a1c91a859a | 214 | |
gke | 0:62a1c91a859a | 215 | X.b1 = b[1]; |
gke | 0:62a1c91a859a | 216 | X.b0 = b[0]; |
gke | 0:62a1c91a859a | 217 | Y.b1 = b[3]; |
gke | 0:62a1c91a859a | 218 | Y.b0 =b[2]; |
gke | 0:62a1c91a859a | 219 | Z.b1 = b[5]; |
gke | 0:62a1c91a859a | 220 | Z.b0 = b[4]; |
gke | 0:62a1c91a859a | 221 | |
gke | 0:62a1c91a859a | 222 | if ( F.Using9DOF ) { // SparkFun/QuadroUFO 9DOF breakouts pins forward components up |
gke | 0:62a1c91a859a | 223 | Acc[BF] = -Y.i16; |
gke | 0:62a1c91a859a | 224 | Acc[LR] = -X.i16; |
gke | 0:62a1c91a859a | 225 | Acc[UD] = -Z.i16; |
gke | 0:62a1c91a859a | 226 | } else {// SparkFun 6DOF breakouts pins forward components down |
gke | 0:62a1c91a859a | 227 | Acc[BF] = -X.i16; |
gke | 0:62a1c91a859a | 228 | Acc[LR] = -Y.i16; |
gke | 0:62a1c91a859a | 229 | Acc[UD] = -Z.i16; |
gke | 0:62a1c91a859a | 230 | } |
gke | 0:62a1c91a859a | 231 | |
gke | 0:62a1c91a859a | 232 | // LP filter here? |
gke | 0:62a1c91a859a | 233 | |
gke | 0:62a1c91a859a | 234 | for ( a = 0; a < (int8)3; a++ ) |
gke | 0:62a1c91a859a | 235 | Acc[a] /= GRAVITY_ADXL345; |
gke | 0:62a1c91a859a | 236 | |
gke | 0:62a1c91a859a | 237 | } // ReadADXL345Acc |
gke | 0:62a1c91a859a | 238 | |
gke | 0:62a1c91a859a | 239 | void InitADXL345Acc() { |
gke | 0:62a1c91a859a | 240 | static uint8 r; |
gke | 0:62a1c91a859a | 241 | |
gke | 0:62a1c91a859a | 242 | I2CACC.start(); |
gke | 0:62a1c91a859a | 243 | I2CACC.write(ADXL345_W); |
gke | 0:62a1c91a859a | 244 | r = I2CACC.write(0x2D); // power register |
gke | 0:62a1c91a859a | 245 | r = I2CACC.write(0x08); // measurement mode |
gke | 0:62a1c91a859a | 246 | I2CACC.stop(); |
gke | 0:62a1c91a859a | 247 | |
gke | 0:62a1c91a859a | 248 | Delay1mS(5); |
gke | 0:62a1c91a859a | 249 | |
gke | 0:62a1c91a859a | 250 | I2CACC.start(); |
gke | 0:62a1c91a859a | 251 | r = I2CACC.write(ADXL345_W); |
gke | 0:62a1c91a859a | 252 | r = I2CACC.write(0x31); // format |
gke | 0:62a1c91a859a | 253 | r = I2CACC.write(0x08); // full resolution, 2g |
gke | 0:62a1c91a859a | 254 | I2CACC.stop(); |
gke | 0:62a1c91a859a | 255 | |
gke | 0:62a1c91a859a | 256 | Delay1mS(5); |
gke | 0:62a1c91a859a | 257 | |
gke | 0:62a1c91a859a | 258 | I2CACC.start(); |
gke | 0:62a1c91a859a | 259 | r = I2CACC.write(ADXL345_W); |
gke | 0:62a1c91a859a | 260 | r = I2CACC.write(0x2C); // Rate |
gke | 0:62a1c91a859a | 261 | r = I2CACC.write(0x09); // 50Hz, 400Hz 0x0C |
gke | 0:62a1c91a859a | 262 | I2CACC.stop(); |
gke | 0:62a1c91a859a | 263 | |
gke | 0:62a1c91a859a | 264 | Delay1mS(5); |
gke | 0:62a1c91a859a | 265 | |
gke | 0:62a1c91a859a | 266 | } // InitADXL345Acc |
gke | 0:62a1c91a859a | 267 | |
gke | 0:62a1c91a859a | 268 | boolean ADXL345AccActive(void) { |
gke | 0:62a1c91a859a | 269 | |
gke | 0:62a1c91a859a | 270 | I2CACC.start(); |
gke | 0:62a1c91a859a | 271 | F.AccelerationsValid = I2CACC.write(ADXL345_ID) == I2C_ACK; |
gke | 0:62a1c91a859a | 272 | I2CACC.stop(); |
gke | 0:62a1c91a859a | 273 | |
gke | 0:62a1c91a859a | 274 | TrackMinI2CRate(400000); |
gke | 0:62a1c91a859a | 275 | |
gke | 0:62a1c91a859a | 276 | return( true ); //zzz F.AccelerationsValid ); |
gke | 0:62a1c91a859a | 277 | |
gke | 0:62a1c91a859a | 278 | } // ADXL345AccActive |
gke | 0:62a1c91a859a | 279 | |
gke | 0:62a1c91a859a | 280 | //________________________________________________________________________________________ |
gke | 0:62a1c91a859a | 281 | |
gke | 0:62a1c91a859a | 282 | // LIS3LV02DG Accelerometer 400KHz |
gke | 0:62a1c91a859a | 283 | |
gke | 0:62a1c91a859a | 284 | void WriteLISL(uint8, uint8); |
gke | 0:62a1c91a859a | 285 | void ReadLISLAcc(void); |
gke | 0:62a1c91a859a | 286 | boolean LISLAccActive(void); |
gke | 0:62a1c91a859a | 287 | |
gke | 0:62a1c91a859a | 288 | const float GRAVITY_LISL = 1024.0; |
gke | 0:62a1c91a859a | 289 | |
gke | 0:62a1c91a859a | 290 | void ReadLISLAcc(void) { |
gke | 0:62a1c91a859a | 291 | static uint8 a; |
gke | 0:62a1c91a859a | 292 | static char b[6]; |
gke | 0:62a1c91a859a | 293 | static i16u X, Y, Z; |
gke | 0:62a1c91a859a | 294 | |
gke | 0:62a1c91a859a | 295 | F.AccelerationsValid = I2CACC.write(LISL_ID) == I2C_ACK; // Acc still there? |
gke | 0:62a1c91a859a | 296 | if ( F.AccelerationsValid ) { |
gke | 0:62a1c91a859a | 297 | |
gke | 0:62a1c91a859a | 298 | // Ax.b0 = I2CACC.write(LISL_OUTX_L + LISL_INCR_ADDR + LISL_READ); |
gke | 0:62a1c91a859a | 299 | |
gke | 0:62a1c91a859a | 300 | I2CACC.read(LISL_ID, b, 6); |
gke | 0:62a1c91a859a | 301 | |
gke | 0:62a1c91a859a | 302 | X.b1 = b[1]; |
gke | 0:62a1c91a859a | 303 | X.b0 = b[0]; |
gke | 0:62a1c91a859a | 304 | Y.b1 = b[3]; |
gke | 0:62a1c91a859a | 305 | Y.b0 = b[2]; |
gke | 0:62a1c91a859a | 306 | Z.b1 = b[5]; |
gke | 0:62a1c91a859a | 307 | Z.b0 = b[4]; |
gke | 0:62a1c91a859a | 308 | |
gke | 0:62a1c91a859a | 309 | Acc[BF] = Z.i16; |
gke | 0:62a1c91a859a | 310 | Acc[LR] = -X.i16; |
gke | 0:62a1c91a859a | 311 | Acc[UD] = Y.i16; |
gke | 0:62a1c91a859a | 312 | |
gke | 0:62a1c91a859a | 313 | // LP Filter here? |
gke | 0:62a1c91a859a | 314 | |
gke | 0:62a1c91a859a | 315 | for ( a = 0; a < (uint8)3; a++ ) |
gke | 0:62a1c91a859a | 316 | Acc[a] /= GRAVITY_LISL; |
gke | 0:62a1c91a859a | 317 | |
gke | 0:62a1c91a859a | 318 | } else { |
gke | 0:62a1c91a859a | 319 | for ( a = 0; a < (uint8)3; a++ ) |
gke | 0:62a1c91a859a | 320 | Acc[a] = AccNeutral[a]; |
gke | 0:62a1c91a859a | 321 | Acc[UD] += 1.0; |
gke | 0:62a1c91a859a | 322 | |
gke | 0:62a1c91a859a | 323 | if ( State == InFlight ) { |
gke | 0:62a1c91a859a | 324 | Stats[AccFailS]++; // data over run - acc out of range |
gke | 0:62a1c91a859a | 325 | // use neutral values!!!! |
gke | 0:62a1c91a859a | 326 | F.AccFailure = true; |
gke | 0:62a1c91a859a | 327 | } |
gke | 0:62a1c91a859a | 328 | } |
gke | 0:62a1c91a859a | 329 | } // ReadLISLAccelerometers |
gke | 0:62a1c91a859a | 330 | |
gke | 0:62a1c91a859a | 331 | void WriteLISL(uint8 d, uint8 a) { |
gke | 0:62a1c91a859a | 332 | I2CACC.start(); |
gke | 0:62a1c91a859a | 333 | I2CACC.write(a); |
gke | 0:62a1c91a859a | 334 | I2CACC.write(d); |
gke | 0:62a1c91a859a | 335 | I2CACC.stop(); |
gke | 0:62a1c91a859a | 336 | } // WriteLISL |
gke | 0:62a1c91a859a | 337 | |
gke | 0:62a1c91a859a | 338 | boolean LISLAccActive(void) { |
gke | 0:62a1c91a859a | 339 | F.AccelerationsValid = false; |
gke | 0:62a1c91a859a | 340 | /* |
gke | 0:62a1c91a859a | 341 | WriteLISL(0x4a, LISL_CTRLREG_2); // enable 3-wire, BDU=1, +/-2g |
gke | 0:62a1c91a859a | 342 | |
gke | 0:62a1c91a859a | 343 | if ( I2CACC.write(LISL_ID) == I2C_ACK ) { |
gke | 0:62a1c91a859a | 344 | WriteLISL(0xc7, LISL_CTRLREG_1); // on always, 40Hz sampling rate, 10Hz LP cutoff, enable all axes |
gke | 0:62a1c91a859a | 345 | WriteLISL(0, LISL_CTRLREG_3); |
gke | 0:62a1c91a859a | 346 | WriteLISL(0x40, LISL_FF_CFG); // latch, no interrupts; |
gke | 0:62a1c91a859a | 347 | WriteLISL(0, LISL_FF_THS_L); |
gke | 0:62a1c91a859a | 348 | WriteLISL(0xFC, LISL_FF_THS_H); // -0,5g threshold |
gke | 0:62a1c91a859a | 349 | WriteLISL(255, LISL_FF_DUR); |
gke | 0:62a1c91a859a | 350 | WriteLISL(0, LISL_DD_CFG); |
gke | 0:62a1c91a859a | 351 | F.AccelerationsValid = true; |
gke | 0:62a1c91a859a | 352 | } else |
gke | 0:62a1c91a859a | 353 | F.AccFailure = true; |
gke | 0:62a1c91a859a | 354 | */ |
gke | 0:62a1c91a859a | 355 | |
gke | 0:62a1c91a859a | 356 | TrackMinI2CRate(400000); |
gke | 0:62a1c91a859a | 357 | |
gke | 0:62a1c91a859a | 358 | return ( false );//F.AccelerationsValid ); |
gke | 0:62a1c91a859a | 359 | } // LISLAccActive |
gke | 0:62a1c91a859a | 360 | |
gke | 0:62a1c91a859a | 361 | |
gke | 0:62a1c91a859a | 362 |