Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
accel.c
00001 // =============================================================================================== 00002 // = UAVXArm Quadrocopter Controller = 00003 // = Copyright (c) 2008 by Prof. Greg Egan = 00004 // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer = 00005 // = http://code.google.com/p/uavp-mods/ = 00006 // =============================================================================================== 00007 00008 // This is part of UAVXArm. 00009 00010 // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU 00011 // General Public License as published by the Free Software Foundation, either version 3 of the 00012 // License, or (at your option) any later version. 00013 00014 // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without 00015 // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 00016 // See the GNU General Public License for more details. 00017 00018 // You should have received a copy of the GNU General Public License along with this program. 00019 // If not, see http://www.gnu.org/licenses/ 00020 00021 #include "UAVXArm.h" 00022 00023 void ShowAccType(void); 00024 void ReadAccelerometers(void); 00025 void GetAccelerations(void); 00026 void GetNeutralAccelerations(void); 00027 void AccelerometerTest(void); 00028 void InitAccelerometers(void); 00029 00030 real32 Vel[3], AccADC[3], AccADCp[3], AccNoise[3], Acc[3], AccNeutral[3], Accp[3]; 00031 int16 NewAccNeutral[3]; 00032 uint8 AccelerometerType; 00033 real32 GravityR; 00034 00035 void ShowAccType(void) { 00036 switch ( AccelerometerType ) { 00037 case LISLAcc: 00038 TxString(" LIS3L"); 00039 break; 00040 case ADXL345Acc: 00041 TxString(" ADXL345"); 00042 break; 00043 case AccUnknown: 00044 TxString(" unknown"); 00045 break; 00046 default: 00047 ; 00048 } // switch 00049 } // ShowAccType 00050 00051 void ReadAccelerometers(void) { 00052 switch ( AccelerometerType ) { 00053 case LISLAcc: 00054 ReadLISLAcc(); 00055 break; 00056 case ADXL345Acc: 00057 ReadADXL345Acc(); 00058 break; 00059 // other accelerometers 00060 default: 00061 Acc[BF] = Acc[LR] = 0.0; 00062 Acc[UD] = 1.0; 00063 break; 00064 } // switch 00065 00066 } // ReadAccelerometers 00067 00068 void GetNeutralAccelerations(void) { 00069 static int16 i; 00070 static uint8 a; 00071 static real32 Temp[3] = {0.0, 0.0, 0.0}; 00072 const int16 Samples = 100; 00073 00074 if ( F.AccelerationsValid ) { 00075 for ( i = Samples; i; i--) { 00076 ReadAccelerometers(); 00077 for ( a = 0; a <(uint8)3; a++ ) 00078 Temp[a] += AccADC[a]; 00079 } 00080 00081 for ( a = 0; a <(uint8)3; a++ ) 00082 Temp[a] /= Samples; 00083 00084 // removes other accelerations 00085 GravityR = 1.0/sqrt(Sqr(Temp[BF])+Sqr(Temp[LR])+Sqr(Temp[UD])); 00086 for ( a = 0; a <(uint8)3; a++ ) 00087 Acc[a] = Temp[a] * GravityR; 00088 00089 NewAccNeutral[BF] = Limit1((int16)(Acc[BF] * 1000.0 ), 99); 00090 NewAccNeutral[LR] = Limit1( (int16)(Acc[LR] * 1000.0 ), 99); 00091 NewAccNeutral[UD] = Limit1( (int16)(( Acc[UD] + 1.0 ) * 1000.0) , 99); 00092 00093 } else 00094 for ( a = 0; a <(uint8)3; a++ ) 00095 AccNeutral[a] = 0.0; 00096 00097 } // GetNeutralAccelerations 00098 00099 void GetAccelerations(void) { 00100 00101 static uint8 a; 00102 static real32 AccA; 00103 00104 if ( F.AccelerationsValid ) { 00105 ReadAccelerometers(); 00106 00107 // Neutral[ {LR, BF, UD} ] pass through UAVPSet 00108 // and come back as AccMiddle[LR] etc. 00109 00110 Acc[BF] = AccADC[BF] * GravityR - K[MiddleBF]; 00111 Acc[LR] = AccADC[LR] * GravityR - K[MiddleLR]; 00112 Acc[UD] = AccADC[UD] * GravityR - K[MiddleUD]; 00113 00114 #ifndef SUPPRESS_ACC_FILTERS 00115 AccA = dT / ( 1.0 / ( TWOPI * ACC_FREQ ) + dT ); 00116 for ( a = 0; a < (uint8)3; a++ ) 00117 Acc[a] = LPFilter( Acc[a], Accp[a], AccA ); 00118 #endif // !SUPPRESS_ACC_FILTERS 00119 for ( a = 0; a < (uint8)3; a++ ) 00120 Accp[a] = Acc[a]; 00121 00122 } else { 00123 Acc[LR] = Acc[BF] = 0; 00124 Acc[UD] = 1.0; 00125 } 00126 } // GetAccelerations 00127 00128 void AccelerometerTest(void) { 00129 TxString("\r\nAccelerometer test -"); 00130 ShowAccType(); 00131 TxString("\r\nRead once - no averaging\r\n"); 00132 00133 InitAccelerometers(); 00134 00135 if ( F.AccelerationsValid ) { 00136 GetAccelerations(); 00137 00138 TxString("Sensor & Max Delta\r\n"); 00139 00140 TxString("\tL->R: \t"); 00141 TxVal32( AccADC[LR], 0, HT); 00142 TxVal32( AccNoise[LR], 0, 0); 00143 if ( fabs(Acc[LR]) > 0.2 ) 00144 TxString(" fault?"); 00145 TxNextLine(); 00146 00147 TxString("\tB->F: \t"); 00148 TxVal32( AccADC[BF], 0, HT); 00149 TxVal32( AccNoise[BF], 0, 0); 00150 if ( fabs(Acc[BF]) > 0.2 ) 00151 TxString(" fault?"); 00152 TxNextLine(); 00153 00154 TxString("\tU->D: \t"); 00155 TxVal32( AccADC[UD], 0, HT); 00156 TxVal32( AccNoise[UD], 0, 0); 00157 if ( fabs(Acc[UD]) > 1.2 ) 00158 TxString(" fault?"); 00159 TxNextLine(); 00160 } 00161 } // AccelerometerTest 00162 00163 void InitAccelerometers(void) { 00164 static uint8 a; 00165 00166 F.AccelerationsValid = true; // optimistic 00167 00168 for ( a = 0; a < (uint8)3; a++ ) 00169 NewAccNeutral[a] = AccADC[a] = AccADCp[a] = AccNoise[a] = Acc[a] = Accp[a] = Vel[a] = 0.0; 00170 00171 Acc[2] = Accp[2] = 1.0; 00172 if ( ADXL345AccActive() ) { 00173 InitADXL345Acc(); 00174 AccelerometerType = ADXL345Acc; 00175 00176 } else 00177 if ( LISLAccActive() ) { 00178 InitLISLAcc(); 00179 AccelerometerType = LISLAcc; 00180 } else 00181 // check for other accs in preferred order 00182 { 00183 AccelerometerType = AccUnknown; 00184 F.AccelerationsValid = false; 00185 } 00186 00187 if ( F.AccelerationsValid ) { 00188 LEDYellow_ON; 00189 GetNeutralAccelerations(); 00190 LEDYellow_OFF; 00191 } else 00192 F.AccFailure = true; 00193 } // InitAccelerometers 00194 00195 //________________________________________________________________________________________ 00196 00197 // ADXL345 3 Axis Accelerometer 00198 00199 void ReadADXL345Acc(void); 00200 void InitADXL345Acc(void); 00201 boolean ADXL345AccActive(void); 00202 00203 void ReadADXL345Acc(void) { 00204 00205 static uint8 a; 00206 static char b[6]; 00207 static i16u X, Y, Z; 00208 static real32 d; 00209 00210 I2CACC.start(); 00211 if ( I2CACC.write(ADXL345_WR) != I2C_ACK ) goto ADXL345Error; // point to acc data 00212 if ( I2CACC.write(0x32) != I2C_ACK ) goto ADXL345Error; // point to acc data 00213 I2CACC.stop(); 00214 00215 if ( I2CACC.blockread(ADXL345_ID, b, 6) ) goto ADXL345Error; 00216 00217 X.b1 = b[1]; 00218 X.b0 = b[0]; 00219 Y.b1 = b[3]; 00220 Y.b0 =b[2]; 00221 Z.b1 = b[5]; 00222 Z.b0 = b[4]; 00223 00224 if ( F.Using9DOF ) { // SparkFun/QuadroUFO 9DOF breakouts pins forward components up 00225 AccADC[BF] = -Y.i16; 00226 AccADC[LR] = -X.i16; 00227 AccADC[UD] = -Z.i16; 00228 } else {// SparkFun 6DOF breakouts pins forward components down 00229 AccADC[BF] = -X.i16; 00230 AccADC[LR] = -Y.i16; 00231 AccADC[UD] = Z.i16; 00232 } 00233 00234 for ( a = 0; a < (int8)3; a++ ) { 00235 d = fabs(AccADC[a]-AccADCp[a]); 00236 if ( d>AccNoise[a] ) AccNoise[a] = d; 00237 } 00238 00239 return; 00240 00241 ADXL345Error: 00242 I2CACC.stop(); 00243 00244 I2CError[ADXL345_ID]++; 00245 if ( State == InFlight ) { 00246 Stats[AccFailS]++; // data over run - acc out of range 00247 // use neutral values!!!! 00248 F.AccFailure = true; 00249 } 00250 00251 } // ReadADXL345Acc 00252 00253 void InitADXL345Acc() { 00254 00255 I2CACC.start(); 00256 if ( I2CACC.write(ADXL345_WR) != I2C_ACK ) goto ADXL345Error;; 00257 if ( I2CACC.write(0x2D) != I2C_ACK ) goto ADXL345Error; // power register 00258 if ( I2CACC.write(0x08) != I2C_ACK ) goto ADXL345Error; // measurement mode 00259 I2CACC.stop(); 00260 00261 Delay1mS(5); 00262 00263 I2CACC.start(); 00264 if ( I2CACC.write(ADXL345_WR) != I2C_ACK ) goto ADXL345Error; 00265 if ( I2CACC.write(0x31) != I2C_ACK ) goto ADXL345Error; // format 00266 if ( I2CACC.write(0x08) != I2C_ACK ) goto ADXL345Error; // full resolution, 2g 00267 I2CACC.stop(); 00268 00269 Delay1mS(5); 00270 00271 I2CACC.start(); 00272 if ( I2CACC.write(ADXL345_WR) != I2C_ACK ) goto ADXL345Error; 00273 if ( I2CACC.write(0x2C) != I2C_ACK ) goto ADXL345Error; // Rate 00274 if ( I2CACC.write(0x09) != I2C_ACK ) goto ADXL345Error; // 50Hz, 400Hz 0x0C 00275 I2CACC.stop(); 00276 00277 Delay1mS(5); 00278 00279 return; 00280 00281 ADXL345Error: 00282 I2CACC.stop(); 00283 00284 I2CError[ADXL345_ID]++; 00285 00286 } // InitADXL345Acc 00287 00288 boolean ADXL345AccActive(void) { 00289 00290 F.AccelerationsValid = I2CACCAddressResponds(ADXL345_ID); 00291 00292 if ( F.AccelerationsValid) 00293 TrackMinI2CRate(400000); 00294 00295 return( F.AccelerationsValid ); 00296 00297 } // ADXL345AccActive 00298 00299 //________________________________________________________________________________________ 00300 00301 // LIS3LV02DG Accelerometer 400KHz 00302 00303 boolean WriteLISL(uint8, uint8); 00304 void ReadLISLAcc(void); 00305 void InitLISLAcc(void); 00306 boolean LISLAccActive(void); 00307 00308 void ReadLISLAcc(void) { 00309 00310 static uint8 a; 00311 static real32 d; 00312 static char b[6]; 00313 static i16u X, Y, Z; 00314 00315 F.AccelerationsValid = I2CACCAddressResponds( LISL_ID ); // Acc still there? 00316 00317 if ( !F.AccelerationsValid ) goto LISLError; 00318 00319 I2CACC.start(); 00320 if ( I2CACC.write(LISL_WR) != I2C_ACK ) goto LISLError; 00321 if ( I2CACC.write(LISL_OUTX_L | 0x80 ) != I2C_ACK ) goto LISLError; // auto increment address 00322 I2CACC.stop(); 00323 00324 if ( I2CACC.blockread(LISL_RD, b, 6) ) goto LISLError; 00325 00326 X.b1 = b[1]; 00327 X.b0 = b[0]; 00328 Y.b1 = b[3]; 00329 Y.b0 = b[2]; 00330 Z.b1 = b[5]; 00331 Z.b0 = b[4]; 00332 00333 // UAVP Breakout Board pins to the rear components up 00334 AccADC[BF] = Y.i16; 00335 AccADC[LR] = X.i16; 00336 AccADC[UD] = -Z.i16; 00337 00338 for ( a = 0; a < (int8)3; a++ ) { 00339 d = fabs(AccADC[a]-AccADCp[a]); 00340 if ( d>AccNoise[a] ) AccNoise[a] = d; 00341 } 00342 00343 return; 00344 00345 LISLError: 00346 I2CACC.stop(); 00347 00348 I2CError[LISL_ID]++; 00349 00350 if ( State == InFlight ) { 00351 Stats[AccFailS]++; // data over run - acc out of range 00352 // use neutral values!!!! 00353 F.AccFailure = true; 00354 } 00355 00356 } // ReadLISLAccelerometers 00357 00358 boolean WriteLISL(uint8 d, uint8 a) { 00359 00360 I2CACC.start(); 00361 if ( I2CACC.write(LISL_WR) != I2C_ACK ) goto LISLError; 00362 if ( I2CACC.write(a) != I2C_ACK ) goto LISLError; 00363 if ( I2CACC.write(d) != I2C_ACK ) goto LISLError; 00364 I2CACC.stop(); 00365 00366 return(false); 00367 00368 LISLError: 00369 I2CACC.stop(); 00370 00371 I2CError[LISL_ID]++; 00372 00373 return(true); 00374 00375 } // WriteLISL 00376 00377 void InitLISLAcc(void) { 00378 00379 if ( WriteLISL(0x4a, LISL_CTRLREG_2) ) goto LISLError; // enable 3-wire, BDU=1, +/-2g 00380 if ( WriteLISL(0xc7, LISL_CTRLREG_1) ) goto LISLError; // on always, 40Hz sampling rate, 10Hz LP cutoff, enable all axes 00381 if ( WriteLISL(0, LISL_CTRLREG_3) ) goto LISLError; 00382 if ( WriteLISL(0x40, LISL_FF_CFG) ) goto LISLError; // latch, no interrupts; 00383 if ( WriteLISL(0, LISL_FF_THS_L) ) goto LISLError; 00384 if ( WriteLISL(0xFC, LISL_FF_THS_H) ) goto LISLError; // -0,5g threshold 00385 if ( WriteLISL(255, LISL_FF_DUR) ) goto LISLError; 00386 if ( WriteLISL(0, LISL_DD_CFG) ) goto LISLError; 00387 00388 TrackMinI2CRate(400000); 00389 F.AccelerationsValid = true; 00390 00391 return; 00392 00393 LISLError: 00394 F.AccelerationsValid = false; 00395 00396 } // InitLISLAcc 00397 00398 boolean LISLAccActive(void) { 00399 00400 F.AccelerationsValid = I2CACCAddressResponds( LISL_ID ); 00401 00402 return ( F.AccelerationsValid ); 00403 00404 } // LISLAccActive 00405 00406 00407
Generated on Wed Jul 13 2022 01:50:20 by
