compass.c

00001 // ===============================================================================================
00002 // =                              UAVXArm Quadrocopter Controller                                =
00003 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
00004 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
00005 // =                           http://code.google.com/p/uavp-mods/                               =
00006 // ===============================================================================================
00007 
00008 //    This is part of UAVXArm.
00009 
00010 //    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
00011 //    General Public License as published by the Free Software Foundation, either version 3 of the
00012 //    License, or (at your option) any later version.
00013 
00014 //    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
00015 //    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016 //    See the GNU General Public License for more details.
00017 
00018 //    You should have received a copy of the GNU General Public License along with this program.
00019 //    If not, see http://www.gnu.org/licenses/
00020 
00021 #include "UAVXArm.h"
00022 
00023 // Local magnetic declination not included
00024 // http://www.ngdc.noaa.gov/geomagmodels/Declination.jsp
00025 
00026 void ReadCompass(void);
00027 void GetHeading(void);
00028 real32 MinimumTurn(real32);
00029 void CalibrateCompass(void);
00030 void ShowCompassType(void);
00031 void DoCompassTest(void);
00032 void InitCompass(void);
00033 
00034 MagStruct Mag[3] = {{ 0,0 },{ 0,0 },{ 0,0 }};
00035 real32 MagDeviation, CompassOffset;
00036 real32 MagHeading, Heading, HeadingP, FakeHeading;
00037 real32 HeadingSin, HeadingCos;
00038 real32 CompassMaxSlew;
00039 uint8 CompassType;
00040 
00041 enum MagCoords { MX, MY, MZ };
00042 
00043 void ReadCompass(void) {
00044     switch ( CompassType ) {
00045         case HMC5843:
00046             ReadHMC5843();
00047             break;
00048         case HMC6352:
00049             ReadHMC6352();
00050             break;
00051         default:
00052             Heading = 0;
00053             break;
00054     } // switch
00055 
00056 } // ReadCompass
00057 
00058 void CalibrateCompass(void) {
00059     switch ( CompassType ) {
00060         case HMC5843:
00061             CalibrateHMC5843();
00062             break;
00063         case HMC6352:
00064             CalibrateHMC6352();
00065             break;
00066         default:
00067             break;
00068     } // switch
00069 } // CalibrateCompass
00070 
00071 void ShowCompassType(void) {
00072     switch ( CompassType ) {
00073         case HMC5843:
00074             TxString("HMC5843");
00075             break;
00076         case HMC6352:
00077             TxString("HMC6352");
00078             break;
00079         default:
00080             break;
00081     }
00082 } // ShowCompassType
00083 
00084 void DoCompassTest(void) {
00085     switch ( CompassType ) {
00086         case HMC5843:
00087             DoHMC5843Test();
00088             break;
00089         case HMC6352:
00090             DoHMC6352Test();
00091             break;
00092         default:
00093             TxString("\r\nCompass test\r\nCompass not detected?\r\n");
00094             break;
00095     } // switch
00096 } // DoCompassTest
00097 
00098 void GetHeading(void) {
00099 
00100     static real32 HeadingChange, CompassA;
00101 
00102     ReadCompass();
00103 
00104     Heading = Make2Pi( MagHeading + MagDeviation - CompassOffset );
00105     HeadingChange = fabs( Heading - HeadingP );
00106     if ( HeadingChange > ONEANDHALFPI ) // wrap 0 -> TwoPI
00107         HeadingP = Heading;
00108     else
00109         if ( HeadingChange > CompassMaxSlew ) { // Sanity check - discard reading
00110             Heading =SlewLimit(HeadingP, Heading, CompassMaxSlew ); 
00111             Stats[CompassFailS]++;
00112         }
00113 
00114 #ifndef SUPPRESS_COMPASS_FILTER
00115     CompassA = dT / ( 1.0 / ( TWOPI * YawFilterLPFreq ) + dT );
00116     Heading = Make2Pi( LPFilter(Heading, HeadingP, CompassA) );
00117 #endif // !SUPPRESS_COMPASS_FILTER
00118     HeadingP = Heading;
00119 
00120 #ifdef SIMULATE
00121 #if ( defined AILERON | defined ELEVON )
00122     if ( State == InFlight )
00123         FakeHeading -= FakeDesiredRoll/5 + FakeDesiredYaw/5;
00124 #else
00125     if ( State == InFlight ) {
00126         if ( Abs(FakeDesiredYaw) > 5 )
00127             FakeHeading -= FakeDesiredYaw/5;
00128     }
00129 
00130     FakeHeading = Make2Pi((int16)FakeHeading);
00131     Heading = FakeHeading;
00132 #endif // AILERON | ELEVON
00133 #endif // SIMULATE 
00134 } // GetHeading
00135 
00136 //boolean DirectionSelected = false;
00137 //real32 DirectionSense = 1.0;
00138 
00139 real32 MinimumTurn(real32 A ) {
00140 
00141     static real32 AbsA;
00142 
00143     AbsA = fabs(A);
00144     if ( AbsA > PI )
00145         A = ( AbsA - TWOPI ) * Sign(A);
00146 
00147     //DirectionSelected = fabs(A) > THIRDPI; // avoid dithering around reciprocal heading
00148     //DirectionSense = Sign(A);
00149 
00150     return ( A );
00151 
00152 } // MinimumTurn
00153 
00154 void InitCompass(void) {
00155     if ( IsHMC5843Active() )
00156         CompassType = HMC5843;
00157     else
00158         if ( HMC6352Active() )
00159             CompassType = HMC6352;
00160         else {
00161             CompassType = NoCompass;
00162             F.CompassValid = false;
00163         }
00164 
00165     switch ( CompassType ) {
00166         case HMC5843:
00167             InitHMC5843();
00168             break;
00169         case HMC6352:
00170             InitHMC6352();
00171             break;
00172         default:
00173             MagHeading = 0;
00174     } // switch
00175 
00176     ReadCompass();
00177     mS[CompassUpdate] = mSClock();
00178     Heading = HeadingP = Make2Pi( MagHeading - MagDeviation - CompassOffset );
00179 
00180 } // InitCompass
00181 
00182 //________________________________________________________________________________________
00183 
00184 // HMC5843 3 Axis Magnetometer
00185 
00186 void ReadHMC5843(void);
00187 void GetHMC5843Parameters(void);
00188 void DoHMC5843Test(void);
00189 void CalibrateHMC5843(void);
00190 void InitHMC5843(void);
00191 boolean HMC5843Active(void);
00192 
00193 void ReadHMC5843(void) {
00194     static char b[6];
00195     static i16u X, Y, Z;
00196     static real32 FX,FY;
00197     static real32 CRoll, SRoll, CPitch, SPitch;
00198 
00199     I2CCOMPASS.start();
00200     if ( I2CCOMPASS.write(HMC5843_WR) != I2C_ACK ) goto HMC5843Error;
00201     if ( I2CCOMPASS.write(0x03) != I2C_ACK ) goto HMC5843Error; // point to data
00202     I2CCOMPASS.stop();
00203 
00204     if ( I2CCOMPASS.blockread(HMC5843_RD, b, 6) ) goto HMC5843Error;
00205 
00206     X.b1 = b[0];
00207     X.b0 = b[1];
00208     Y.b1 = b[2];
00209     Y.b0  =b[3];
00210     Z.b1 = b[4];
00211     Z.b0 = b[5];
00212 
00213     if ( F.Using9DOF ) { // SparkFun/QuadroUFO 9DOF Breakout pins  front edge components up
00214         Mag[BF].V = X.i16;
00215         Mag[LR].V = -Y.i16;
00216         Mag[UD].V = -Z.i16;
00217     } else { // SparkFun Magnetometer Breakout pins  right edge components up
00218         Mag[BF].V = -X.i16;
00219         Mag[LR].V = Y.i16;
00220         Mag[UD].V = -Z.i16;
00221     }
00222     DebugPin = true;
00223     CRoll = cos(Angle[Roll]);
00224     SRoll = sin(Angle[Roll]);       // Acc[LR] - optimisation not worthwhile
00225     CPitch = cos(Angle[Pitch]);
00226     SPitch = sin(Angle[Pitch]);     // Acc[BF]
00227 
00228     FX = (Mag[BF].V-Mag[BF].Offset) * CPitch + (Mag[LR].V-Mag[LR].Offset) * SRoll * SPitch + (Mag[UD].V-Mag[UD].Offset) * CRoll * SPitch;
00229     FY = (Mag[LR].V-Mag[LR].Offset) * CRoll - (Mag[UD].V-Mag[UD].Offset) * SRoll;
00230 
00231     // Magnetic Heading
00232     MagHeading = MakePi(atan2( -FY, FX ));
00233 
00234     DebugPin = false;
00235     F.CompassValid = true;
00236 
00237     return;
00238 
00239 HMC5843Error:
00240     I2CCOMPASS.stop();
00241 
00242     I2CError[HMC5843_ID]++;
00243     if ( State == InFlight ) Stats[CompassFailS]++;
00244 
00245     F.CompassMissRead = true;
00246     F.CompassValid = false;
00247 
00248 } // ReadHMC5843
00249 
00250 real32  magFieldEarth[3],  magFieldBody[3], dcmMatrix[9];
00251 boolean firstPassMagOffset = true;
00252 
00253 void CalibrateHMC5843(void) {
00254 
00255     /*
00256     void magOffsetCalc()
00257     {
00258       int16   i, j ;
00259       static real32 tempMatrix[3] ;
00260       static real32 offsetSum[3] ;
00261 
00262       // Compute magnetic field of the earth
00263 
00264       magFieldEarth[0] = vectorDotProduct(3, &dcmMatrix[0], &magFieldBody[0]);
00265       magFieldEarth[1] = vectorDotProduct(3, &dcmMatrix[3], &magFieldBody[0]);
00266       magFieldEarth[2] = vectorDotProduct(3, &dcmMatrix[6], &magFieldBody[0]);
00267 
00268       // First pass thru?
00269 
00270       if ( firstPassMagOffset )
00271       {
00272         setPastValues();                         // Yes, set initial values for previous values
00273         firstPassMagOffset = false;              // Clear first pass flag
00274       }
00275 
00276       // Compute the offsets in the magnetometer:
00277       vectorAdd(3, offsetSum , magFieldBody, magFieldBodyPrevious) ;
00278 
00279       matrixMultiply(1, 3, 3, tempMatrix, magFieldEarthPrevious, dcmMatrix);
00280       vectorSubtract(3, offsetSum, offsetSum, tempMatrix);
00281       matrixMultiply(1, 3, 3, tempMatrix, magFieldEarth, dcmMatrixPrevious);
00282       vectorSubtract(3, offsetSum, offsetSum, tempMatrix) ;
00283 
00284       for ( i = 0 ; i < 3 ; i++ )
00285         if ( abs(offsetSum[i] ) < 3 )
00286           offsetSum[i] = 0 ;
00287 
00288       vectorAdd (3, magOffset, magOffset, offsetSum);
00289       setPastValues();
00290     }
00291 
00292     void setPastValues()
00293     {
00294       static uint8 i;
00295 
00296       for ( i = 0; i < 3; i++)
00297       {
00298         magFieldEarthPrevious[i] = magFieldEarth[i];
00299         magFieldBodyPrevious[i]  = magFieldBody[i];
00300       }
00301 
00302       for ( i = 0; i < 9; i++)
00303         dcmMatrixPrevious[i] = dcmMatrix[i];
00304 
00305     }
00306     */
00307 } // CalibrateHMC5843
00308 
00309 void GetHMC5843Parameters(void) {
00310 
00311     static char CP[16];
00312     static uint8 i;
00313 
00314     I2CCOMPASS.start();
00315     if ( I2CCOMPASS.write(HMC5843_WR) != I2C_ACK) goto HMC5843Error;
00316     if ( I2CCOMPASS.write(0x00) != I2C_ACK) goto HMC5843Error; // point to data
00317     I2CCOMPASS.stop();
00318 
00319     if ( I2CCOMPASS.blockread(HMC5843_RD, CP, 13) ) goto HMC5843Error;
00320 
00321     for ( i = 0; i < (uint8)13; i++ ) {
00322         TxVal32(i,0,0);
00323         TxString(":\t0x");
00324         TxValH(CP[i]);
00325         TxChar(HT);
00326         TxBin8(CP[i]);
00327         TxNextLine();
00328     }
00329 
00330     return;
00331 
00332 HMC5843Error:
00333     I2CCOMPASS.stop();
00334 
00335     I2CError[HMC5843_ID]++;
00336 
00337 } // GetHMC5843Parameters
00338 
00339 void DoHMC5843Test(void) {
00340 
00341     TxString("\r\nCompass test (HMC5843)\r\n\r\n");
00342 
00343     ReadHMC5843();
00344 
00345     GetHMC5843Parameters();
00346 
00347     TxString("\r\nMag:\t");
00348     TxVal32(Mag[BF].V, 0, HT);
00349     TxVal32(Mag[LR].V, 0, HT);
00350     TxVal32(Mag[UD].V, 0, HT);
00351     TxNextLine();
00352     TxNextLine();
00353 
00354     TxVal32(MagHeading * RADDEG * 10.0, 1, ' ');
00355     TxString("deg (Magnetic)\r\n");
00356 
00357     Heading = HeadingP = Make2Pi( MagHeading + MagDeviation - CompassOffset );
00358     TxVal32(Heading * RADDEG * 10.0, 1, ' ');
00359     TxString("deg (True)\r\n");
00360 } // DoHMC5843Test
00361 
00362 boolean WriteByteHMC5843(uint8 a, uint8 d) {
00363 
00364     I2CCOMPASS.start();
00365     if ( I2CCOMPASS.write(HMC5843_WR) != I2C_ACK ) goto HMC5843Error;
00366     if ( I2CCOMPASS.write(a) != I2C_ACK ) goto HMC5843Error;
00367     if ( I2CCOMPASS.write(d) != I2C_ACK ) goto HMC5843Error;
00368     I2CCOMPASS.stop();
00369 
00370     return( false );
00371 
00372 HMC5843Error:
00373     I2CCOMPASS.stop();
00374 
00375     I2CError[HMC5843_ID]++;
00376 
00377     return(true);
00378 
00379 } // WriteByteHMC5843
00380 
00381 void InitHMC5843(void) {
00382 
00383     CompassMaxSlew = (COMPASS_SANITY_CHECK_RAD_S/HMC5843_UPDATE_S);
00384 
00385     if ( WriteByteHMC5843(0x00, HMC5843_DR  << 2) ) goto HMC5843Error; // rate, normal measurement mode
00386     if ( WriteByteHMC5843(0x02, 0x00) ) goto HMC5843Error; // mode continuous
00387 
00388     Delay1mS(50);
00389 
00390     return;
00391 
00392 HMC5843Error:
00393     I2CCOMPASS.stop();
00394 
00395     I2CError[HMC5843_ID]++;
00396 
00397     F.CompassValid = false;
00398 
00399 } // InitHMC5843Magnetometer
00400 
00401 boolean IsHMC5843Active(void) {
00402 
00403     F.CompassValid = I2CCOMPASSAddressResponds( HMC5843_ID );
00404 
00405     if ( F.CompassValid )
00406         TrackMinI2CRate(400000);
00407 
00408     return ( F.CompassValid );
00409 
00410 } // IsHMC5843Active
00411 
00412 //________________________________________________________________________________________
00413 
00414 // HMC6352 Compass
00415 
00416 void ReadHMC6352(void);
00417 uint8 WriteByteHMC6352(uint8);
00418 void GetHMC6352Parameters(void);
00419 void DoHMC6352Test(void);
00420 void CalibrateHMC6352(void);
00421 void InitHMC6352(void);
00422 boolean IsHMC6352Active(void);
00423 
00424 void ReadHMC6352(void) {
00425     static i16u v;
00426 
00427     I2CCOMPASS.start();
00428     if ( I2CCOMPASS.write(HMC6352_RD) != I2C_ACK ) goto HMC6352Error;
00429     v.b1 = I2CCOMPASS.read(I2C_ACK);
00430     v.b0 = I2CCOMPASS.read(I2C_NACK);
00431     I2CCOMPASS.stop();
00432 
00433     MagHeading = Make2Pi( ((real32)v.i16 * PI) / 1800.0 - CompassOffset ); // Radians
00434 
00435     return;
00436 
00437 HMC6352Error:
00438     I2CCOMPASS.stop();
00439 
00440     if ( State == InFlight ) Stats[CompassFailS]++;
00441 
00442     F.CompassMissRead = true;
00443 
00444 } // ReadHMC6352
00445 
00446 uint8 WriteByteHMC6352(uint8 d) {
00447 
00448     I2CCOMPASS.start();
00449     if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
00450     if ( I2CCOMPASS.write(d) != I2C_ACK ) goto HMC6352Error;
00451     I2CCOMPASS.stop();
00452 
00453     return( I2C_ACK );
00454 
00455 HMC6352Error:
00456     I2CCOMPASS.stop();
00457 
00458     I2CError[HMC6352_ID]++;
00459 
00460     return ( I2C_NACK );
00461 } // WriteByteHMC6352
00462 
00463 char CP[9];
00464 
00465 #define TEST_COMP_OPMODE 0x70    // standby mode to reliably read EEPROM
00466 
00467 void GetHMC6352Parameters(void) {
00468     int16 Temp;
00469 
00470     I2CCOMPASS.start();
00471     if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
00472     if ( I2CCOMPASS.write('G')  != I2C_ACK ) goto HMC6352Error;
00473     if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto HMC6352Error;
00474     if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto HMC6352Error;
00475     I2CCOMPASS.stop();
00476 
00477     Delay1mS(20);
00478 
00479     for (r = 0; r <= (uint8)8; r++) { // must have this timing - not block read!
00480 
00481         I2CCOMPASS.start();
00482         if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
00483         if ( I2CCOMPASS.write('r')  != I2C_ACK ) goto HMC6352Error;
00484         if ( I2CCOMPASS.write(r)  != I2C_ACK ) goto HMC6352Error;
00485         I2CCOMPASS.stop();
00486 
00487         Delay1mS(10);
00488 
00489         I2CCOMPASS.start();
00490         if ( I2CCOMPASS.write(HMC6352_RD) != I2C_ACK ) goto HMC6352Error;
00491         CP[r] = I2CCOMPASS.read(I2C_NACK);
00492         I2CCOMPASS.stop();
00493 
00494         Delay1mS(10);
00495     }
00496 
00497     TxString("\r\nRegisters\r\n");
00498     TxString("\t0:\tI2C");
00499     TxString("\t 0x");
00500     TxValH(CP[0]);
00501     if ( CP[0] != (uint8)0x42 )
00502         TxString("\t Error expected 0x42 for HMC6352");
00503     TxNextLine();
00504 
00505     Temp = (CP[1]*256)|CP[2];
00506     TxString("\t1:2:\tXOffset\t");
00507     TxVal32((int32)Temp, 0, 0);
00508     TxNextLine();
00509 
00510     Temp = (CP[3]*256)|CP[4];
00511     TxString("\t3:4:\tYOffset\t");
00512     TxVal32((int32)Temp, 0, 0);
00513     TxNextLine();
00514 
00515     TxString("\t5:\tDelay\t");
00516     TxVal32((int32)CP[5], 0, 0);
00517     TxNextLine();
00518 
00519     TxString("\t6:\tNSum\t");
00520     TxVal32((int32)CP[6], 0, 0);
00521     TxNextLine();
00522 
00523     TxString("\t7:\tSW Ver\t");
00524     TxString(" 0x");
00525     TxValH(CP[7]);
00526     TxNextLine();
00527 
00528     TxString("\t8:\tOpMode:");
00529     switch ( ( CP[8] >> 5 ) & 0x03 ) {
00530         case 0:
00531             TxString("  1Hz");
00532             break;
00533         case 1:
00534             TxString("  5Hz");
00535             break;
00536         case 2:
00537             TxString("  10Hz");
00538             break;
00539         case 3:
00540             TxString("  20Hz");
00541             break;
00542     }
00543 
00544     if ( CP[8] & 0x10 ) TxString(" S/R");
00545 
00546     switch ( CP[8] & 0x03 ) {
00547         case 0:
00548             TxString(" Standby");
00549             break;
00550         case 1:
00551             TxString(" Query");
00552             break;
00553         case 2:
00554             TxString(" Continuous");
00555             break;
00556         case 3:
00557             TxString(" Not-allowed");
00558             break;
00559     }
00560     TxNextLine();
00561 
00562     return;
00563 
00564 HMC6352Error:
00565     I2CCOMPASS.stop();
00566 
00567     I2CError[HMC6352_ID]++;
00568 
00569     TxString("FAIL\r\n");
00570 
00571 } // GetHMC6352Parameters
00572 
00573 void DoHMC6352Test(void) {
00574 
00575     TxString("\r\nCompass test (HMC6352)\r\n");
00576 
00577     I2CCOMPASS.start();
00578     if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
00579     if ( I2CCOMPASS.write('G')  != I2C_ACK ) goto HMC6352Error;
00580     if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto HMC6352Error;
00581     if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto HMC6352Error;
00582     I2CCOMPASS.stop();
00583 
00584     Delay1mS(1);
00585 
00586     //  I2CCOMPASS.start(); // Do Set/Reset now
00587     if ( WriteByteHMC6352('O')  != I2C_ACK ) goto HMC6352Error;
00588 
00589     Delay1mS(7);
00590 
00591     GetHMC6352Parameters();
00592 
00593     InitCompass();
00594     if ( !F.CompassValid ) goto HMC6352Error;
00595 
00596     Delay1mS(50);
00597 
00598     ReadHMC6352();
00599     if ( F.CompassMissRead ) goto HMC6352Error;
00600 
00601     TxNextLine();
00602     TxVal32(MagHeading * RADDEG * 10.0, 1, 0);
00603     TxString(" deg (Magnetic)\r\n");
00604     Heading = HeadingP = Make2Pi( MagHeading - MagDeviation - CompassOffset );
00605     TxVal32(Heading * RADDEG * 10.0, 1, 0);
00606     TxString(" deg (True)\r\n");
00607 
00608     return;
00609 
00610 HMC6352Error:
00611     I2CCOMPASS.stop();
00612 
00613     I2CError[HMC6352_ID]++;
00614 
00615     TxString("FAIL\r\n");
00616 } // DoHMC6352Test
00617 
00618 void CalibrateHMC6352(void) {   // calibrate the compass by rotating the ufo through 720 deg smoothly
00619     TxString("\r\nCalib. compass - Press CONTINUE button (x) to Start\r\n");
00620     while ( PollRxChar() != 'x' ); // UAVPSet uses 'x' for CONTINUE button
00621 
00622     // Do Set/Reset now
00623     if ( WriteByteHMC6352('O') != I2C_ACK ) goto HMC6352Error;
00624 
00625     Delay1mS(7);
00626 
00627     // set Compass device to Calibration mode
00628     if ( WriteByteHMC6352('C') != I2C_ACK ) goto HMC6352Error;
00629 
00630     TxString("\r\nRotate horizontally 720 deg in ~30 sec. - Press CONTINUE button (x) to Finish\r\n");
00631     while ( PollRxChar() != 'x' );
00632 
00633     // set Compass device to End-Calibration mode
00634     if ( WriteByteHMC6352('E') != I2C_ACK ) goto HMC6352Error;
00635 
00636     TxString("\r\nCalibration complete\r\n");
00637 
00638     Delay1mS(50);
00639 
00640     InitCompass();
00641 
00642     return;
00643 
00644 HMC6352Error:
00645     I2CCOMPASS.stop();
00646 
00647     I2CError[HMC6352_ID]++;
00648 
00649     TxString("Calibration FAILED\r\n");
00650 } // CalibrateHMC6352
00651 
00652 void InitHMC6352(void) {
00653 
00654     // 20Hz continuous read with periodic reset.
00655 #ifdef SUPPRESS_COMPASS_SR
00656 #define COMP_OPMODE 0x62
00657 #else
00658 #define COMP_OPMODE 0x72
00659 #endif // SUPPRESS_COMPASS_SR
00660 
00661     CompassMaxSlew =  (COMPASS_SANITY_CHECK_RAD_S/HMC6352_UPDATE_S);
00662 
00663     // Set device to Compass mode
00664     I2CCOMPASS.start();
00665     if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
00666     if ( I2CCOMPASS.write('G')  != I2C_ACK ) goto HMC6352Error;
00667     if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto HMC6352Error;
00668     if ( I2CCOMPASS.write(COMP_OPMODE) != I2C_ACK ) goto HMC6352Error;
00669     I2CCOMPASS.stop();
00670 
00671     Delay1mS(10);
00672 
00673     // save operation mode in Flash
00674     if ( WriteByteHMC6352('L') != I2C_ACK ) goto HMC6352Error;
00675 
00676     Delay1mS(10);
00677 
00678     // Do Bridge Offset Set/Reset now
00679     if ( WriteByteHMC6352('O') != I2C_ACK ) goto HMC6352Error;
00680 
00681     Delay1mS(50);
00682 
00683     F.CompassValid = true;
00684 
00685     return;
00686 
00687 HMC6352Error:
00688     I2CCOMPASS.stop();
00689 
00690     I2CError[HMC6352_ID]++;
00691 
00692     F.CompassValid = false;
00693 
00694     F.CompassFailure = true;
00695 
00696 } // InitHMC6352
00697 
00698 boolean HMC6352Active(void) {
00699 
00700     F.CompassValid = I2CCOMPASSAddressResponds( HMC6352_ID );
00701 
00702     if ( F.CompassValid )
00703         TrackMinI2CRate(100000);
00704 
00705     return ( F.CompassValid );
00706 
00707 } // HMC6352Active