Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
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);