Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

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?

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
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