Diff: compass.c

Revision:

1:1e3318a30ddd

Parent:

0:62a1c91a859a

Child:

2:90292f8bd179

--- a/compass.c	Fri Feb 18 22:28:05 2011 +0000
+++ b/compass.c	Fri Feb 25 01:35:24 2011 +0000
@@ -66,9 +66,9 @@
 } // CalibrateCompass
 
 void ShowCompassType(void) {
- switch ( CompassType ) {
+    switch ( CompassType ) {
         case HMC5843:
-             TxString("HMC5843");
+            TxString("HMC5843");
             break;
         case HMC6352:
             TxString("HMC6352");
@@ -76,7 +76,7 @@
         default:
             break;
     }
- } // ShowCompassType
+} // ShowCompassType
 
 void DoCompassTest(void) {
     switch ( CompassType ) {
@@ -168,11 +168,11 @@
     static real32 CRoll, SRoll, CPitch, SPitch;
 
     I2CCOMPASS.start();
-    r = I2CCOMPASS.write(HMC5843_ID);
+    r = I2CCOMPASS.write(HMC5843_WR);
     r = I2CCOMPASS.write(0x03); // point to data
     I2CCOMPASS.stop();
 
-    I2CCOMPASS.read(HMC5843_ID, b, 6);
+    I2CCOMPASS.blockread(HMC5843_RD, b, 6);
 
     X.b1 = b[0];
     X.b0 = b[1];
@@ -190,7 +190,7 @@
         Mag[LR].V = Y.i16;
         Mag[UD].V = -Z.i16;
     }
-
+    DebugPin = true;
     CRoll = cos(Angle[Roll]);
     SRoll = sin(Angle[Roll]);
     CPitch = cos(Angle[Pitch]);
@@ -201,7 +201,7 @@
 
     // Magnetic Heading
     MagHeading = MakePi(atan2( -my, mx ));
-
+    DebugPin = false;
     F.CompassValid = true;
     return;
 
@@ -225,7 +225,7 @@
 
     TxVal32(MagHeading * RADDEG * 10.0, 1, 0);
     TxString(" deg (Magnetic)\r\n");
-    
+
     Heading = Headingp = Make2Pi( MagHeading - MagDeviation - CompassOffset );
     TxVal32(Heading * RADDEG * 10.0, 1, 0);
     TxString(" deg (True)\r\n");
@@ -235,7 +235,7 @@
     static uint8 r;
 
     I2CCOMPASS.start();
-    r = I2CCOMPASS.write(HMC5843_ID);
+    r = I2CCOMPASS.write(HMC5843_WR);
     r = I2CCOMPASS.write(0x02);
     r = I2CCOMPASS.write(0x00);   // Set continuous mode (default to 10Hz)
     I2CCOMPASS.stop();
@@ -245,10 +245,9 @@
 } // InitHMC5843Magnetometer
 
 boolean IsHMC5843Active(void) {
-    I2CCOMPASS.start();
-    F.CompassValid = !(I2CCOMPASS.write(HMC5843_ID) != I2C_ACK);
-    I2CCOMPASS.stop();
-    
+
+    F.CompassValid = I2CCOMPASSAddressResponds( HMC5843_ID );
+
     if ( F.CompassValid )
         TrackMinI2CRate(400000);
 
@@ -272,7 +271,7 @@
     static i16u v;
 
     I2CCOMPASS.start();
-    F.CompassMissRead = I2CCOMPASS.write(HMC6352_ID + 1) != I2C_ACK;
+    F.CompassMissRead = I2CCOMPASS.write(HMC6352_RD) != I2C_ACK;
     v.b1 = I2CCOMPASS.read(I2C_ACK);
     v.b0 = I2CCOMPASS.read(I2C_NACK);
     I2CCOMPASS.stop();
@@ -282,7 +281,7 @@
 
 uint8 WriteByteHMC6352(uint8 d) {
     I2CCOMPASS.start();
-    if ( I2CCOMPASS.write(HMC6352_ID) != I2C_ACK ) goto WError;
+    if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto WError;
     if ( I2CCOMPASS.write(d) != I2C_ACK ) goto WError;
     I2CCOMPASS.stop();
 
@@ -300,7 +299,7 @@
     uint8 r;
 
     I2CCOMPASS.start();
-    if ( I2CCOMPASS.write(HMC6352_ID) != I2C_ACK ) goto CTerror;
+    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;
@@ -311,7 +310,7 @@
     for (r = 0; r <= (uint8)8; r++) { // must have this timing - not block read!
 
         I2CCOMPASS.start();
-        if ( I2CCOMPASS.write(HMC6352_ID) != I2C_ACK ) goto CTerror;
+        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;
         I2CCOMPASS.stop();
@@ -319,7 +318,7 @@
         Delay1mS(10);
 
         I2CCOMPASS.start();
-        if ( I2CCOMPASS.write(HMC6352_ID+1) != I2C_ACK ) goto CTerror;
+        if ( I2CCOMPASS.write(HMC6352_RD) != I2C_ACK ) goto CTerror;
         CP[r] = I2CCOMPASS.read(I2C_NACK);
         I2CCOMPASS.stop();
 
@@ -340,7 +339,7 @@
     TxString("\r\nCompass test (HMC6352)\r\n");
 
     I2CCOMPASS.start();
-    if ( I2CCOMPASS.write(HMC6352_ID) != I2C_ACK ) goto CTerror;
+    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;
@@ -430,7 +429,7 @@
 
     TxNextLine();
     TxVal32(MagHeading * RADDEG * 10.0, 1, 0);
-    TxString(" deg (Magnetic)\r\n");   
+    TxString(" deg (Magnetic)\r\n");
     Heading = Headingp = Make2Pi( MagHeading - MagDeviation - CompassOffset );
     TxVal32(Heading * RADDEG * 10.0, 1, 0);
     TxString(" deg (True)\r\n");
@@ -482,7 +481,7 @@
 
     // Set device to Compass mode
     I2CCOMPASS.start();
-    if ( I2CCOMPASS.write(HMC6352_ID) != I2C_ACK ) goto CTerror;
+    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;
@@ -513,9 +512,7 @@
 
 boolean HMC6352Active(void) {
 
-    I2CCOMPASS.start();
-    F.CompassValid = !(I2CCOMPASS.write(HMC6352_ID) != I2C_ACK);
-    I2CCOMPASS.stop();
+    F.CompassValid = I2CCOMPASSAddressResponds( HMC6352_ID );
 
     if ( F.CompassValid )
         TrackMinI2CRate(100000);