Prof Greg Egan
UAVX Multicopter Flight Controller.
Diff: compass.c
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
--- a/compass.c Fri Feb 25 01:35:24 2011 +0000
+++ b/compass.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.
@@ -23,9 +23,9 @@
// Local magnetic declination not included
// http://www.ngdc.noaa.gov/geomagmodels/Declination.jsp
-
void ReadCompass(void);
void GetHeading(void);
+real32 MinimumTurn(real32);
void CalibrateCompass(void);
void ShowCompassType(void);
void DoCompassTest(void);
@@ -33,10 +33,13 @@
MagStruct Mag[3] = {{ 0,0 },{ 0,0 },{ 0,0 }};
real32 MagDeviation, CompassOffset;
-real32 MagHeading, Heading, Headingp, FakeHeading;
+real32 MagHeading, Heading, HeadingP, FakeHeading;
real32 HeadingSin, HeadingCos;
+real32 CompassMaxSlew;
uint8 CompassType;
+enum MagCoords { MX, MY, MZ };
+
void ReadCompass(void) {
switch ( CompassType ) {
case HMC5843:
@@ -94,16 +97,25 @@
void GetHeading(void) {
- const real32 CompassA = COMPASS_UPDATE_S / ( OneOnTwoPiCompassF + COMPASS_UPDATE_S );
+ static real32 HeadingChange, CompassA;
ReadCompass();
- Heading = Make2Pi( MagHeading - MagDeviation - CompassOffset );
- if ( fabs(Heading - Headingp ) > PI )
- Headingp = Heading;
+ Heading = Make2Pi( MagHeading + MagDeviation - CompassOffset );
+ HeadingChange = fabs( Heading - HeadingP );
+ if ( HeadingChange > ONEANDHALFPI ) // wrap 0 -> TwoPI
+ HeadingP = Heading;
+ else
+ if ( HeadingChange > CompassMaxSlew ) { // Sanity check - discard reading
+ Heading =SlewLimit(HeadingP, Heading, CompassMaxSlew );
+ Stats[CompassFailS]++;
+ }
- Heading = Headingp + (Heading - Headingp) * CompassA;
- Headingp = Heading;
+#ifndef SUPPRESS_COMPASS_FILTER
+ CompassA = dT / ( 1.0 / ( TWOPI * YawFilterLPFreq ) + dT );
+ Heading = Make2Pi( LPFilter(Heading, HeadingP, CompassA) );
+#endif // !SUPPRESS_COMPASS_FILTER
+ HeadingP = Heading;
#ifdef SIMULATE
#if ( defined AILERON | defined ELEVON )
@@ -121,6 +133,24 @@
#endif // SIMULATE
} // GetHeading
+//boolean DirectionSelected = false;
+//real32 DirectionSense = 1.0;
+
+real32 MinimumTurn(real32 A ) {
+
+ static real32 AbsA;
+
+ AbsA = fabs(A);
+ if ( AbsA > PI )
+ A = ( AbsA - TWOPI ) * Sign(A);
+
+ //DirectionSelected = fabs(A) > THIRDPI; // avoid dithering around reciprocal heading
+ //DirectionSense = Sign(A);
+
+ return ( A );
+
+} // MinimumTurn
+
void InitCompass(void) {
if ( IsHMC5843Active() )
CompassType = HMC5843;
@@ -145,7 +175,7 @@
ReadCompass();
mS[CompassUpdate] = mSClock();
- Heading = Headingp = Make2Pi( MagHeading - MagDeviation - CompassOffset );
+ Heading = HeadingP = Make2Pi( MagHeading - MagDeviation - CompassOffset );
} // InitCompass
@@ -163,16 +193,15 @@
void ReadHMC5843(void) {
static char b[6];
static i16u X, Y, Z;
- static uint8 r;
- static real32 mx, my;
+ static real32 FX,FY;
static real32 CRoll, SRoll, CPitch, SPitch;
I2CCOMPASS.start();
- r = I2CCOMPASS.write(HMC5843_WR);
- r = I2CCOMPASS.write(0x03); // point to data
+ if ( I2CCOMPASS.write(HMC5843_WR) != I2C_ACK ) goto HMC5843Error;
+ if ( I2CCOMPASS.write(0x03) != I2C_ACK ) goto HMC5843Error; // point to data
I2CCOMPASS.stop();
- I2CCOMPASS.blockread(HMC5843_RD, b, 6);
+ if ( I2CCOMPASS.blockread(HMC5843_RD, b, 6) ) goto HMC5843Error;
X.b1 = b[0];
X.b0 = b[1];
@@ -192,55 +221,180 @@
}
DebugPin = true;
CRoll = cos(Angle[Roll]);
- SRoll = sin(Angle[Roll]);
+ SRoll = sin(Angle[Roll]); // Acc[LR] - optimisation not worthwhile
CPitch = cos(Angle[Pitch]);
- SPitch = sin(Angle[Pitch]);
+ SPitch = sin(Angle[Pitch]); // Acc[BF]
- mx = (Mag[BF].V-Mag[BF].Offset) * CPitch + (Mag[LR].V-Mag[LR].Offset) * SRoll * SPitch + (Mag[UD].V-Mag[UD].Offset) * CRoll * SPitch;
- my = (Mag[LR].V-Mag[LR].Offset) * CRoll - (Mag[UD].V-Mag[UD].Offset) * SRoll;
+ FX = (Mag[BF].V-Mag[BF].Offset) * CPitch + (Mag[LR].V-Mag[LR].Offset) * SRoll * SPitch + (Mag[UD].V-Mag[UD].Offset) * CRoll * SPitch;
+ FY = (Mag[LR].V-Mag[LR].Offset) * CRoll - (Mag[UD].V-Mag[UD].Offset) * SRoll;
// Magnetic Heading
- MagHeading = MakePi(atan2( -my, mx ));
+ MagHeading = MakePi(atan2( -FY, FX ));
+
DebugPin = false;
F.CompassValid = true;
+
return;
+HMC5843Error:
+ I2CCOMPASS.stop();
+
+ I2CError[HMC5843_ID]++;
+ if ( State == InFlight ) Stats[CompassFailS]++;
+
+ F.CompassMissRead = true;
+ F.CompassValid = false;
+
} // ReadHMC5843
+real32 magFieldEarth[3], magFieldBody[3], dcmMatrix[9];
+boolean firstPassMagOffset = true;
+
void CalibrateHMC5843(void) {
-} // DoHMC5843Test
+ /*
+ void magOffsetCalc()
+ {
+ int16 i, j ;
+ static real32 tempMatrix[3] ;
+ static real32 offsetSum[3] ;
+
+ // Compute magnetic field of the earth
+
+ magFieldEarth[0] = vectorDotProduct(3, &dcmMatrix[0], &magFieldBody[0]);
+ magFieldEarth[1] = vectorDotProduct(3, &dcmMatrix[3], &magFieldBody[0]);
+ magFieldEarth[2] = vectorDotProduct(3, &dcmMatrix[6], &magFieldBody[0]);
+
+ // First pass thru?
+
+ if ( firstPassMagOffset )
+ {
+ setPastValues(); // Yes, set initial values for previous values
+ firstPassMagOffset = false; // Clear first pass flag
+ }
+
+ // Compute the offsets in the magnetometer:
+ vectorAdd(3, offsetSum , magFieldBody, magFieldBodyPrevious) ;
+
+ matrixMultiply(1, 3, 3, tempMatrix, magFieldEarthPrevious, dcmMatrix);
+ vectorSubtract(3, offsetSum, offsetSum, tempMatrix);
+ matrixMultiply(1, 3, 3, tempMatrix, magFieldEarth, dcmMatrixPrevious);
+ vectorSubtract(3, offsetSum, offsetSum, tempMatrix) ;
+
+ for ( i = 0 ; i < 3 ; i++ )
+ if ( abs(offsetSum[i] ) < 3 )
+ offsetSum[i] = 0 ;
+
+ vectorAdd (3, magOffset, magOffset, offsetSum);
+ setPastValues();
+ }
+
+ void setPastValues()
+ {
+ static uint8 i;
+
+ for ( i = 0; i < 3; i++)
+ {
+ magFieldEarthPrevious[i] = magFieldEarth[i];
+ magFieldBodyPrevious[i] = magFieldBody[i];
+ }
+
+ for ( i = 0; i < 9; i++)
+ dcmMatrixPrevious[i] = dcmMatrix[i];
+
+ }
+ */
+} // CalibrateHMC5843
+
+void GetHMC5843Parameters(void) {
+
+ static char CP[16];
+ static uint8 i;
+
+ I2CCOMPASS.start();
+ if ( I2CCOMPASS.write(HMC5843_WR) != I2C_ACK) goto HMC5843Error;
+ if ( I2CCOMPASS.write(0x00) != I2C_ACK) goto HMC5843Error; // point to data
+ I2CCOMPASS.stop();
+
+ if ( I2CCOMPASS.blockread(HMC5843_RD, CP, 13) ) goto HMC5843Error;
+
+ for ( i = 0; i < (uint8)13; i++ ) {
+ TxVal32(i,0,0);
+ TxString(":\t0x");
+ TxValH(CP[i]);
+ TxChar(HT);
+ TxBin8(CP[i]);
+ TxNextLine();
+ }
+
+ return;
+
+HMC5843Error:
+ I2CCOMPASS.stop();
+
+ I2CError[HMC5843_ID]++;
+
+} // GetHMC5843Parameters
void DoHMC5843Test(void) {
+
TxString("\r\nCompass test (HMC5843)\r\n\r\n");
ReadHMC5843();
- TxString("Mag:\t");
+ GetHMC5843Parameters();
+
+ TxString("\r\nMag:\t");
+ TxVal32(Mag[BF].V, 0, HT);
TxVal32(Mag[LR].V, 0, HT);
- TxVal32(Mag[BF].V, 0, HT);
TxVal32(Mag[UD].V, 0, HT);
TxNextLine();
TxNextLine();
- TxVal32(MagHeading * RADDEG * 10.0, 1, 0);
- TxString(" deg (Magnetic)\r\n");
+ TxVal32(MagHeading * RADDEG * 10.0, 1, ' ');
+ TxString("deg (Magnetic)\r\n");
+
+ Heading = HeadingP = Make2Pi( MagHeading + MagDeviation - CompassOffset );
+ TxVal32(Heading * RADDEG * 10.0, 1, ' ');
+ TxString("deg (True)\r\n");
+} // DoHMC5843Test
+
+boolean WriteByteHMC5843(uint8 a, uint8 d) {
- Heading = Headingp = Make2Pi( MagHeading - MagDeviation - CompassOffset );
- TxVal32(Heading * RADDEG * 10.0, 1, 0);
- TxString(" deg (True)\r\n");
-} // DoHMC5843Test
+ I2CCOMPASS.start();
+ if ( I2CCOMPASS.write(HMC5843_WR) != I2C_ACK ) goto HMC5843Error;
+ if ( I2CCOMPASS.write(a) != I2C_ACK ) goto HMC5843Error;
+ if ( I2CCOMPASS.write(d) != I2C_ACK ) goto HMC5843Error;
+ I2CCOMPASS.stop();
+
+ return( false );
+
+HMC5843Error:
+ I2CCOMPASS.stop();
+
+ I2CError[HMC5843_ID]++;
+
+ return(true);
+
+} // WriteByteHMC5843
void InitHMC5843(void) {
- static uint8 r;
+
+ CompassMaxSlew = (COMPASS_SANITY_CHECK_RAD_S/HMC5843_UPDATE_S);
+
+ if ( WriteByteHMC5843(0x00, HMC5843_DR << 2) ) goto HMC5843Error; // rate, normal measurement mode
+ if ( WriteByteHMC5843(0x02, 0x00) ) goto HMC5843Error; // mode continuous
- I2CCOMPASS.start();
- r = I2CCOMPASS.write(HMC5843_WR);
- r = I2CCOMPASS.write(0x02);
- r = I2CCOMPASS.write(0x00); // Set continuous mode (default to 10Hz)
+ Delay1mS(50);
+
+ return;
+
+HMC5843Error:
I2CCOMPASS.stop();
- Delay1mS(50);
+ I2CError[HMC5843_ID]++;
+
+ F.CompassValid = false;
} // InitHMC5843Magnetometer
@@ -271,23 +425,38 @@
static i16u v;
I2CCOMPASS.start();
- F.CompassMissRead = I2CCOMPASS.write(HMC6352_RD) != I2C_ACK;
+ if ( I2CCOMPASS.write(HMC6352_RD) != I2C_ACK ) goto HMC6352Error;
v.b1 = I2CCOMPASS.read(I2C_ACK);
v.b0 = I2CCOMPASS.read(I2C_NACK);
I2CCOMPASS.stop();
MagHeading = Make2Pi( ((real32)v.i16 * PI) / 1800.0 - CompassOffset ); // Radians
+
+ return;
+
+HMC6352Error:
+ I2CCOMPASS.stop();
+
+ if ( State == InFlight ) Stats[CompassFailS]++;
+
+ F.CompassMissRead = true;
+
} // ReadHMC6352
uint8 WriteByteHMC6352(uint8 d) {
+
I2CCOMPASS.start();
- if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto WError;
- if ( I2CCOMPASS.write(d) != I2C_ACK ) goto WError;
+ if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
+ if ( I2CCOMPASS.write(d) != I2C_ACK ) goto HMC6352Error;
I2CCOMPASS.stop();
return( I2C_ACK );
-WError:
+
+HMC6352Error:
I2CCOMPASS.stop();
+
+ I2CError[HMC6352_ID]++;
+
return ( I2C_NACK );
} // WriteByteHMC6352
@@ -296,13 +465,13 @@
#define TEST_COMP_OPMODE 0x70 // standby mode to reliably read EEPROM
void GetHMC6352Parameters(void) {
- uint8 r;
+ int16 Temp;
I2CCOMPASS.start();
- if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto CTerror;
- if ( I2CCOMPASS.write('G') != I2C_ACK ) goto CTerror;
- if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto CTerror;
- if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto CTerror;
+ if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
+ if ( I2CCOMPASS.write('G') != I2C_ACK ) goto HMC6352Error;
+ if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto HMC6352Error;
+ if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto HMC6352Error;
I2CCOMPASS.stop();
Delay1mS(20);
@@ -310,50 +479,21 @@
for (r = 0; r <= (uint8)8; r++) { // must have this timing - not block read!
I2CCOMPASS.start();
- if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto CTerror;
- if ( I2CCOMPASS.write('r') != I2C_ACK ) goto CTerror;
- if ( I2CCOMPASS.write(r) != I2C_ACK ) goto CTerror;
+ if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
+ if ( I2CCOMPASS.write('r') != I2C_ACK ) goto HMC6352Error;
+ if ( I2CCOMPASS.write(r) != I2C_ACK ) goto HMC6352Error;
I2CCOMPASS.stop();
Delay1mS(10);
I2CCOMPASS.start();
- if ( I2CCOMPASS.write(HMC6352_RD) != I2C_ACK ) goto CTerror;
+ if ( I2CCOMPASS.write(HMC6352_RD) != I2C_ACK ) goto HMC6352Error;
CP[r] = I2CCOMPASS.read(I2C_NACK);
I2CCOMPASS.stop();
Delay1mS(10);
}
- return;
-
-CTerror:
- I2CCOMPASS.stop();
- TxString("FAIL\r\n");
-
-} // GetHMC6352Parameters
-
-void DoHMC6352Test(void) {
- static real32 Temp;
-
- TxString("\r\nCompass test (HMC6352)\r\n");
-
- I2CCOMPASS.start();
- if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto CTerror;
- if ( I2CCOMPASS.write('G') != I2C_ACK ) goto CTerror;
- if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto CTerror;
- if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto CTerror;
- I2CCOMPASS.stop();
-
- Delay1mS(1);
-
- // I2CCOMPASS.start(); // Do Set/Reset now
- if ( WriteByteHMC6352('O') != I2C_ACK ) goto CTerror;
-
- Delay1mS(7);
-
- GetHMC6352Parameters();
-
TxString("\r\nRegisters\r\n");
TxString("\t0:\tI2C");
TxString("\t 0x");
@@ -419,24 +559,59 @@
}
TxNextLine();
+ return;
+
+HMC6352Error:
+ I2CCOMPASS.stop();
+
+ I2CError[HMC6352_ID]++;
+
+ TxString("FAIL\r\n");
+
+} // GetHMC6352Parameters
+
+void DoHMC6352Test(void) {
+
+ TxString("\r\nCompass test (HMC6352)\r\n");
+
+ I2CCOMPASS.start();
+ if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
+ if ( I2CCOMPASS.write('G') != I2C_ACK ) goto HMC6352Error;
+ if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto HMC6352Error;
+ if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto HMC6352Error;
+ I2CCOMPASS.stop();
+
+ Delay1mS(1);
+
+ // I2CCOMPASS.start(); // Do Set/Reset now
+ if ( WriteByteHMC6352('O') != I2C_ACK ) goto HMC6352Error;
+
+ Delay1mS(7);
+
+ GetHMC6352Parameters();
+
InitCompass();
- if ( !F.CompassValid ) goto CTerror;
+ if ( !F.CompassValid ) goto HMC6352Error;
Delay1mS(50);
ReadHMC6352();
- if ( F.CompassMissRead ) goto CTerror;
+ if ( F.CompassMissRead ) goto HMC6352Error;
TxNextLine();
TxVal32(MagHeading * RADDEG * 10.0, 1, 0);
TxString(" deg (Magnetic)\r\n");
- Heading = Headingp = Make2Pi( MagHeading - MagDeviation - CompassOffset );
+ Heading = HeadingP = Make2Pi( MagHeading - MagDeviation - CompassOffset );
TxVal32(Heading * RADDEG * 10.0, 1, 0);
TxString(" deg (True)\r\n");
return;
-CTerror:
+
+HMC6352Error:
I2CCOMPASS.stop();
+
+ I2CError[HMC6352_ID]++;
+
TxString("FAIL\r\n");
} // DoHMC6352Test
@@ -445,18 +620,18 @@
while ( PollRxChar() != 'x' ); // UAVPSet uses 'x' for CONTINUE button
// Do Set/Reset now
- if ( WriteByteHMC6352('O') != I2C_ACK ) goto CCerror;
+ if ( WriteByteHMC6352('O') != I2C_ACK ) goto HMC6352Error;
Delay1mS(7);
// set Compass device to Calibration mode
- if ( WriteByteHMC6352('C') != I2C_ACK ) goto CCerror;
+ if ( WriteByteHMC6352('C') != I2C_ACK ) goto HMC6352Error;
TxString("\r\nRotate horizontally 720 deg in ~30 sec. - Press CONTINUE button (x) to Finish\r\n");
while ( PollRxChar() != 'x' );
// set Compass device to End-Calibration mode
- if ( WriteByteHMC6352('E') != I2C_ACK ) goto CCerror;
+ if ( WriteByteHMC6352('E') != I2C_ACK ) goto HMC6352Error;
TxString("\r\nCalibration complete\r\n");
@@ -466,7 +641,11 @@
return;
-CCerror:
+HMC6352Error:
+ I2CCOMPASS.stop();
+
+ I2CError[HMC6352_ID]++;
+
TxString("Calibration FAILED\r\n");
} // CalibrateHMC6352
@@ -479,35 +658,41 @@
#define COMP_OPMODE 0x72
#endif // SUPPRESS_COMPASS_SR
+ CompassMaxSlew = (COMPASS_SANITY_CHECK_RAD_S/HMC6352_UPDATE_S);
+
// Set device to Compass mode
I2CCOMPASS.start();
- if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto CTerror;
- if ( I2CCOMPASS.write('G') != I2C_ACK ) goto CTerror;
- if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto CTerror;
- if ( I2CCOMPASS.write(COMP_OPMODE) != I2C_ACK ) goto CTerror;
+ if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
+ if ( I2CCOMPASS.write('G') != I2C_ACK ) goto HMC6352Error;
+ if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto HMC6352Error;
+ if ( I2CCOMPASS.write(COMP_OPMODE) != I2C_ACK ) goto HMC6352Error;
I2CCOMPASS.stop();
- Delay1mS(1);
+ Delay1mS(10);
// save operation mode in Flash
- if ( WriteByteHMC6352('L') != I2C_ACK ) goto CTerror;
+ if ( WriteByteHMC6352('L') != I2C_ACK ) goto HMC6352Error;
- Delay1mS(1);
+ Delay1mS(10);
// Do Bridge Offset Set/Reset now
- if ( WriteByteHMC6352('O') != I2C_ACK ) goto CTerror;
+ if ( WriteByteHMC6352('O') != I2C_ACK ) goto HMC6352Error;
Delay1mS(50);
F.CompassValid = true;
return;
-CTerror:
+
+HMC6352Error:
+ I2CCOMPASS.stop();
+
+ I2CError[HMC6352_ID]++;
+
F.CompassValid = false;
- Stats[CompassFailS]++;
+
F.CompassFailure = true;
- I2CCOMPASS.stop();
} // InitHMC6352
boolean HMC6352Active(void) {