UAVX Multicopter Flight Controller.

Dependencies:   mbed

Committer:
gke
Date:
Fri Feb 18 22:28:05 2011 +0000
Revision:
0:62a1c91a859a
Child:
1:1e3318a30ddd
First release

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 0:62a1c91a859a 5 // = http://code.google.com/p/uavp-mods/ http://uavp.ch =
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 0:62a1c91a859a 30 real32 Vel[3], Acc[3], AccNeutral[3], Accp[3];
gke 0:62a1c91a859a 31 int16 NewAccNeutral[3];
gke 0:62a1c91a859a 32 uint8 AccelerometerType;
gke 0:62a1c91a859a 33
gke 0:62a1c91a859a 34 void ShowAccType(void) {
gke 0:62a1c91a859a 35 switch ( AccelerometerType ) {
gke 0:62a1c91a859a 36 case LISLAcc:
gke 0:62a1c91a859a 37 TxString("LIS3L");
gke 0:62a1c91a859a 38 break;
gke 0:62a1c91a859a 39 case ADXL345Acc:
gke 0:62a1c91a859a 40 TxString("ADXL345");
gke 0:62a1c91a859a 41 break;
gke 0:62a1c91a859a 42 case AccUnknown:
gke 0:62a1c91a859a 43 TxString("unknown");
gke 0:62a1c91a859a 44 break;
gke 0:62a1c91a859a 45 default:
gke 0:62a1c91a859a 46 ;
gke 0:62a1c91a859a 47 } // switch
gke 0:62a1c91a859a 48 } // ShowAccType
gke 0:62a1c91a859a 49
gke 0:62a1c91a859a 50 void ReadAccelerometers(void) {
gke 0:62a1c91a859a 51 switch ( AccelerometerType ) {
gke 0:62a1c91a859a 52 case LISLAcc:
gke 0:62a1c91a859a 53 ReadLISLAcc();
gke 0:62a1c91a859a 54 break;
gke 0:62a1c91a859a 55 case ADXL345Acc:
gke 0:62a1c91a859a 56 ReadADXL345Acc();
gke 0:62a1c91a859a 57 break;
gke 0:62a1c91a859a 58 // other accelerometers
gke 0:62a1c91a859a 59 default:
gke 0:62a1c91a859a 60 Acc[BF] = Acc[LR] = 0.0;
gke 0:62a1c91a859a 61 Acc[UD] = 1.0;
gke 0:62a1c91a859a 62 break;
gke 0:62a1c91a859a 63 } // switch
gke 0:62a1c91a859a 64 } // ReadAccelerometers
gke 0:62a1c91a859a 65
gke 0:62a1c91a859a 66 void GetNeutralAccelerations(void) {
gke 0:62a1c91a859a 67 static uint8 i, a;
gke 0:62a1c91a859a 68 static real32 Temp[3] = {0.0, 0.0, 0.0};
gke 0:62a1c91a859a 69
gke 0:62a1c91a859a 70 if ( F.AccelerationsValid ) {
gke 0:62a1c91a859a 71 for ( i = 16; i; i--) {
gke 0:62a1c91a859a 72 ReadAccelerometers();
gke 0:62a1c91a859a 73 for ( a = 0; a <(uint8)3; a++ )
gke 0:62a1c91a859a 74 Temp[a] += Acc[a];
gke 0:62a1c91a859a 75 }
gke 0:62a1c91a859a 76
gke 0:62a1c91a859a 77 for ( a = 0; a <(uint8)3; a++ )
gke 0:62a1c91a859a 78 Temp[a] = Temp[a] * 0.0625;
gke 0:62a1c91a859a 79
gke 0:62a1c91a859a 80 NewAccNeutral[BF] = Limit((int16)(Temp[BF] * 1000.0 ), -99, 99);
gke 0:62a1c91a859a 81 NewAccNeutral[LR] = Limit( (int16)(Temp[LR] * 1000.0 ), -99, 99);
gke 0:62a1c91a859a 82 NewAccNeutral[UD] = Limit( (int16)(( Temp[UD] - 1.0 ) * 1000.0) , -99, 99);
gke 0:62a1c91a859a 83
gke 0:62a1c91a859a 84 } else
gke 0:62a1c91a859a 85 for ( a = 0; a <(uint8)3; a++ )
gke 0:62a1c91a859a 86 AccNeutral[a] = 0.0;
gke 0:62a1c91a859a 87
gke 0:62a1c91a859a 88 } // GetNeutralAccelerations
gke 0:62a1c91a859a 89
gke 0:62a1c91a859a 90 void GetAccelerations(void) {
gke 0:62a1c91a859a 91
gke 0:62a1c91a859a 92 static real32 AccA;
gke 0:62a1c91a859a 93 static uint8 a;
gke 0:62a1c91a859a 94
gke 0:62a1c91a859a 95 if ( F.AccelerationsValid ) {
gke 0:62a1c91a859a 96 ReadAccelerometers();
gke 0:62a1c91a859a 97
gke 0:62a1c91a859a 98 // Neutral[ {LR, BF, UD} ] pass through UAVPSet
gke 0:62a1c91a859a 99 // and come back as AccMiddle[LR] etc.
gke 0:62a1c91a859a 100
gke 0:62a1c91a859a 101 Acc[BF] -= K[MiddleBF];
gke 0:62a1c91a859a 102 Acc[LR] -= K[MiddleLR];
gke 0:62a1c91a859a 103 Acc[UD] -= K[MiddleUD];
gke 0:62a1c91a859a 104
gke 0:62a1c91a859a 105 AccA = dT / ( OneOnTwoPiAccF + dT );
gke 0:62a1c91a859a 106 for ( a = 0; a < (uint8)3; a++ ) {
gke 0:62a1c91a859a 107 Acc[a] = Accp[a] + (Acc[a] - Accp[a]) * AccA;
gke 0:62a1c91a859a 108 Accp[a] = Acc[a];
gke 0:62a1c91a859a 109 }
gke 0:62a1c91a859a 110
gke 0:62a1c91a859a 111 } else {
gke 0:62a1c91a859a 112 Acc[LR] = Acc[BF] = 0;
gke 0:62a1c91a859a 113 Acc[UD] = 1.0;
gke 0:62a1c91a859a 114 }
gke 0:62a1c91a859a 115 } // GetAccelerations
gke 0:62a1c91a859a 116
gke 0:62a1c91a859a 117 void AccelerometerTest(void) {
gke 0:62a1c91a859a 118 TxString("\r\nAccelerometer test -");
gke 0:62a1c91a859a 119 ShowAccType();
gke 0:62a1c91a859a 120 TxString("\r\nRead once - no averaging\r\n");
gke 0:62a1c91a859a 121
gke 0:62a1c91a859a 122 InitAccelerometers();
gke 0:62a1c91a859a 123
gke 0:62a1c91a859a 124 if ( F.AccelerationsValid ) {
gke 0:62a1c91a859a 125 ReadAccelerometers();
gke 0:62a1c91a859a 126
gke 0:62a1c91a859a 127 TxString("\tL->R: \t");
gke 0:62a1c91a859a 128 TxVal32( Acc[LR] * 1000.0, 3, 'G');
gke 0:62a1c91a859a 129 if ( fabs(Acc[LR]) > 0.2 )
gke 0:62a1c91a859a 130 TxString(" fault?");
gke 0:62a1c91a859a 131 TxNextLine();
gke 0:62a1c91a859a 132
gke 0:62a1c91a859a 133 TxString("\tB->F: \t");
gke 0:62a1c91a859a 134 TxVal32( Acc[BF] * 1000.0, 3, 'G');
gke 0:62a1c91a859a 135 if ( fabs(Acc[BF]) > 0.2 )
gke 0:62a1c91a859a 136 TxString(" fault?");
gke 0:62a1c91a859a 137 TxNextLine();
gke 0:62a1c91a859a 138
gke 0:62a1c91a859a 139
gke 0:62a1c91a859a 140 TxString("\tU->D: \t");
gke 0:62a1c91a859a 141 TxVal32( Acc[UD] * 1000.0, 3, 'G');
gke 0:62a1c91a859a 142 if ( fabs(Acc[UD]) > 1.2 )
gke 0:62a1c91a859a 143 TxString(" fault?");
gke 0:62a1c91a859a 144 TxNextLine();
gke 0:62a1c91a859a 145 }
gke 0:62a1c91a859a 146 } // AccelerometerTest
gke 0:62a1c91a859a 147
gke 0:62a1c91a859a 148 void InitAccelerometers(void) {
gke 0:62a1c91a859a 149 static uint8 a;
gke 0:62a1c91a859a 150
gke 0:62a1c91a859a 151 F.AccelerationsValid = true; // optimistic
gke 0:62a1c91a859a 152
gke 0:62a1c91a859a 153 for ( a = 0; a < (uint8)3; a++ ) {
gke 0:62a1c91a859a 154 NewAccNeutral[a] = Acc[a] = Accp[a] = Vel[a] = 0.0;
gke 0:62a1c91a859a 155 Comp[a] = 0;
gke 0:62a1c91a859a 156 }
gke 0:62a1c91a859a 157 Acc[2] = Accp[2] = 1.0;
gke 0:62a1c91a859a 158
gke 0:62a1c91a859a 159 if ( ADXL345AccActive() ) {
gke 0:62a1c91a859a 160 InitADXL345Acc();
gke 0:62a1c91a859a 161 AccelerometerType = ADXL345Acc;
gke 0:62a1c91a859a 162
gke 0:62a1c91a859a 163 } else
gke 0:62a1c91a859a 164 if ( LISLAccActive() )
gke 0:62a1c91a859a 165 AccelerometerType = LISLAcc;
gke 0:62a1c91a859a 166 else
gke 0:62a1c91a859a 167 // check for other accs in preferred order
gke 0:62a1c91a859a 168 {
gke 0:62a1c91a859a 169 AccelerometerType = AccUnknown;
gke 0:62a1c91a859a 170 F.AccelerationsValid = false;
gke 0:62a1c91a859a 171 }
gke 0:62a1c91a859a 172
gke 0:62a1c91a859a 173 if ( F.AccelerationsValid ) {
gke 0:62a1c91a859a 174 LEDYellow_ON;
gke 0:62a1c91a859a 175 GetNeutralAccelerations();
gke 0:62a1c91a859a 176 LEDYellow_OFF;
gke 0:62a1c91a859a 177 } else
gke 0:62a1c91a859a 178 F.AccFailure = true;
gke 0:62a1c91a859a 179 } // InitAccelerometers
gke 0:62a1c91a859a 180
gke 0:62a1c91a859a 181 //________________________________________________________________________________________
gke 0:62a1c91a859a 182
gke 0:62a1c91a859a 183 // ADXL345 3 Axis Accelerometer
gke 0:62a1c91a859a 184
gke 0:62a1c91a859a 185 void ReadADXL345Acc(void);
gke 0:62a1c91a859a 186 void InitADXL345Acc(void);
gke 0:62a1c91a859a 187 boolean ADXL345AccActive(void);
gke 0:62a1c91a859a 188
gke 0:62a1c91a859a 189 const float GRAVITY_ADXL345 = 250.0; // ~4mG/LSB
gke 0:62a1c91a859a 190
gke 0:62a1c91a859a 191 void ReadADXL345Acc(void) {
gke 0:62a1c91a859a 192
gke 0:62a1c91a859a 193 static uint8 a, r;
gke 0:62a1c91a859a 194 static char b[6];
gke 0:62a1c91a859a 195 static i16u X, Y, Z;
gke 0:62a1c91a859a 196
gke 0:62a1c91a859a 197 /*
gke 0:62a1c91a859a 198 r = 0;
gke 0:62a1c91a859a 199 while ( r == 0 ) {
gke 0:62a1c91a859a 200 I2CACC.start();
gke 0:62a1c91a859a 201 r = I2CACC.write(ADXL345_ID);
gke 0:62a1c91a859a 202 r = I2CACC.write(0x30);
gke 0:62a1c91a859a 203 r = I2CACC.read(true) & 0x80;
gke 0:62a1c91a859a 204 I2CACC.stop();
gke 0:62a1c91a859a 205 }
gke 0:62a1c91a859a 206 */
gke 0:62a1c91a859a 207
gke 0:62a1c91a859a 208 I2CACC.start();
gke 0:62a1c91a859a 209 r = I2CACC.write(ADXL345_ID);
gke 0:62a1c91a859a 210 r = I2CACC.write(0x32); // point to acc data
gke 0:62a1c91a859a 211 I2CACC.stop();
gke 0:62a1c91a859a 212
gke 0:62a1c91a859a 213 I2CACC.read(ADXL345_ID, b, 6);
gke 0:62a1c91a859a 214
gke 0:62a1c91a859a 215 X.b1 = b[1];
gke 0:62a1c91a859a 216 X.b0 = b[0];
gke 0:62a1c91a859a 217 Y.b1 = b[3];
gke 0:62a1c91a859a 218 Y.b0 =b[2];
gke 0:62a1c91a859a 219 Z.b1 = b[5];
gke 0:62a1c91a859a 220 Z.b0 = b[4];
gke 0:62a1c91a859a 221
gke 0:62a1c91a859a 222 if ( F.Using9DOF ) { // SparkFun/QuadroUFO 9DOF breakouts pins forward components up
gke 0:62a1c91a859a 223 Acc[BF] = -Y.i16;
gke 0:62a1c91a859a 224 Acc[LR] = -X.i16;
gke 0:62a1c91a859a 225 Acc[UD] = -Z.i16;
gke 0:62a1c91a859a 226 } else {// SparkFun 6DOF breakouts pins forward components down
gke 0:62a1c91a859a 227 Acc[BF] = -X.i16;
gke 0:62a1c91a859a 228 Acc[LR] = -Y.i16;
gke 0:62a1c91a859a 229 Acc[UD] = -Z.i16;
gke 0:62a1c91a859a 230 }
gke 0:62a1c91a859a 231
gke 0:62a1c91a859a 232 // LP filter here?
gke 0:62a1c91a859a 233
gke 0:62a1c91a859a 234 for ( a = 0; a < (int8)3; a++ )
gke 0:62a1c91a859a 235 Acc[a] /= GRAVITY_ADXL345;
gke 0:62a1c91a859a 236
gke 0:62a1c91a859a 237 } // ReadADXL345Acc
gke 0:62a1c91a859a 238
gke 0:62a1c91a859a 239 void InitADXL345Acc() {
gke 0:62a1c91a859a 240 static uint8 r;
gke 0:62a1c91a859a 241
gke 0:62a1c91a859a 242 I2CACC.start();
gke 0:62a1c91a859a 243 I2CACC.write(ADXL345_W);
gke 0:62a1c91a859a 244 r = I2CACC.write(0x2D); // power register
gke 0:62a1c91a859a 245 r = I2CACC.write(0x08); // measurement mode
gke 0:62a1c91a859a 246 I2CACC.stop();
gke 0:62a1c91a859a 247
gke 0:62a1c91a859a 248 Delay1mS(5);
gke 0:62a1c91a859a 249
gke 0:62a1c91a859a 250 I2CACC.start();
gke 0:62a1c91a859a 251 r = I2CACC.write(ADXL345_W);
gke 0:62a1c91a859a 252 r = I2CACC.write(0x31); // format
gke 0:62a1c91a859a 253 r = I2CACC.write(0x08); // full resolution, 2g
gke 0:62a1c91a859a 254 I2CACC.stop();
gke 0:62a1c91a859a 255
gke 0:62a1c91a859a 256 Delay1mS(5);
gke 0:62a1c91a859a 257
gke 0:62a1c91a859a 258 I2CACC.start();
gke 0:62a1c91a859a 259 r = I2CACC.write(ADXL345_W);
gke 0:62a1c91a859a 260 r = I2CACC.write(0x2C); // Rate
gke 0:62a1c91a859a 261 r = I2CACC.write(0x09); // 50Hz, 400Hz 0x0C
gke 0:62a1c91a859a 262 I2CACC.stop();
gke 0:62a1c91a859a 263
gke 0:62a1c91a859a 264 Delay1mS(5);
gke 0:62a1c91a859a 265
gke 0:62a1c91a859a 266 } // InitADXL345Acc
gke 0:62a1c91a859a 267
gke 0:62a1c91a859a 268 boolean ADXL345AccActive(void) {
gke 0:62a1c91a859a 269
gke 0:62a1c91a859a 270 I2CACC.start();
gke 0:62a1c91a859a 271 F.AccelerationsValid = I2CACC.write(ADXL345_ID) == I2C_ACK;
gke 0:62a1c91a859a 272 I2CACC.stop();
gke 0:62a1c91a859a 273
gke 0:62a1c91a859a 274 TrackMinI2CRate(400000);
gke 0:62a1c91a859a 275
gke 0:62a1c91a859a 276 return( true ); //zzz F.AccelerationsValid );
gke 0:62a1c91a859a 277
gke 0:62a1c91a859a 278 } // ADXL345AccActive
gke 0:62a1c91a859a 279
gke 0:62a1c91a859a 280 //________________________________________________________________________________________
gke 0:62a1c91a859a 281
gke 0:62a1c91a859a 282 // LIS3LV02DG Accelerometer 400KHz
gke 0:62a1c91a859a 283
gke 0:62a1c91a859a 284 void WriteLISL(uint8, uint8);
gke 0:62a1c91a859a 285 void ReadLISLAcc(void);
gke 0:62a1c91a859a 286 boolean LISLAccActive(void);
gke 0:62a1c91a859a 287
gke 0:62a1c91a859a 288 const float GRAVITY_LISL = 1024.0;
gke 0:62a1c91a859a 289
gke 0:62a1c91a859a 290 void ReadLISLAcc(void) {
gke 0:62a1c91a859a 291 static uint8 a;
gke 0:62a1c91a859a 292 static char b[6];
gke 0:62a1c91a859a 293 static i16u X, Y, Z;
gke 0:62a1c91a859a 294
gke 0:62a1c91a859a 295 F.AccelerationsValid = I2CACC.write(LISL_ID) == I2C_ACK; // Acc still there?
gke 0:62a1c91a859a 296 if ( F.AccelerationsValid ) {
gke 0:62a1c91a859a 297
gke 0:62a1c91a859a 298 // Ax.b0 = I2CACC.write(LISL_OUTX_L + LISL_INCR_ADDR + LISL_READ);
gke 0:62a1c91a859a 299
gke 0:62a1c91a859a 300 I2CACC.read(LISL_ID, b, 6);
gke 0:62a1c91a859a 301
gke 0:62a1c91a859a 302 X.b1 = b[1];
gke 0:62a1c91a859a 303 X.b0 = b[0];
gke 0:62a1c91a859a 304 Y.b1 = b[3];
gke 0:62a1c91a859a 305 Y.b0 = b[2];
gke 0:62a1c91a859a 306 Z.b1 = b[5];
gke 0:62a1c91a859a 307 Z.b0 = b[4];
gke 0:62a1c91a859a 308
gke 0:62a1c91a859a 309 Acc[BF] = Z.i16;
gke 0:62a1c91a859a 310 Acc[LR] = -X.i16;
gke 0:62a1c91a859a 311 Acc[UD] = Y.i16;
gke 0:62a1c91a859a 312
gke 0:62a1c91a859a 313 // LP Filter here?
gke 0:62a1c91a859a 314
gke 0:62a1c91a859a 315 for ( a = 0; a < (uint8)3; a++ )
gke 0:62a1c91a859a 316 Acc[a] /= GRAVITY_LISL;
gke 0:62a1c91a859a 317
gke 0:62a1c91a859a 318 } else {
gke 0:62a1c91a859a 319 for ( a = 0; a < (uint8)3; a++ )
gke 0:62a1c91a859a 320 Acc[a] = AccNeutral[a];
gke 0:62a1c91a859a 321 Acc[UD] += 1.0;
gke 0:62a1c91a859a 322
gke 0:62a1c91a859a 323 if ( State == InFlight ) {
gke 0:62a1c91a859a 324 Stats[AccFailS]++; // data over run - acc out of range
gke 0:62a1c91a859a 325 // use neutral values!!!!
gke 0:62a1c91a859a 326 F.AccFailure = true;
gke 0:62a1c91a859a 327 }
gke 0:62a1c91a859a 328 }
gke 0:62a1c91a859a 329 } // ReadLISLAccelerometers
gke 0:62a1c91a859a 330
gke 0:62a1c91a859a 331 void WriteLISL(uint8 d, uint8 a) {
gke 0:62a1c91a859a 332 I2CACC.start();
gke 0:62a1c91a859a 333 I2CACC.write(a);
gke 0:62a1c91a859a 334 I2CACC.write(d);
gke 0:62a1c91a859a 335 I2CACC.stop();
gke 0:62a1c91a859a 336 } // WriteLISL
gke 0:62a1c91a859a 337
gke 0:62a1c91a859a 338 boolean LISLAccActive(void) {
gke 0:62a1c91a859a 339 F.AccelerationsValid = false;
gke 0:62a1c91a859a 340 /*
gke 0:62a1c91a859a 341 WriteLISL(0x4a, LISL_CTRLREG_2); // enable 3-wire, BDU=1, +/-2g
gke 0:62a1c91a859a 342
gke 0:62a1c91a859a 343 if ( I2CACC.write(LISL_ID) == I2C_ACK ) {
gke 0:62a1c91a859a 344 WriteLISL(0xc7, LISL_CTRLREG_1); // on always, 40Hz sampling rate, 10Hz LP cutoff, enable all axes
gke 0:62a1c91a859a 345 WriteLISL(0, LISL_CTRLREG_3);
gke 0:62a1c91a859a 346 WriteLISL(0x40, LISL_FF_CFG); // latch, no interrupts;
gke 0:62a1c91a859a 347 WriteLISL(0, LISL_FF_THS_L);
gke 0:62a1c91a859a 348 WriteLISL(0xFC, LISL_FF_THS_H); // -0,5g threshold
gke 0:62a1c91a859a 349 WriteLISL(255, LISL_FF_DUR);
gke 0:62a1c91a859a 350 WriteLISL(0, LISL_DD_CFG);
gke 0:62a1c91a859a 351 F.AccelerationsValid = true;
gke 0:62a1c91a859a 352 } else
gke 0:62a1c91a859a 353 F.AccFailure = true;
gke 0:62a1c91a859a 354 */
gke 0:62a1c91a859a 355
gke 0:62a1c91a859a 356 TrackMinI2CRate(400000);
gke 0:62a1c91a859a 357
gke 0:62a1c91a859a 358 return ( false );//F.AccelerationsValid );
gke 0:62a1c91a859a 359 } // LISLAccActive
gke 0:62a1c91a859a 360
gke 0:62a1c91a859a 361
gke 0:62a1c91a859a 362