Prof Greg Egan
UAVX Multicopter Flight Controller.
Diff: gyro.c
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
--- a/gyro.c Fri Feb 25 01:35:24 2011 +0000
+++ b/gyro.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.
@@ -20,12 +20,12 @@
#include "UAVXArm.h"
const real32 GyroToRadian[UnknownGyro] = {
- 0.023841, // MLX90609
- 0.044680, // ADXRS150
- 0.007149, // IDG300
- 0.011796, // LY530
- 0.017872, // ADXRS300
- 0.000607, // ITG3200
+ 8.635062, // MLX90609
+ 4.607669, // ADXRS150
+ 28.797933, // IDG300
+ 17.453293, // LY530
+ 11.519173, // ADXRS300
+ 0.000438704 * 2.0 * 1.31, // ITG3200 16bit 2's complement
1.0 // Infrared Sensors
// add others as required
};
@@ -38,15 +38,41 @@
void GyroTest(void);
void ShowGyroType(void);
-real32 GyroADC[3], GyroNeutral[3], Gyro[3]; // Radians
+real32 GyroADC[3], GyroNoise[3], GyroNeutral[3], Gyro[3], Gyrop[3]; // Radians
uint8 GyroType;
void GetGyroRates(void) {
static uint8 g;
+ static real32 d, GyroA;
ReadGyros();
+
+ for ( g = 0; g < (uint8)3; g++ ) {
+ d = fabs(Gyro[g]-Gyrop[g]);
+ if ( d > GyroNoise[g] ) GyroNoise[g] = d;
+ }
+
+#ifndef SUPPRESS_ROLL_PITCH_GYRO_FILTERS
+ // dT is almost unchanged so this could be optimised
+ GyroA = dT / ( 1.0 / ( TWOPI * ROLL_PITCH_FREQ ) + dT );
+ Gyro[Roll] = LPFilter( Gyro[Roll] - GyroNeutral[Roll], Gyrop[Roll], GyroA );
+ Gyro[Pitch] = LPFilter( Gyro[Pitch] - GyroNeutral[Pitch], Gyrop[Pitch], GyroA );
+#endif // !SUPPRESS_ROLL_PITCH_GYRO_FILTERS
+
+#ifndef SUPPRESS_YAW_GYRO_FILTERS
+
+#ifdef USE_FIXED_YAW_FILTER
+ GyroA = dT / ( 1.0 / ( TWOPI * MAX_YAW_FREQ ) + dT );
+#else
+ GyroA = dT / ( 1.0 / ( TWOPI * YawFilterLPFreq ) + dT );
+#endif // USE_FIXED_YAW_FILTER
+
+ Gyro[Yaw] = LPFilter( Gyro[Yaw] - GyroNeutral[Yaw], Gyrop[Yaw], GyroA );
+#endif // !SUPPRESS_GYRO_FILTERS
+
for ( g = 0; g < (uint8)3; g++ )
- Gyro[g] = ( GyroADC[g] - GyroNeutral[g] ) ;
+ Gyrop[g] = Gyro[g];
+
} // GetGyroRates
void ReadGyros(void) {
@@ -64,24 +90,26 @@
} // ReadGyros
void ErectGyros(void) {
- static int16 i, g;
+ static uint8 s, i, g;
LEDRed_ON;
- for ( g = 0; g <(int8)3; g++ )
+ for ( g = 0; g <(uint8)3; g++ )
GyroNeutral[g] = 0.0;
for ( i = 0; i < 32 ; i++ ) {
Delay1mS(10);
ReadGyros();
- for ( g = 0; g <(int8)3; g++ )
- GyroNeutral[g] += GyroADC[g];
+ for ( g = 0; g <(uint8)3; g++ )
+ GyroNeutral[g] += Gyro[g];
}
- for ( g = 0; g <(int8)3; g++ ) {
+ for ( g = 0; g <(uint8)3; g++ ) {
GyroNeutral[g] *= 0.03125;
- Gyro[g] = 0.0;
+ Gyro[g] = Gyrop[g] = 0.0;
+ for ( s = 0; s < MaxAttitudeScheme; s++ )
+ EstAngle[g][s] = EstRate[g][s] = 0.0;
}
LEDRed_OFF;
@@ -94,18 +122,21 @@
ReadGyros();
+ TxString("\r\n\tRate and Max Delta(Deg./Sec.)\r\n");
+
TxString("\r\n\tRoll: \t");
- TxVal32(GyroADC[Roll] * 1000.0, 3, 0);
+ TxVal32(Gyro[Roll] * MILLIANGLE, 3, HT);
+ TxVal32(GyroNoise[Roll] * MILLIANGLE, 3, 0);
TxNextLine();
TxString("\tPitch: \t");
- TxVal32(GyroADC[Pitch] * 1000.0, 3, 0);
+ TxVal32(Gyro[Pitch] * MILLIANGLE, 3, HT);
+ TxVal32(GyroNoise[Pitch] * MILLIANGLE, 3, 0);
TxNextLine();
TxString("\tYaw: \t");
- TxVal32(GyroADC[Yaw] * 1000.0, 3, 0);
+ TxVal32(Gyro[Yaw] * MILLIANGLE, 3, HT);
+ TxVal32(GyroNoise[Yaw] * MILLIANGLE, 3, 0);
TxNextLine();
- TxString("Expect ~0.0 ( ~0.5 for for analog gyros)\r\n");
-
switch ( GyroType ) {
case ITG3200Gyro:
ITG3200Test();
@@ -120,6 +151,7 @@
} // GyroTest
void InitGyros(void) {
+
if ( ITG3200GyroActive() )
GyroType = ITG3200Gyro;
else
@@ -182,12 +214,13 @@
void ReadAnalogGyros(void) {
static uint8 g;
- GyroADC[Roll] = RollADC.read();
- GyroADC[Pitch] = PitchADC.read();
+ GyroADC[Roll] = -RollADC.read();
+ GyroADC[Pitch] = -PitchADC.read();
GyroADC[Yaw] = YawADC.read();
for ( g = 0; g < (uint8)3; g++ )
- GyroADC[g] *= GyroToRadian[GyroType];
+ Gyro[g] = GyroADC[g] * GyroToRadian[GyroType];
+
} // ReadAnalogGyros
void InitAnalogGyros(void) {
@@ -201,7 +234,7 @@
void ReadITG3200(void);
uint8 ReadByteITG3200(uint8);
-void WriteByteITG3200(uint8, uint8);
+boolean WriteByteITG3200(uint8, uint8);
void InitITG3200(void);
void ITG3200Test(void);
boolean ITG3200Active(void);
@@ -210,41 +243,45 @@
void ReadITG3200Gyro(void) {
static char G[6];
- static uint8 g, r;
+ static uint8 g;
static i16u GX, GY, GZ;
I2CGYRO.start();
- r = ( I2CGYRO.write(ITG3200_WR) != I2C_ACK );
- r = ( I2CGYRO.write(ITG3200_GX_H) != I2C_ACK );
+ if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto ITG3200Error;
+ if ( I2CGYRO.write(ITG3200_GX_H) != I2C_ACK ) goto ITG3200Error;
I2CGYRO.stop();
- if ( I2CGYRO.blockread(ITG3200_ID, G, 6) == 0 ) {
+ if ( I2CGYRO.blockread(ITG3200_ID, G, 6) ) goto ITG3200Error;
- GX.b0 = G[1];
- GX.b1 = G[0];
- GY.b0 = G[3];
- GY.b1 = G[2];
- GZ.b0 = G[5];
- GZ.b1 = G[4];
+ GX.b0 = G[1];
+ GX.b1 = G[0];
+ GY.b0 = G[3];
+ GY.b1 = G[2];
+ GZ.b0 = G[5];
+ GZ.b1 = G[4];
- if ( F.Using9DOF ) { // SparkFun/QuadroUFO breakout pins forward components up
- GyroADC[Roll] = -(real32)GY.i16;
- GyroADC[Pitch] = -(real32)GX.i16;
- GyroADC[Yaw] = -(real32)GZ.i16;
- } else { // SparkFun 6DOF breakout pins forward components down
- GyroADC[Roll] = -(real32)GX.i16;
- GyroADC[Pitch] = -(real32)GY.i16;
- GyroADC[Yaw] = (real32)GZ.i16;
- }
+ if ( F.Using9DOF ) { // SparkFun/QuadroUFO breakout pins forward components up
+ GyroADC[Roll] = -(real32)GY.i16;
+ GyroADC[Pitch] = -(real32)GX.i16;
+ GyroADC[Yaw] = -(real32)GZ.i16;
+ } else { // SparkFun 6DOF breakout pins forward components down
+ GyroADC[Roll] = -(real32)GX.i16;
+ GyroADC[Pitch] = -(real32)GY.i16;
+ GyroADC[Yaw] = (real32)GZ.i16;
+ }
- for ( g = 0; g < (uint8)3; g++ )
- GyroADC[g] *= GyroToRadian[ITG3200Gyro];
+ for ( g = 0; g < (uint8)3; g++ )
+ Gyro[g] = GyroADC[g] * GyroToRadian[ITG3200Gyro];
+
+ return;
- } else {
- // GYRO FAILURE - FATAL
- Stats[GyroFailS]++;
- // not in flight keep trying F.GyroFailure = true;
- }
+ITG3200Error:
+ I2CGYRO.stop();
+
+ I2CError[ITG3200_ID]++;
+
+ Stats[GyroFailS]++; // not in flight keep trying
+ F.GyroFailure = true;
} // ReadITG3200Gyro
@@ -252,53 +289,76 @@
static uint8 d;
I2CGYRO.start();
- if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto SGerror;
- if ( I2CGYRO.write(a) != I2C_ACK ) goto SGerror;
-
+ if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto ITG3200Error;
+ if ( I2CGYRO.write(a) != I2C_ACK ) goto ITG3200Error;
I2CGYRO.start();
- if ( I2CGYRO.write(ITG3200_RD) != I2C_ACK ) goto SGerror;
+ if ( I2CGYRO.write(ITG3200_RD) != I2C_ACK ) goto ITG3200Error;
d = I2CGYRO.read(I2C_NACK);
I2CGYRO.stop();
return ( d );
-SGerror:
+ITG3200Error:
I2CGYRO.stop();
+
+ I2CError[ITG3200_ID]++;
+ // GYRO FAILURE - FATAL
+ Stats[GyroFailS]++;
+
+ //F.GyroFailure = true;
+
+ return ( 0 );
+
+} // ReadByteITG3200
+
+boolean WriteByteITG3200(uint8 a, uint8 d) {
+
+ I2CGYRO.start(); // restart
+ if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto ITG3200Error;
+ if ( I2CGYRO.write(a) != I2C_ACK ) goto ITG3200Error;
+ if ( I2CGYRO.write(d) != I2C_ACK ) goto ITG3200Error;
+ I2CGYRO.stop();
+
+ return(false);
+
+ITG3200Error:
+ I2CGYRO.stop();
+
+ I2CError[ITG3200_ID]++;
// GYRO FAILURE - FATAL
Stats[GyroFailS]++;
F.GyroFailure = true;
- return (I2C_NACK);
-} // ReadByteITG3200
-void WriteByteITG3200(uint8 a, uint8 d) {
- I2CGYRO.start(); // restart
- if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto SGerror;
- if ( I2CGYRO.write(a) != I2C_ACK ) goto SGerror;
- if ( I2CGYRO.write(d) != I2C_ACK ) goto SGerror;
- I2CGYRO.stop();
- return;
+ return(true);
-SGerror:
- I2CGYRO.stop();
- // GYRO FAILURE - FATAL
- Stats[GyroFailS]++;
- F.GyroFailure = true;
- return;
} // WriteByteITG3200
void InitITG3200Gyro(void) {
- F.GyroFailure = false; // reset optimistically!
+
+#define FS_SEL 3
- WriteByteITG3200(ITG3200_PWR_M, 0x80); // Reset to defaults
- WriteByteITG3200(ITG3200_SMPL, 0x00); // continuous update
- WriteByteITG3200(ITG3200_DLPF, 0x19); // 188Hz, 2000deg/S
- WriteByteITG3200(ITG3200_INT_C, 0x00); // no interrupts
- WriteByteITG3200(ITG3200_PWR_M, 0x01); // X Gyro as Clock Ref.
+//#define DLPF_CFG 1 // 188HZ
+#define DLPF_CFG 2 // 98HZ
+//#define DLPF_CFG 3 // 42HZ
+
+ if ( WriteByteITG3200(ITG3200_PWR_M, 0x80) ) goto ITG3200Error; // Reset to defaults
+ if ( WriteByteITG3200(ITG3200_SMPL, 0x00) ) goto ITG3200Error; // continuous update
+ if ( WriteByteITG3200(ITG3200_DLPF, (FS_SEL << 3) | DLPF_CFG ) ) goto ITG3200Error; // 188Hz, 2000deg/S
+ if ( WriteByteITG3200(ITG3200_INT_C, 0x00) ) goto ITG3200Error; // no interrupts
+ if ( WriteByteITG3200(ITG3200_PWR_M, 0x01) ) goto ITG3200Error; // X Gyro as Clock Ref.
Delay1mS(50);
+ F.GyroFailure = false;
+
ReadITG3200Gyro();
+ return;
+
+ITG3200Error:
+
+ F.GyroFailure = true;
+
} // InitITG3200Gyro
void ITG3200Test(void) {