Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

compass.c@0:62a1c91a859a, 2011-02-18 (annotated)

Committer:
gke
Date:
Fri Feb 18 22:28:05 2011 +0000
Revision:
0:62a1c91a859a
Child:
1:1e3318a30ddd
First release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gke 0:62a1c91a859a 1 // ===============================================================================================
gke 0:62a1c91a859a 2 // = UAVXArm Quadrocopter Controller =
gke 0:62a1c91a859a 3 // = Copyright (c) 2008 by Prof. Greg Egan =
gke 0:62a1c91a859a 4 // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer =
gke 0:62a1c91a859a 5 // = http://code.google.com/p/uavp-mods/ http://uavp.ch =
gke 0:62a1c91a859a 6 // ===============================================================================================
gke 0:62a1c91a859a 7
gke 0:62a1c91a859a 8 // This is part of UAVXArm.
gke 0:62a1c91a859a 9
gke 0:62a1c91a859a 10 // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
gke 0:62a1c91a859a 11 // General Public License as published by the Free Software Foundation, either version 3 of the
gke 0:62a1c91a859a 12 // License, or (at your option) any later version.
gke 0:62a1c91a859a 13
gke 0:62a1c91a859a 14 // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
gke 0:62a1c91a859a 15 // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
gke 0:62a1c91a859a 16 // See the GNU General Public License for more details.
gke 0:62a1c91a859a 17
gke 0:62a1c91a859a 18 // You should have received a copy of the GNU General Public License along with this program.
gke 0:62a1c91a859a 19 // If not, see http://www.gnu.org/licenses/
gke 0:62a1c91a859a 20
gke 0:62a1c91a859a 21 #include "UAVXArm.h"
gke 0:62a1c91a859a 22
gke 0:62a1c91a859a 23 // Local magnetic declination not included
gke 0:62a1c91a859a 24 // http://www.ngdc.noaa.gov/geomagmodels/Declination.jsp
gke 0:62a1c91a859a 25
gke 0:62a1c91a859a 26
gke 0:62a1c91a859a 27 void ReadCompass(void);
gke 0:62a1c91a859a 28 void GetHeading(void);
gke 0:62a1c91a859a 29 void CalibrateCompass(void);
gke 0:62a1c91a859a 30 void ShowCompassType(void);
gke 0:62a1c91a859a 31 void DoCompassTest(void);
gke 0:62a1c91a859a 32 void InitCompass(void);
gke 0:62a1c91a859a 33
gke 0:62a1c91a859a 34 MagStruct Mag[3] = {{ 0,0 },{ 0,0 },{ 0,0 }};
gke 0:62a1c91a859a 35 real32 MagDeviation, CompassOffset;
gke 0:62a1c91a859a 36 real32 MagHeading, Heading, Headingp, FakeHeading;
gke 0:62a1c91a859a 37 real32 HeadingSin, HeadingCos;
gke 0:62a1c91a859a 38 uint8 CompassType;
gke 0:62a1c91a859a 39
gke 0:62a1c91a859a 40 void ReadCompass(void) {
gke 0:62a1c91a859a 41 switch ( CompassType ) {
gke 0:62a1c91a859a 42 case HMC5843:
gke 0:62a1c91a859a 43 ReadHMC5843();
gke 0:62a1c91a859a 44 break;
gke 0:62a1c91a859a 45 case HMC6352:
gke 0:62a1c91a859a 46 ReadHMC6352();
gke 0:62a1c91a859a 47 break;
gke 0:62a1c91a859a 48 default:
gke 0:62a1c91a859a 49 Heading = 0;
gke 0:62a1c91a859a 50 break;
gke 0:62a1c91a859a 51 } // switch
gke 0:62a1c91a859a 52
gke 0:62a1c91a859a 53 } // ReadCompass
gke 0:62a1c91a859a 54
gke 0:62a1c91a859a 55 void CalibrateCompass(void) {
gke 0:62a1c91a859a 56 switch ( CompassType ) {
gke 0:62a1c91a859a 57 case HMC5843:
gke 0:62a1c91a859a 58 CalibrateHMC5843();
gke 0:62a1c91a859a 59 break;
gke 0:62a1c91a859a 60 case HMC6352:
gke 0:62a1c91a859a 61 CalibrateHMC6352();
gke 0:62a1c91a859a 62 break;
gke 0:62a1c91a859a 63 default:
gke 0:62a1c91a859a 64 break;
gke 0:62a1c91a859a 65 } // switch
gke 0:62a1c91a859a 66 } // CalibrateCompass
gke 0:62a1c91a859a 67
gke 0:62a1c91a859a 68 void ShowCompassType(void) {
gke 0:62a1c91a859a 69 switch ( CompassType ) {
gke 0:62a1c91a859a 70 case HMC5843:
gke 0:62a1c91a859a 71 TxString("HMC5843");
gke 0:62a1c91a859a 72 break;
gke 0:62a1c91a859a 73 case HMC6352:
gke 0:62a1c91a859a 74 TxString("HMC6352");
gke 0:62a1c91a859a 75 break;
gke 0:62a1c91a859a 76 default:
gke 0:62a1c91a859a 77 break;
gke 0:62a1c91a859a 78 }
gke 0:62a1c91a859a 79 } // ShowCompassType
gke 0:62a1c91a859a 80
gke 0:62a1c91a859a 81 void DoCompassTest(void) {
gke 0:62a1c91a859a 82 switch ( CompassType ) {
gke 0:62a1c91a859a 83 case HMC5843:
gke 0:62a1c91a859a 84 DoHMC5843Test();
gke 0:62a1c91a859a 85 break;
gke 0:62a1c91a859a 86 case HMC6352:
gke 0:62a1c91a859a 87 DoHMC6352Test();
gke 0:62a1c91a859a 88 break;
gke 0:62a1c91a859a 89 default:
gke 0:62a1c91a859a 90 TxString("\r\nCompass test\r\nCompass not detected?\r\n");
gke 0:62a1c91a859a 91 break;
gke 0:62a1c91a859a 92 } // switch
gke 0:62a1c91a859a 93 } // DoCompassTest
gke 0:62a1c91a859a 94
gke 0:62a1c91a859a 95 void GetHeading(void) {
gke 0:62a1c91a859a 96
gke 0:62a1c91a859a 97 const real32 CompassA = COMPASS_UPDATE_S / ( OneOnTwoPiCompassF + COMPASS_UPDATE_S );
gke 0:62a1c91a859a 98
gke 0:62a1c91a859a 99 ReadCompass();
gke 0:62a1c91a859a 100
gke 0:62a1c91a859a 101 Heading = Make2Pi( MagHeading - MagDeviation - CompassOffset );
gke 0:62a1c91a859a 102 if ( fabs(Heading - Headingp ) > PI )
gke 0:62a1c91a859a 103 Headingp = Heading;
gke 0:62a1c91a859a 104
gke 0:62a1c91a859a 105 Heading = Headingp + (Heading - Headingp) * CompassA;
gke 0:62a1c91a859a 106 Headingp = Heading;
gke 0:62a1c91a859a 107
gke 0:62a1c91a859a 108 #ifdef SIMULATE
gke 0:62a1c91a859a 109 #if ( defined AILERON | defined ELEVON )
gke 0:62a1c91a859a 110 if ( State == InFlight )
gke 0:62a1c91a859a 111 FakeHeading -= FakeDesiredRoll/5 + FakeDesiredYaw/5;
gke 0:62a1c91a859a 112 #else
gke 0:62a1c91a859a 113 if ( State == InFlight ) {
gke 0:62a1c91a859a 114 if ( Abs(FakeDesiredYaw) > 5 )
gke 0:62a1c91a859a 115 FakeHeading -= FakeDesiredYaw/5;
gke 0:62a1c91a859a 116 }
gke 0:62a1c91a859a 117
gke 0:62a1c91a859a 118 FakeHeading = Make2Pi((int16)FakeHeading);
gke 0:62a1c91a859a 119 Heading = FakeHeading;
gke 0:62a1c91a859a 120 #endif // AILERON | ELEVON
gke 0:62a1c91a859a 121 #endif // SIMULATE
gke 0:62a1c91a859a 122 } // GetHeading
gke 0:62a1c91a859a 123
gke 0:62a1c91a859a 124 void InitCompass(void) {
gke 0:62a1c91a859a 125 if ( IsHMC5843Active() )
gke 0:62a1c91a859a 126 CompassType = HMC5843;
gke 0:62a1c91a859a 127 else
gke 0:62a1c91a859a 128 if ( HMC6352Active() )
gke 0:62a1c91a859a 129 CompassType = HMC6352;
gke 0:62a1c91a859a 130 else {
gke 0:62a1c91a859a 131 CompassType = NoCompass;
gke 0:62a1c91a859a 132 F.CompassValid = false;
gke 0:62a1c91a859a 133 }
gke 0:62a1c91a859a 134
gke 0:62a1c91a859a 135 switch ( CompassType ) {
gke 0:62a1c91a859a 136 case HMC5843:
gke 0:62a1c91a859a 137 InitHMC5843();
gke 0:62a1c91a859a 138 break;
gke 0:62a1c91a859a 139 case HMC6352:
gke 0:62a1c91a859a 140 InitHMC6352();
gke 0:62a1c91a859a 141 break;
gke 0:62a1c91a859a 142 default:
gke 0:62a1c91a859a 143 MagHeading = 0;
gke 0:62a1c91a859a 144 } // switch
gke 0:62a1c91a859a 145
gke 0:62a1c91a859a 146 ReadCompass();
gke 0:62a1c91a859a 147 mS[CompassUpdate] = mSClock();
gke 0:62a1c91a859a 148 Heading = Headingp = Make2Pi( MagHeading - MagDeviation - CompassOffset );
gke 0:62a1c91a859a 149
gke 0:62a1c91a859a 150 } // InitCompass
gke 0:62a1c91a859a 151
gke 0:62a1c91a859a 152 //________________________________________________________________________________________
gke 0:62a1c91a859a 153
gke 0:62a1c91a859a 154 // HMC5843 3 Axis Magnetometer
gke 0:62a1c91a859a 155
gke 0:62a1c91a859a 156 void ReadHMC5843(void);
gke 0:62a1c91a859a 157 void GetHMC5843Parameters(void);
gke 0:62a1c91a859a 158 void DoHMC5843Test(void);
gke 0:62a1c91a859a 159 void CalibrateHMC5843(void);
gke 0:62a1c91a859a 160 void InitHMC5843(void);
gke 0:62a1c91a859a 161 boolean HMC5843Active(void);
gke 0:62a1c91a859a 162
gke 0:62a1c91a859a 163 void ReadHMC5843(void) {
gke 0:62a1c91a859a 164 static char b[6];
gke 0:62a1c91a859a 165 static i16u X, Y, Z;
gke 0:62a1c91a859a 166 static uint8 r;
gke 0:62a1c91a859a 167 static real32 mx, my;
gke 0:62a1c91a859a 168 static real32 CRoll, SRoll, CPitch, SPitch;
gke 0:62a1c91a859a 169
gke 0:62a1c91a859a 170 I2CCOMPASS.start();
gke 0:62a1c91a859a 171 r = I2CCOMPASS.write(HMC5843_ID);
gke 0:62a1c91a859a 172 r = I2CCOMPASS.write(0x03); // point to data
gke 0:62a1c91a859a 173 I2CCOMPASS.stop();
gke 0:62a1c91a859a 174
gke 0:62a1c91a859a 175 I2CCOMPASS.read(HMC5843_ID, b, 6);
gke 0:62a1c91a859a 176
gke 0:62a1c91a859a 177 X.b1 = b[0];
gke 0:62a1c91a859a 178 X.b0 = b[1];
gke 0:62a1c91a859a 179 Y.b1 = b[2];
gke 0:62a1c91a859a 180 Y.b0 =b[3];
gke 0:62a1c91a859a 181 Z.b1 = b[4];
gke 0:62a1c91a859a 182 Z.b0 = b[5];
gke 0:62a1c91a859a 183
gke 0:62a1c91a859a 184 if ( F.Using9DOF ) { // SparkFun/QuadroUFO 9DOF Breakout pins front edge components up
gke 0:62a1c91a859a 185 Mag[BF].V = X.i16;
gke 0:62a1c91a859a 186 Mag[LR].V = -Y.i16;
gke 0:62a1c91a859a 187 Mag[UD].V = -Z.i16;
gke 0:62a1c91a859a 188 } else { // SparkFun Magnetometer Breakout pins right edge components up
gke 0:62a1c91a859a 189 Mag[BF].V = -X.i16;
gke 0:62a1c91a859a 190 Mag[LR].V = Y.i16;
gke 0:62a1c91a859a 191 Mag[UD].V = -Z.i16;
gke 0:62a1c91a859a 192 }
gke 0:62a1c91a859a 193
gke 0:62a1c91a859a 194 CRoll = cos(Angle[Roll]);
gke 0:62a1c91a859a 195 SRoll = sin(Angle[Roll]);
gke 0:62a1c91a859a 196 CPitch = cos(Angle[Pitch]);
gke 0:62a1c91a859a 197 SPitch = sin(Angle[Pitch]);
gke 0:62a1c91a859a 198
gke 0:62a1c91a859a 199 mx = (Mag[BF].V-Mag[BF].Offset) * CPitch + (Mag[LR].V-Mag[LR].Offset) * SRoll * SPitch + (Mag[UD].V-Mag[UD].Offset) * CRoll * SPitch;
gke 0:62a1c91a859a 200 my = (Mag[LR].V-Mag[LR].Offset) * CRoll - (Mag[UD].V-Mag[UD].Offset) * SRoll;
gke 0:62a1c91a859a 201
gke 0:62a1c91a859a 202 // Magnetic Heading
gke 0:62a1c91a859a 203 MagHeading = MakePi(atan2( -my, mx ));
gke 0:62a1c91a859a 204
gke 0:62a1c91a859a 205 F.CompassValid = true;
gke 0:62a1c91a859a 206 return;
gke 0:62a1c91a859a 207
gke 0:62a1c91a859a 208 } // ReadHMC5843
gke 0:62a1c91a859a 209
gke 0:62a1c91a859a 210 void CalibrateHMC5843(void) {
gke 0:62a1c91a859a 211
gke 0:62a1c91a859a 212 } // DoHMC5843Test
gke 0:62a1c91a859a 213
gke 0:62a1c91a859a 214 void DoHMC5843Test(void) {
gke 0:62a1c91a859a 215 TxString("\r\nCompass test (HMC5843)\r\n\r\n");
gke 0:62a1c91a859a 216
gke 0:62a1c91a859a 217 ReadHMC5843();
gke 0:62a1c91a859a 218
gke 0:62a1c91a859a 219 TxString("Mag:\t");
gke 0:62a1c91a859a 220 TxVal32(Mag[LR].V, 0, HT);
gke 0:62a1c91a859a 221 TxVal32(Mag[BF].V, 0, HT);
gke 0:62a1c91a859a 222 TxVal32(Mag[UD].V, 0, HT);
gke 0:62a1c91a859a 223 TxNextLine();
gke 0:62a1c91a859a 224 TxNextLine();
gke 0:62a1c91a859a 225
gke 0:62a1c91a859a 226 TxVal32(MagHeading * RADDEG * 10.0, 1, 0);
gke 0:62a1c91a859a 227 TxString(" deg (Magnetic)\r\n");
gke 0:62a1c91a859a 228
gke 0:62a1c91a859a 229 Heading = Headingp = Make2Pi( MagHeading - MagDeviation - CompassOffset );
gke 0:62a1c91a859a 230 TxVal32(Heading * RADDEG * 10.0, 1, 0);
gke 0:62a1c91a859a 231 TxString(" deg (True)\r\n");
gke 0:62a1c91a859a 232 } // DoHMC5843Test
gke 0:62a1c91a859a 233
gke 0:62a1c91a859a 234 void InitHMC5843(void) {
gke 0:62a1c91a859a 235 static uint8 r;
gke 0:62a1c91a859a 236
gke 0:62a1c91a859a 237 I2CCOMPASS.start();
gke 0:62a1c91a859a 238 r = I2CCOMPASS.write(HMC5843_ID);
gke 0:62a1c91a859a 239 r = I2CCOMPASS.write(0x02);
gke 0:62a1c91a859a 240 r = I2CCOMPASS.write(0x00); // Set continuous mode (default to 10Hz)
gke 0:62a1c91a859a 241 I2CCOMPASS.stop();
gke 0:62a1c91a859a 242
gke 0:62a1c91a859a 243 Delay1mS(50);
gke 0:62a1c91a859a 244
gke 0:62a1c91a859a 245 } // InitHMC5843Magnetometer
gke 0:62a1c91a859a 246
gke 0:62a1c91a859a 247 boolean IsHMC5843Active(void) {
gke 0:62a1c91a859a 248 I2CCOMPASS.start();
gke 0:62a1c91a859a 249 F.CompassValid = !(I2CCOMPASS.write(HMC5843_ID) != I2C_ACK);
gke 0:62a1c91a859a 250 I2CCOMPASS.stop();
gke 0:62a1c91a859a 251
gke 0:62a1c91a859a 252 if ( F.CompassValid )
gke 0:62a1c91a859a 253 TrackMinI2CRate(400000);
gke 0:62a1c91a859a 254
gke 0:62a1c91a859a 255 return ( F.CompassValid );
gke 0:62a1c91a859a 256
gke 0:62a1c91a859a 257 } // IsHMC5843Active
gke 0:62a1c91a859a 258
gke 0:62a1c91a859a 259 //________________________________________________________________________________________
gke 0:62a1c91a859a 260
gke 0:62a1c91a859a 261 // HMC6352 Compass
gke 0:62a1c91a859a 262
gke 0:62a1c91a859a 263 void ReadHMC6352(void);
gke 0:62a1c91a859a 264 uint8 WriteByteHMC6352(uint8);
gke 0:62a1c91a859a 265 void GetHMC6352Parameters(void);
gke 0:62a1c91a859a 266 void DoHMC6352Test(void);
gke 0:62a1c91a859a 267 void CalibrateHMC6352(void);
gke 0:62a1c91a859a 268 void InitHMC6352(void);
gke 0:62a1c91a859a 269 boolean IsHMC6352Active(void);
gke 0:62a1c91a859a 270
gke 0:62a1c91a859a 271 void ReadHMC6352(void) {
gke 0:62a1c91a859a 272 static i16u v;
gke 0:62a1c91a859a 273
gke 0:62a1c91a859a 274 I2CCOMPASS.start();
gke 0:62a1c91a859a 275 F.CompassMissRead = I2CCOMPASS.write(HMC6352_ID + 1) != I2C_ACK;
gke 0:62a1c91a859a 276 v.b1 = I2CCOMPASS.read(I2C_ACK);
gke 0:62a1c91a859a 277 v.b0 = I2CCOMPASS.read(I2C_NACK);
gke 0:62a1c91a859a 278 I2CCOMPASS.stop();
gke 0:62a1c91a859a 279
gke 0:62a1c91a859a 280 MagHeading = Make2Pi( ((real32)v.i16 * PI) / 1800.0 - CompassOffset ); // Radians
gke 0:62a1c91a859a 281 } // ReadHMC6352
gke 0:62a1c91a859a 282
gke 0:62a1c91a859a 283 uint8 WriteByteHMC6352(uint8 d) {
gke 0:62a1c91a859a 284 I2CCOMPASS.start();
gke 0:62a1c91a859a 285 if ( I2CCOMPASS.write(HMC6352_ID) != I2C_ACK ) goto WError;
gke 0:62a1c91a859a 286 if ( I2CCOMPASS.write(d) != I2C_ACK ) goto WError;
gke 0:62a1c91a859a 287 I2CCOMPASS.stop();
gke 0:62a1c91a859a 288
gke 0:62a1c91a859a 289 return( I2C_ACK );
gke 0:62a1c91a859a 290 WError:
gke 0:62a1c91a859a 291 I2CCOMPASS.stop();
gke 0:62a1c91a859a 292 return ( I2C_NACK );
gke 0:62a1c91a859a 293 } // WriteByteHMC6352
gke 0:62a1c91a859a 294
gke 0:62a1c91a859a 295 char CP[9];
gke 0:62a1c91a859a 296
gke 0:62a1c91a859a 297 #define TEST_COMP_OPMODE 0x70 // standby mode to reliably read EEPROM
gke 0:62a1c91a859a 298
gke 0:62a1c91a859a 299 void GetHMC6352Parameters(void) {
gke 0:62a1c91a859a 300 uint8 r;
gke 0:62a1c91a859a 301
gke 0:62a1c91a859a 302 I2CCOMPASS.start();
gke 0:62a1c91a859a 303 if ( I2CCOMPASS.write(HMC6352_ID) != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 304 if ( I2CCOMPASS.write('G') != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 305 if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 306 if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 307 I2CCOMPASS.stop();
gke 0:62a1c91a859a 308
gke 0:62a1c91a859a 309 Delay1mS(20);
gke 0:62a1c91a859a 310
gke 0:62a1c91a859a 311 for (r = 0; r <= (uint8)8; r++) { // must have this timing - not block read!
gke 0:62a1c91a859a 312
gke 0:62a1c91a859a 313 I2CCOMPASS.start();
gke 0:62a1c91a859a 314 if ( I2CCOMPASS.write(HMC6352_ID) != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 315 if ( I2CCOMPASS.write('r') != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 316 if ( I2CCOMPASS.write(r) != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 317 I2CCOMPASS.stop();
gke 0:62a1c91a859a 318
gke 0:62a1c91a859a 319 Delay1mS(10);
gke 0:62a1c91a859a 320
gke 0:62a1c91a859a 321 I2CCOMPASS.start();
gke 0:62a1c91a859a 322 if ( I2CCOMPASS.write(HMC6352_ID+1) != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 323 CP[r] = I2CCOMPASS.read(I2C_NACK);
gke 0:62a1c91a859a 324 I2CCOMPASS.stop();
gke 0:62a1c91a859a 325
gke 0:62a1c91a859a 326 Delay1mS(10);
gke 0:62a1c91a859a 327 }
gke 0:62a1c91a859a 328
gke 0:62a1c91a859a 329 return;
gke 0:62a1c91a859a 330
gke 0:62a1c91a859a 331 CTerror:
gke 0:62a1c91a859a 332 I2CCOMPASS.stop();
gke 0:62a1c91a859a 333 TxString("FAIL\r\n");
gke 0:62a1c91a859a 334
gke 0:62a1c91a859a 335 } // GetHMC6352Parameters
gke 0:62a1c91a859a 336
gke 0:62a1c91a859a 337 void DoHMC6352Test(void) {
gke 0:62a1c91a859a 338 static real32 Temp;
gke 0:62a1c91a859a 339
gke 0:62a1c91a859a 340 TxString("\r\nCompass test (HMC6352)\r\n");
gke 0:62a1c91a859a 341
gke 0:62a1c91a859a 342 I2CCOMPASS.start();
gke 0:62a1c91a859a 343 if ( I2CCOMPASS.write(HMC6352_ID) != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 344 if ( I2CCOMPASS.write('G') != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 345 if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 346 if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 347 I2CCOMPASS.stop();
gke 0:62a1c91a859a 348
gke 0:62a1c91a859a 349 Delay1mS(1);
gke 0:62a1c91a859a 350
gke 0:62a1c91a859a 351 // I2CCOMPASS.start(); // Do Set/Reset now
gke 0:62a1c91a859a 352 if ( WriteByteHMC6352('O') != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 353
gke 0:62a1c91a859a 354 Delay1mS(7);
gke 0:62a1c91a859a 355
gke 0:62a1c91a859a 356 GetHMC6352Parameters();
gke 0:62a1c91a859a 357
gke 0:62a1c91a859a 358 TxString("\r\nRegisters\r\n");
gke 0:62a1c91a859a 359 TxString("\t0:\tI2C");
gke 0:62a1c91a859a 360 TxString("\t 0x");
gke 0:62a1c91a859a 361 TxValH(CP[0]);
gke 0:62a1c91a859a 362 if ( CP[0] != (uint8)0x42 )
gke 0:62a1c91a859a 363 TxString("\t Error expected 0x42 for HMC6352");
gke 0:62a1c91a859a 364 TxNextLine();
gke 0:62a1c91a859a 365
gke 0:62a1c91a859a 366 Temp = (CP[1]*256)|CP[2];
gke 0:62a1c91a859a 367 TxString("\t1:2:\tXOffset\t");
gke 0:62a1c91a859a 368 TxVal32((int32)Temp, 0, 0);
gke 0:62a1c91a859a 369 TxNextLine();
gke 0:62a1c91a859a 370
gke 0:62a1c91a859a 371 Temp = (CP[3]*256)|CP[4];
gke 0:62a1c91a859a 372 TxString("\t3:4:\tYOffset\t");
gke 0:62a1c91a859a 373 TxVal32((int32)Temp, 0, 0);
gke 0:62a1c91a859a 374 TxNextLine();
gke 0:62a1c91a859a 375
gke 0:62a1c91a859a 376 TxString("\t5:\tDelay\t");
gke 0:62a1c91a859a 377 TxVal32((int32)CP[5], 0, 0);
gke 0:62a1c91a859a 378 TxNextLine();
gke 0:62a1c91a859a 379
gke 0:62a1c91a859a 380 TxString("\t6:\tNSum\t");
gke 0:62a1c91a859a 381 TxVal32((int32)CP[6], 0, 0);
gke 0:62a1c91a859a 382 TxNextLine();
gke 0:62a1c91a859a 383
gke 0:62a1c91a859a 384 TxString("\t7:\tSW Ver\t");
gke 0:62a1c91a859a 385 TxString(" 0x");
gke 0:62a1c91a859a 386 TxValH(CP[7]);
gke 0:62a1c91a859a 387 TxNextLine();
gke 0:62a1c91a859a 388
gke 0:62a1c91a859a 389 TxString("\t8:\tOpMode:");
gke 0:62a1c91a859a 390 switch ( ( CP[8] >> 5 ) & 0x03 ) {
gke 0:62a1c91a859a 391 case 0:
gke 0:62a1c91a859a 392 TxString(" 1Hz");
gke 0:62a1c91a859a 393 break;
gke 0:62a1c91a859a 394 case 1:
gke 0:62a1c91a859a 395 TxString(" 5Hz");
gke 0:62a1c91a859a 396 break;
gke 0:62a1c91a859a 397 case 2:
gke 0:62a1c91a859a 398 TxString(" 10Hz");
gke 0:62a1c91a859a 399 break;
gke 0:62a1c91a859a 400 case 3:
gke 0:62a1c91a859a 401 TxString(" 20Hz");
gke 0:62a1c91a859a 402 break;
gke 0:62a1c91a859a 403 }
gke 0:62a1c91a859a 404
gke 0:62a1c91a859a 405 if ( CP[8] & 0x10 ) TxString(" S/R");
gke 0:62a1c91a859a 406
gke 0:62a1c91a859a 407 switch ( CP[8] & 0x03 ) {
gke 0:62a1c91a859a 408 case 0:
gke 0:62a1c91a859a 409 TxString(" Standby");
gke 0:62a1c91a859a 410 break;
gke 0:62a1c91a859a 411 case 1:
gke 0:62a1c91a859a 412 TxString(" Query");
gke 0:62a1c91a859a 413 break;
gke 0:62a1c91a859a 414 case 2:
gke 0:62a1c91a859a 415 TxString(" Continuous");
gke 0:62a1c91a859a 416 break;
gke 0:62a1c91a859a 417 case 3:
gke 0:62a1c91a859a 418 TxString(" Not-allowed");
gke 0:62a1c91a859a 419 break;
gke 0:62a1c91a859a 420 }
gke 0:62a1c91a859a 421 TxNextLine();
gke 0:62a1c91a859a 422
gke 0:62a1c91a859a 423 InitCompass();
gke 0:62a1c91a859a 424 if ( !F.CompassValid ) goto CTerror;
gke 0:62a1c91a859a 425
gke 0:62a1c91a859a 426 Delay1mS(50);
gke 0:62a1c91a859a 427
gke 0:62a1c91a859a 428 ReadHMC6352();
gke 0:62a1c91a859a 429 if ( F.CompassMissRead ) goto CTerror;
gke 0:62a1c91a859a 430
gke 0:62a1c91a859a 431 TxNextLine();
gke 0:62a1c91a859a 432 TxVal32(MagHeading * RADDEG * 10.0, 1, 0);
gke 0:62a1c91a859a 433 TxString(" deg (Magnetic)\r\n");
gke 0:62a1c91a859a 434 Heading = Headingp = Make2Pi( MagHeading - MagDeviation - CompassOffset );
gke 0:62a1c91a859a 435 TxVal32(Heading * RADDEG * 10.0, 1, 0);
gke 0:62a1c91a859a 436 TxString(" deg (True)\r\n");
gke 0:62a1c91a859a 437
gke 0:62a1c91a859a 438 return;
gke 0:62a1c91a859a 439 CTerror:
gke 0:62a1c91a859a 440 I2CCOMPASS.stop();
gke 0:62a1c91a859a 441 TxString("FAIL\r\n");
gke 0:62a1c91a859a 442 } // DoHMC6352Test
gke 0:62a1c91a859a 443
gke 0:62a1c91a859a 444 void CalibrateHMC6352(void) { // calibrate the compass by rotating the ufo through 720 deg smoothly
gke 0:62a1c91a859a 445 TxString("\r\nCalib. compass - Press CONTINUE button (x) to Start\r\n");
gke 0:62a1c91a859a 446 while ( PollRxChar() != 'x' ); // UAVPSet uses 'x' for CONTINUE button
gke 0:62a1c91a859a 447
gke 0:62a1c91a859a 448 // Do Set/Reset now
gke 0:62a1c91a859a 449 if ( WriteByteHMC6352('O') != I2C_ACK ) goto CCerror;
gke 0:62a1c91a859a 450
gke 0:62a1c91a859a 451 Delay1mS(7);
gke 0:62a1c91a859a 452
gke 0:62a1c91a859a 453 // set Compass device to Calibration mode
gke 0:62a1c91a859a 454 if ( WriteByteHMC6352('C') != I2C_ACK ) goto CCerror;
gke 0:62a1c91a859a 455
gke 0:62a1c91a859a 456 TxString("\r\nRotate horizontally 720 deg in ~30 sec. - Press CONTINUE button (x) to Finish\r\n");
gke 0:62a1c91a859a 457 while ( PollRxChar() != 'x' );
gke 0:62a1c91a859a 458
gke 0:62a1c91a859a 459 // set Compass device to End-Calibration mode
gke 0:62a1c91a859a 460 if ( WriteByteHMC6352('E') != I2C_ACK ) goto CCerror;
gke 0:62a1c91a859a 461
gke 0:62a1c91a859a 462 TxString("\r\nCalibration complete\r\n");
gke 0:62a1c91a859a 463
gke 0:62a1c91a859a 464 Delay1mS(50);
gke 0:62a1c91a859a 465
gke 0:62a1c91a859a 466 InitCompass();
gke 0:62a1c91a859a 467
gke 0:62a1c91a859a 468 return;
gke 0:62a1c91a859a 469
gke 0:62a1c91a859a 470 CCerror:
gke 0:62a1c91a859a 471 TxString("Calibration FAILED\r\n");
gke 0:62a1c91a859a 472 } // CalibrateHMC6352
gke 0:62a1c91a859a 473
gke 0:62a1c91a859a 474 void InitHMC6352(void) {
gke 0:62a1c91a859a 475
gke 0:62a1c91a859a 476 // 20Hz continuous read with periodic reset.
gke 0:62a1c91a859a 477 #ifdef SUPPRESS_COMPASS_SR
gke 0:62a1c91a859a 478 #define COMP_OPMODE 0x62
gke 0:62a1c91a859a 479 #else
gke 0:62a1c91a859a 480 #define COMP_OPMODE 0x72
gke 0:62a1c91a859a 481 #endif // SUPPRESS_COMPASS_SR
gke 0:62a1c91a859a 482
gke 0:62a1c91a859a 483 // Set device to Compass mode
gke 0:62a1c91a859a 484 I2CCOMPASS.start();
gke 0:62a1c91a859a 485 if ( I2CCOMPASS.write(HMC6352_ID) != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 486 if ( I2CCOMPASS.write('G') != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 487 if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 488 if ( I2CCOMPASS.write(COMP_OPMODE) != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 489 I2CCOMPASS.stop();
gke 0:62a1c91a859a 490
gke 0:62a1c91a859a 491 Delay1mS(1);
gke 0:62a1c91a859a 492
gke 0:62a1c91a859a 493 // save operation mode in Flash
gke 0:62a1c91a859a 494 if ( WriteByteHMC6352('L') != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 495
gke 0:62a1c91a859a 496 Delay1mS(1);
gke 0:62a1c91a859a 497
gke 0:62a1c91a859a 498 // Do Bridge Offset Set/Reset now
gke 0:62a1c91a859a 499 if ( WriteByteHMC6352('O') != I2C_ACK ) goto CTerror;
gke 0:62a1c91a859a 500
gke 0:62a1c91a859a 501 Delay1mS(50);
gke 0:62a1c91a859a 502
gke 0:62a1c91a859a 503 F.CompassValid = true;
gke 0:62a1c91a859a 504
gke 0:62a1c91a859a 505 return;
gke 0:62a1c91a859a 506 CTerror:
gke 0:62a1c91a859a 507 F.CompassValid = false;
gke 0:62a1c91a859a 508 Stats[CompassFailS]++;
gke 0:62a1c91a859a 509 F.CompassFailure = true;
gke 0:62a1c91a859a 510
gke 0:62a1c91a859a 511 I2CCOMPASS.stop();
gke 0:62a1c91a859a 512 } // InitHMC6352
gke 0:62a1c91a859a 513
gke 0:62a1c91a859a 514 boolean HMC6352Active(void) {
gke 0:62a1c91a859a 515
gke 0:62a1c91a859a 516 I2CCOMPASS.start();
gke 0:62a1c91a859a 517 F.CompassValid = !(I2CCOMPASS.write(HMC6352_ID) != I2C_ACK);
gke 0:62a1c91a859a 518 I2CCOMPASS.stop();
gke 0:62a1c91a859a 519
gke 0:62a1c91a859a 520 if ( F.CompassValid )
gke 0:62a1c91a859a 521 TrackMinI2CRate(100000);
gke 0:62a1c91a859a 522
gke 0:62a1c91a859a 523 return ( F.CompassValid );
gke 0:62a1c91a859a 524
gke 0:62a1c91a859a 525 } // HMC6352Active