Prof Greg Egan
UAVX Multicopter Flight Controller.
Diff: accel.c
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
--- 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