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