Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers accel.c Source File

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