Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
Diff: accel.c
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
diff -r 1e3318a30ddd -r 90292f8bd179 accel.c --- a/accel.c Fri Feb 25 01:35:24 2011 +0000 +++ b/accel.c Tue Apr 26 12:12:29 2011 +0000 @@ -2,7 +2,7 @@ // = UAVXArm Quadrocopter Controller = // = Copyright (c) 2008 by Prof. Greg Egan = // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer = -// = http://code.google.com/p/uavp-mods/ http://uavp.ch = +// = http://code.google.com/p/uavp-mods/ = // =============================================================================================== // This is part of UAVXArm. @@ -27,20 +27,21 @@ void AccelerometerTest(void); void InitAccelerometers(void); -real32 Vel[3], Acc[3], AccNeutral[3], Accp[3]; +real32 Vel[3], AccADC[3], AccADCp[3], AccNoise[3], Acc[3], AccNeutral[3], Accp[3]; int16 NewAccNeutral[3]; uint8 AccelerometerType; +real32 GravityR; void ShowAccType(void) { switch ( AccelerometerType ) { case LISLAcc: - TxString("LIS3L"); + TxString(" LIS3L"); break; case ADXL345Acc: - TxString("ADXL345"); + TxString(" ADXL345"); break; case AccUnknown: - TxString("unknown"); + TxString(" unknown"); break; default: ; @@ -61,25 +62,33 @@ Acc[UD] = 1.0; break; } // switch + } // ReadAccelerometers void GetNeutralAccelerations(void) { - static uint8 i, a; + static int16 i; + static uint8 a; static real32 Temp[3] = {0.0, 0.0, 0.0}; + const int16 Samples = 100; if ( F.AccelerationsValid ) { - for ( i = 16; i; i--) { + for ( i = Samples; i; i--) { ReadAccelerometers(); for ( a = 0; a <(uint8)3; a++ ) - Temp[a] += Acc[a]; + Temp[a] += AccADC[a]; } for ( a = 0; a <(uint8)3; a++ ) - Temp[a] = Temp[a] * 0.0625; + Temp[a] /= Samples; - NewAccNeutral[BF] = Limit((int16)(Temp[BF] * 1000.0 ), -99, 99); - NewAccNeutral[LR] = Limit( (int16)(Temp[LR] * 1000.0 ), -99, 99); - NewAccNeutral[UD] = Limit( (int16)(( Temp[UD] - 1.0 ) * 1000.0) , -99, 99); + // removes other accelerations + GravityR = 1.0/sqrt(Sqr(Temp[BF])+Sqr(Temp[LR])+Sqr(Temp[UD])); + for ( a = 0; a <(uint8)3; a++ ) + Acc[a] = Temp[a] * GravityR; + + NewAccNeutral[BF] = Limit1((int16)(Acc[BF] * 1000.0 ), 99); + NewAccNeutral[LR] = Limit1( (int16)(Acc[LR] * 1000.0 ), 99); + NewAccNeutral[UD] = Limit1( (int16)(( Acc[UD] + 1.0 ) * 1000.0) , 99); } else for ( a = 0; a <(uint8)3; a++ ) @@ -89,8 +98,8 @@ void GetAccelerations(void) { + static uint8 a; static real32 AccA; - static uint8 a; if ( F.AccelerationsValid ) { ReadAccelerometers(); @@ -98,15 +107,17 @@ // Neutral[ {LR, BF, UD} ] pass through UAVPSet // and come back as AccMiddle[LR] etc. - Acc[BF] -= K[MiddleBF]; - Acc[LR] -= K[MiddleLR]; - Acc[UD] -= K[MiddleUD]; + Acc[BF] = AccADC[BF] * GravityR - K[MiddleBF]; + Acc[LR] = AccADC[LR] * GravityR - K[MiddleLR]; + Acc[UD] = AccADC[UD] * GravityR - K[MiddleUD]; - AccA = dT / ( OneOnTwoPiAccF + dT ); - for ( a = 0; a < (uint8)3; a++ ) { - Acc[a] = Accp[a] + (Acc[a] - Accp[a]) * AccA; +#ifndef SUPPRESS_ACC_FILTERS + AccA = dT / ( 1.0 / ( TWOPI * ACC_FREQ ) + dT ); + for ( a = 0; a < (uint8)3; a++ ) + Acc[a] = LPFilter( Acc[a], Accp[a], AccA ); +#endif // !SUPPRESS_ACC_FILTERS + for ( a = 0; a < (uint8)3; a++ ) Accp[a] = Acc[a]; - } } else { Acc[LR] = Acc[BF] = 0; @@ -122,23 +133,27 @@ InitAccelerometers(); if ( F.AccelerationsValid ) { - ReadAccelerometers(); + GetAccelerations(); + + TxString("Sensor & Max Delta\r\n"); TxString("\tL->R: \t"); - TxVal32( Acc[LR] * 1000.0, 3, 'G'); + TxVal32( AccADC[LR], 0, HT); + TxVal32( AccNoise[LR], 0, 0); if ( fabs(Acc[LR]) > 0.2 ) TxString(" fault?"); TxNextLine(); TxString("\tB->F: \t"); - TxVal32( Acc[BF] * 1000.0, 3, 'G'); + TxVal32( AccADC[BF], 0, HT); + TxVal32( AccNoise[BF], 0, 0); if ( fabs(Acc[BF]) > 0.2 ) TxString(" fault?"); TxNextLine(); - TxString("\tU->D: \t"); - TxVal32( Acc[UD] * 1000.0, 3, 'G'); + TxVal32( AccADC[UD], 0, HT); + TxVal32( AccNoise[UD], 0, 0); if ( fabs(Acc[UD]) > 1.2 ) TxString(" fault?"); TxNextLine(); @@ -150,20 +165,19 @@ F.AccelerationsValid = true; // optimistic - for ( a = 0; a < (uint8)3; a++ ) { - NewAccNeutral[a] = Acc[a] = Accp[a] = Vel[a] = 0.0; - Comp[a] = 0; - } + for ( a = 0; a < (uint8)3; a++ ) + NewAccNeutral[a] = AccADC[a] = AccADCp[a] = AccNoise[a] = Acc[a] = Accp[a] = Vel[a] = 0.0; + Acc[2] = Accp[2] = 1.0; - if ( ADXL345AccActive() ) { InitADXL345Acc(); AccelerometerType = ADXL345Acc; } else - if ( LISLAccActive() ) + if ( LISLAccActive() ) { + InitLISLAcc(); AccelerometerType = LISLAcc; - else + } else // check for other accs in preferred order { AccelerometerType = AccUnknown; @@ -186,31 +200,19 @@ void InitADXL345Acc(void); boolean ADXL345AccActive(void); -const float GRAVITY_ADXL345 = 250.0; // ~4mG/LSB - void ReadADXL345Acc(void) { - static uint8 a, r; + static uint8 a; static char b[6]; static i16u X, Y, Z; - - /* - r = 0; - while ( r == 0 ) { - I2CACC.start(); - r = I2CACC.write(ADXL345_ID); - r = I2CACC.write(0x30); - r = I2CACC.read(true) & 0x80; - I2CACC.stop(); - } - */ + static real32 d; I2CACC.start(); - r = I2CACC.write(ADXL345_WR); - r = I2CACC.write(0x32); // point to acc data + if ( I2CACC.write(ADXL345_WR) != I2C_ACK ) goto ADXL345Error; // point to acc data + if ( I2CACC.write(0x32) != I2C_ACK ) goto ADXL345Error; // point to acc data I2CACC.stop(); - I2CACC.blockread(ADXL345_ID, b, 6); + if ( I2CACC.blockread(ADXL345_ID, b, 6) ) goto ADXL345Error; X.b1 = b[1]; X.b0 = b[0]; @@ -220,60 +222,77 @@ Z.b0 = b[4]; if ( F.Using9DOF ) { // SparkFun/QuadroUFO 9DOF breakouts pins forward components up - Acc[BF] = -Y.i16; - Acc[LR] = -X.i16; - Acc[UD] = -Z.i16; - } else {// SparkFun 6DOF breakouts pins forward components down - Acc[BF] = -X.i16; - Acc[LR] = -Y.i16; - Acc[UD] = -Z.i16; + AccADC[BF] = -Y.i16; + AccADC[LR] = -X.i16; + AccADC[UD] = -Z.i16; + } else {// SparkFun 6DOF breakouts pins forward components down + AccADC[BF] = -X.i16; + AccADC[LR] = -Y.i16; + AccADC[UD] = Z.i16; } - // LP filter here? + for ( a = 0; a < (int8)3; a++ ) { + d = fabs(AccADC[a]-AccADCp[a]); + if ( d>AccNoise[a] ) AccNoise[a] = d; + } + + return; - for ( a = 0; a < (int8)3; a++ ) - Acc[a] /= GRAVITY_ADXL345; +ADXL345Error: + I2CACC.stop(); + + I2CError[ADXL345_ID]++; + if ( State == InFlight ) { + Stats[AccFailS]++; // data over run - acc out of range + // use neutral values!!!! + F.AccFailure = true; + } } // ReadADXL345Acc void InitADXL345Acc() { - static uint8 r; I2CACC.start(); - I2CACC.write(ADXL345_WR); - r = I2CACC.write(0x2D); // power register - r = I2CACC.write(0x08); // measurement mode + if ( I2CACC.write(ADXL345_WR) != I2C_ACK ) goto ADXL345Error;; + if ( I2CACC.write(0x2D) != I2C_ACK ) goto ADXL345Error; // power register + if ( I2CACC.write(0x08) != I2C_ACK ) goto ADXL345Error; // measurement mode I2CACC.stop(); Delay1mS(5); I2CACC.start(); - r = I2CACC.write(ADXL345_WR); - r = I2CACC.write(0x31); // format - r = I2CACC.write(0x08); // full resolution, 2g + if ( I2CACC.write(ADXL345_WR) != I2C_ACK ) goto ADXL345Error; + if ( I2CACC.write(0x31) != I2C_ACK ) goto ADXL345Error; // format + if ( I2CACC.write(0x08) != I2C_ACK ) goto ADXL345Error; // full resolution, 2g I2CACC.stop(); Delay1mS(5); I2CACC.start(); - r = I2CACC.write(ADXL345_WR); - r = I2CACC.write(0x2C); // Rate - r = I2CACC.write(0x09); // 50Hz, 400Hz 0x0C + if ( I2CACC.write(ADXL345_WR) != I2C_ACK ) goto ADXL345Error; + if ( I2CACC.write(0x2C) != I2C_ACK ) goto ADXL345Error; // Rate + if ( I2CACC.write(0x09) != I2C_ACK ) goto ADXL345Error; // 50Hz, 400Hz 0x0C I2CACC.stop(); Delay1mS(5); + return; + +ADXL345Error: + I2CACC.stop(); + + I2CError[ADXL345_ID]++; + } // InitADXL345Acc boolean ADXL345AccActive(void) { - I2CACC.start(); - F.AccelerationsValid = I2CACC.write(ADXL345_WR) == I2C_ACK; - I2CACC.stop(); - - TrackMinI2CRate(400000); - - return( true ); //zzz F.AccelerationsValid ); + F.AccelerationsValid = I2CACCAddressResponds(ADXL345_ID); + + if ( F.AccelerationsValid) + TrackMinI2CRate(400000); + + return( F.AccelerationsValid ); } // ADXL345AccActive @@ -281,81 +300,107 @@ // LIS3LV02DG Accelerometer 400KHz -void WriteLISL(uint8, uint8); +boolean WriteLISL(uint8, uint8); void ReadLISLAcc(void); +void InitLISLAcc(void); boolean LISLAccActive(void); -const float GRAVITY_LISL = 1024.0; +void ReadLISLAcc(void) { -void ReadLISLAcc(void) { static uint8 a; + static real32 d; static char b[6]; static i16u X, Y, Z; F.AccelerationsValid = I2CACCAddressResponds( LISL_ID ); // Acc still there? - if ( F.AccelerationsValid ) { + + if ( !F.AccelerationsValid ) goto LISLError; - // Ax.b0 = I2CACC.write(LISL_OUTX_L + LISL_INCR_ADDR + LISL_READ); - - I2CACC.blockread(LISL_READ, b, 6); + I2CACC.start(); + if ( I2CACC.write(LISL_WR) != I2C_ACK ) goto LISLError; + if ( I2CACC.write(LISL_OUTX_L | 0x80 ) != I2C_ACK ) goto LISLError; // auto increment address + I2CACC.stop(); - X.b1 = b[1]; - X.b0 = b[0]; - Y.b1 = b[3]; - Y.b0 = b[2]; - Z.b1 = b[5]; - Z.b0 = b[4]; + if ( I2CACC.blockread(LISL_RD, b, 6) ) goto LISLError; - Acc[BF] = Z.i16; - Acc[LR] = -X.i16; - Acc[UD] = Y.i16; + X.b1 = b[1]; + X.b0 = b[0]; + Y.b1 = b[3]; + Y.b0 = b[2]; + Z.b1 = b[5]; + Z.b0 = b[4]; - // LP Filter here? + // UAVP Breakout Board pins to the rear components up + AccADC[BF] = Y.i16; + AccADC[LR] = X.i16; + AccADC[UD] = -Z.i16; - for ( a = 0; a < (uint8)3; a++ ) - Acc[a] /= GRAVITY_LISL; + for ( a = 0; a < (int8)3; a++ ) { + d = fabs(AccADC[a]-AccADCp[a]); + if ( d>AccNoise[a] ) AccNoise[a] = d; + } + + return; - } else { - for ( a = 0; a < (uint8)3; a++ ) - Acc[a] = AccNeutral[a]; - Acc[UD] += 1.0; +LISLError: + I2CACC.stop(); + + I2CError[LISL_ID]++; - if ( State == InFlight ) { - Stats[AccFailS]++; // data over run - acc out of range - // use neutral values!!!! - F.AccFailure = true; - } + if ( State == InFlight ) { + Stats[AccFailS]++; // data over run - acc out of range + // use neutral values!!!! + F.AccFailure = true; } + } // ReadLISLAccelerometers -void WriteLISL(uint8 d, uint8 a) { +boolean WriteLISL(uint8 d, uint8 a) { + I2CACC.start(); - I2CACC.write(a); - I2CACC.write(d); + if ( I2CACC.write(LISL_WR) != I2C_ACK ) goto LISLError; + if ( I2CACC.write(a) != I2C_ACK ) goto LISLError; + if ( I2CACC.write(d) != I2C_ACK ) goto LISLError; + I2CACC.stop(); + + return(false); + +LISLError: I2CACC.stop(); + + I2CError[LISL_ID]++; + + return(true); + } // WriteLISL +void InitLISLAcc(void) { + + if ( WriteLISL(0x4a, LISL_CTRLREG_2) ) goto LISLError; // enable 3-wire, BDU=1, +/-2g + if ( WriteLISL(0xc7, LISL_CTRLREG_1) ) goto LISLError; // on always, 40Hz sampling rate, 10Hz LP cutoff, enable all axes + if ( WriteLISL(0, LISL_CTRLREG_3) ) goto LISLError; + if ( WriteLISL(0x40, LISL_FF_CFG) ) goto LISLError; // latch, no interrupts; + if ( WriteLISL(0, LISL_FF_THS_L) ) goto LISLError; + if ( WriteLISL(0xFC, LISL_FF_THS_H) ) goto LISLError; // -0,5g threshold + if ( WriteLISL(255, LISL_FF_DUR) ) goto LISLError; + if ( WriteLISL(0, LISL_DD_CFG) ) goto LISLError; + + TrackMinI2CRate(400000); + F.AccelerationsValid = true; + + return; + +LISLError: + F.AccelerationsValid = false; + +} // InitLISLAcc + boolean LISLAccActive(void) { - F.AccelerationsValid = false; - /* - WriteLISL(0x4a, LISL_CTRLREG_2); // enable 3-wire, BDU=1, +/-2g + + F.AccelerationsValid = I2CACCAddressResponds( LISL_ID ); - if ( I2CACC.write(LISL_ID) == I2C_ACK ) { - WriteLISL(0xc7, LISL_CTRLREG_1); // on always, 40Hz sampling rate, 10Hz LP cutoff, enable all axes - WriteLISL(0, LISL_CTRLREG_3); - WriteLISL(0x40, LISL_FF_CFG); // latch, no interrupts; - WriteLISL(0, LISL_FF_THS_L); - WriteLISL(0xFC, LISL_FF_THS_H); // -0,5g threshold - WriteLISL(255, LISL_FF_DUR); - WriteLISL(0, LISL_DD_CFG); - F.AccelerationsValid = true; - } else - F.AccFailure = true; - */ - - TrackMinI2CRate(400000); - - return ( false );//F.AccelerationsValid ); + return ( F.AccelerationsValid ); + } // LISLAccActive