Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

compass.c@2:90292f8bd179, 2011-04-26 (annotated)

Committer:
gke
Date:
Tue Apr 26 12:12:29 2011 +0000
Revision:
2:90292f8bd179
Parent:
1:1e3318a30ddd 
Not flightworthy. Posted for others to make use of the I2C SW code.

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 2:90292f8bd179 5 // = http://code.google.com/p/uavp-mods/ =
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 void ReadCompass(void);
gke 0:62a1c91a859a 27 void GetHeading(void);
gke 2:90292f8bd179 28 real32 MinimumTurn(real32);
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 2:90292f8bd179 36 real32 MagHeading, Heading, HeadingP, FakeHeading;
gke 0:62a1c91a859a 37 real32 HeadingSin, HeadingCos;
gke 2:90292f8bd179 38 real32 CompassMaxSlew;
gke 0:62a1c91a859a 39 uint8 CompassType;
gke 0:62a1c91a859a 40
gke 2:90292f8bd179 41 enum MagCoords { MX, MY, MZ };
gke 2:90292f8bd179 42
gke 0:62a1c91a859a 43 void ReadCompass(void) {
gke 0:62a1c91a859a 44 switch ( CompassType ) {
gke 0:62a1c91a859a 45 case HMC5843:
gke 0:62a1c91a859a 46 ReadHMC5843();
gke 0:62a1c91a859a 47 break;
gke 0:62a1c91a859a 48 case HMC6352:
gke 0:62a1c91a859a 49 ReadHMC6352();
gke 0:62a1c91a859a 50 break;
gke 0:62a1c91a859a 51 default:
gke 0:62a1c91a859a 52 Heading = 0;
gke 0:62a1c91a859a 53 break;
gke 0:62a1c91a859a 54 } // switch
gke 0:62a1c91a859a 55
gke 0:62a1c91a859a 56 } // ReadCompass
gke 0:62a1c91a859a 57
gke 0:62a1c91a859a 58 void CalibrateCompass(void) {
gke 0:62a1c91a859a 59 switch ( CompassType ) {
gke 0:62a1c91a859a 60 case HMC5843:
gke 0:62a1c91a859a 61 CalibrateHMC5843();
gke 0:62a1c91a859a 62 break;
gke 0:62a1c91a859a 63 case HMC6352:
gke 0:62a1c91a859a 64 CalibrateHMC6352();
gke 0:62a1c91a859a 65 break;
gke 0:62a1c91a859a 66 default:
gke 0:62a1c91a859a 67 break;
gke 0:62a1c91a859a 68 } // switch
gke 0:62a1c91a859a 69 } // CalibrateCompass
gke 0:62a1c91a859a 70
gke 0:62a1c91a859a 71 void ShowCompassType(void) {
gke 1:1e3318a30ddd 72 switch ( CompassType ) {
gke 0:62a1c91a859a 73 case HMC5843:
gke 1:1e3318a30ddd 74 TxString("HMC5843");
gke 0:62a1c91a859a 75 break;
gke 0:62a1c91a859a 76 case HMC6352:
gke 0:62a1c91a859a 77 TxString("HMC6352");
gke 0:62a1c91a859a 78 break;
gke 0:62a1c91a859a 79 default:
gke 0:62a1c91a859a 80 break;
gke 0:62a1c91a859a 81 }
gke 1:1e3318a30ddd 82 } // ShowCompassType
gke 0:62a1c91a859a 83
gke 0:62a1c91a859a 84 void DoCompassTest(void) {
gke 0:62a1c91a859a 85 switch ( CompassType ) {
gke 0:62a1c91a859a 86 case HMC5843:
gke 0:62a1c91a859a 87 DoHMC5843Test();
gke 0:62a1c91a859a 88 break;
gke 0:62a1c91a859a 89 case HMC6352:
gke 0:62a1c91a859a 90 DoHMC6352Test();
gke 0:62a1c91a859a 91 break;
gke 0:62a1c91a859a 92 default:
gke 0:62a1c91a859a 93 TxString("\r\nCompass test\r\nCompass not detected?\r\n");
gke 0:62a1c91a859a 94 break;
gke 0:62a1c91a859a 95 } // switch
gke 0:62a1c91a859a 96 } // DoCompassTest
gke 0:62a1c91a859a 97
gke 0:62a1c91a859a 98 void GetHeading(void) {
gke 0:62a1c91a859a 99
gke 2:90292f8bd179 100 static real32 HeadingChange, CompassA;
gke 0:62a1c91a859a 101
gke 0:62a1c91a859a 102 ReadCompass();
gke 0:62a1c91a859a 103
gke 2:90292f8bd179 104 Heading = Make2Pi( MagHeading + MagDeviation - CompassOffset );
gke 2:90292f8bd179 105 HeadingChange = fabs( Heading - HeadingP );
gke 2:90292f8bd179 106 if ( HeadingChange > ONEANDHALFPI ) // wrap 0 -> TwoPI
gke 2:90292f8bd179 107 HeadingP = Heading;
gke 2:90292f8bd179 108 else
gke 2:90292f8bd179 109 if ( HeadingChange > CompassMaxSlew ) { // Sanity check - discard reading
gke 2:90292f8bd179 110 Heading =SlewLimit(HeadingP, Heading, CompassMaxSlew );
gke 2:90292f8bd179 111 Stats[CompassFailS]++;
gke 2:90292f8bd179 112 }
gke 0:62a1c91a859a 113
gke 2:90292f8bd179 114 #ifndef SUPPRESS_COMPASS_FILTER
gke 2:90292f8bd179 115 CompassA = dT / ( 1.0 / ( TWOPI * YawFilterLPFreq ) + dT );
gke 2:90292f8bd179 116 Heading = Make2Pi( LPFilter(Heading, HeadingP, CompassA) );
gke 2:90292f8bd179 117 #endif // !SUPPRESS_COMPASS_FILTER
gke 2:90292f8bd179 118 HeadingP = Heading;
gke 0:62a1c91a859a 119
gke 0:62a1c91a859a 120 #ifdef SIMULATE
gke 0:62a1c91a859a 121 #if ( defined AILERON | defined ELEVON )
gke 0:62a1c91a859a 122 if ( State == InFlight )
gke 0:62a1c91a859a 123 FakeHeading -= FakeDesiredRoll/5 + FakeDesiredYaw/5;
gke 0:62a1c91a859a 124 #else
gke 0:62a1c91a859a 125 if ( State == InFlight ) {
gke 0:62a1c91a859a 126 if ( Abs(FakeDesiredYaw) > 5 )
gke 0:62a1c91a859a 127 FakeHeading -= FakeDesiredYaw/5;
gke 0:62a1c91a859a 128 }
gke 0:62a1c91a859a 129
gke 0:62a1c91a859a 130 FakeHeading = Make2Pi((int16)FakeHeading);
gke 0:62a1c91a859a 131 Heading = FakeHeading;
gke 0:62a1c91a859a 132 #endif // AILERON | ELEVON
gke 0:62a1c91a859a 133 #endif // SIMULATE
gke 0:62a1c91a859a 134 } // GetHeading
gke 0:62a1c91a859a 135
gke 2:90292f8bd179 136 //boolean DirectionSelected = false;
gke 2:90292f8bd179 137 //real32 DirectionSense = 1.0;
gke 2:90292f8bd179 138
gke 2:90292f8bd179 139 real32 MinimumTurn(real32 A ) {
gke 2:90292f8bd179 140
gke 2:90292f8bd179 141 static real32 AbsA;
gke 2:90292f8bd179 142
gke 2:90292f8bd179 143 AbsA = fabs(A);
gke 2:90292f8bd179 144 if ( AbsA > PI )
gke 2:90292f8bd179 145 A = ( AbsA - TWOPI ) * Sign(A);
gke 2:90292f8bd179 146
gke 2:90292f8bd179 147 //DirectionSelected = fabs(A) > THIRDPI; // avoid dithering around reciprocal heading
gke 2:90292f8bd179 148 //DirectionSense = Sign(A);
gke 2:90292f8bd179 149
gke 2:90292f8bd179 150 return ( A );
gke 2:90292f8bd179 151
gke 2:90292f8bd179 152 } // MinimumTurn
gke 2:90292f8bd179 153
gke 0:62a1c91a859a 154 void InitCompass(void) {
gke 0:62a1c91a859a 155 if ( IsHMC5843Active() )
gke 0:62a1c91a859a 156 CompassType = HMC5843;
gke 0:62a1c91a859a 157 else
gke 0:62a1c91a859a 158 if ( HMC6352Active() )
gke 0:62a1c91a859a 159 CompassType = HMC6352;
gke 0:62a1c91a859a 160 else {
gke 0:62a1c91a859a 161 CompassType = NoCompass;
gke 0:62a1c91a859a 162 F.CompassValid = false;
gke 0:62a1c91a859a 163 }
gke 0:62a1c91a859a 164
gke 0:62a1c91a859a 165 switch ( CompassType ) {
gke 0:62a1c91a859a 166 case HMC5843:
gke 0:62a1c91a859a 167 InitHMC5843();
gke 0:62a1c91a859a 168 break;
gke 0:62a1c91a859a 169 case HMC6352:
gke 0:62a1c91a859a 170 InitHMC6352();
gke 0:62a1c91a859a 171 break;
gke 0:62a1c91a859a 172 default:
gke 0:62a1c91a859a 173 MagHeading = 0;
gke 0:62a1c91a859a 174 } // switch
gke 0:62a1c91a859a 175
gke 0:62a1c91a859a 176 ReadCompass();
gke 0:62a1c91a859a 177 mS[CompassUpdate] = mSClock();
gke 2:90292f8bd179 178 Heading = HeadingP = Make2Pi( MagHeading - MagDeviation - CompassOffset );
gke 0:62a1c91a859a 179
gke 0:62a1c91a859a 180 } // InitCompass
gke 0:62a1c91a859a 181
gke 0:62a1c91a859a 182 //________________________________________________________________________________________
gke 0:62a1c91a859a 183
gke 0:62a1c91a859a 184 // HMC5843 3 Axis Magnetometer
gke 0:62a1c91a859a 185
gke 0:62a1c91a859a 186 void ReadHMC5843(void);
gke 0:62a1c91a859a 187 void GetHMC5843Parameters(void);
gke 0:62a1c91a859a 188 void DoHMC5843Test(void);
gke 0:62a1c91a859a 189 void CalibrateHMC5843(void);
gke 0:62a1c91a859a 190 void InitHMC5843(void);
gke 0:62a1c91a859a 191 boolean HMC5843Active(void);
gke 0:62a1c91a859a 192
gke 0:62a1c91a859a 193 void ReadHMC5843(void) {
gke 0:62a1c91a859a 194 static char b[6];
gke 0:62a1c91a859a 195 static i16u X, Y, Z;
gke 2:90292f8bd179 196 static real32 FX,FY;
gke 0:62a1c91a859a 197 static real32 CRoll, SRoll, CPitch, SPitch;
gke 0:62a1c91a859a 198
gke 0:62a1c91a859a 199 I2CCOMPASS.start();
gke 2:90292f8bd179 200 if ( I2CCOMPASS.write(HMC5843_WR) != I2C_ACK ) goto HMC5843Error;
gke 2:90292f8bd179 201 if ( I2CCOMPASS.write(0x03) != I2C_ACK ) goto HMC5843Error; // point to data
gke 0:62a1c91a859a 202 I2CCOMPASS.stop();
gke 0:62a1c91a859a 203
gke 2:90292f8bd179 204 if ( I2CCOMPASS.blockread(HMC5843_RD, b, 6) ) goto HMC5843Error;
gke 0:62a1c91a859a 205
gke 0:62a1c91a859a 206 X.b1 = b[0];
gke 0:62a1c91a859a 207 X.b0 = b[1];
gke 0:62a1c91a859a 208 Y.b1 = b[2];
gke 0:62a1c91a859a 209 Y.b0 =b[3];
gke 0:62a1c91a859a 210 Z.b1 = b[4];
gke 0:62a1c91a859a 211 Z.b0 = b[5];
gke 0:62a1c91a859a 212
gke 0:62a1c91a859a 213 if ( F.Using9DOF ) { // SparkFun/QuadroUFO 9DOF Breakout pins front edge components up
gke 0:62a1c91a859a 214 Mag[BF].V = X.i16;
gke 0:62a1c91a859a 215 Mag[LR].V = -Y.i16;
gke 0:62a1c91a859a 216 Mag[UD].V = -Z.i16;
gke 0:62a1c91a859a 217 } else { // SparkFun Magnetometer Breakout pins right edge components up
gke 0:62a1c91a859a 218 Mag[BF].V = -X.i16;
gke 0:62a1c91a859a 219 Mag[LR].V = Y.i16;
gke 0:62a1c91a859a 220 Mag[UD].V = -Z.i16;
gke 0:62a1c91a859a 221 }
gke 1:1e3318a30ddd 222 DebugPin = true;
gke 0:62a1c91a859a 223 CRoll = cos(Angle[Roll]);
gke 2:90292f8bd179 224 SRoll = sin(Angle[Roll]); // Acc[LR] - optimisation not worthwhile
gke 0:62a1c91a859a 225 CPitch = cos(Angle[Pitch]);
gke 2:90292f8bd179 226 SPitch = sin(Angle[Pitch]); // Acc[BF]
gke 0:62a1c91a859a 227
gke 2:90292f8bd179 228 FX = (Mag[BF].V-Mag[BF].Offset) * CPitch + (Mag[LR].V-Mag[LR].Offset) * SRoll * SPitch + (Mag[UD].V-Mag[UD].Offset) * CRoll * SPitch;
gke 2:90292f8bd179 229 FY = (Mag[LR].V-Mag[LR].Offset) * CRoll - (Mag[UD].V-Mag[UD].Offset) * SRoll;
gke 0:62a1c91a859a 230
gke 0:62a1c91a859a 231 // Magnetic Heading
gke 2:90292f8bd179 232 MagHeading = MakePi(atan2( -FY, FX ));
gke 2:90292f8bd179 233
gke 1:1e3318a30ddd 234 DebugPin = false;
gke 0:62a1c91a859a 235 F.CompassValid = true;
gke 2:90292f8bd179 236
gke 0:62a1c91a859a 237 return;
gke 0:62a1c91a859a 238
gke 2:90292f8bd179 239 HMC5843Error:
gke 2:90292f8bd179 240 I2CCOMPASS.stop();
gke 2:90292f8bd179 241
gke 2:90292f8bd179 242 I2CError[HMC5843_ID]++;
gke 2:90292f8bd179 243 if ( State == InFlight ) Stats[CompassFailS]++;
gke 2:90292f8bd179 244
gke 2:90292f8bd179 245 F.CompassMissRead = true;
gke 2:90292f8bd179 246 F.CompassValid = false;
gke 2:90292f8bd179 247
gke 0:62a1c91a859a 248 } // ReadHMC5843
gke 0:62a1c91a859a 249
gke 2:90292f8bd179 250 real32 magFieldEarth[3], magFieldBody[3], dcmMatrix[9];
gke 2:90292f8bd179 251 boolean firstPassMagOffset = true;
gke 2:90292f8bd179 252
gke 0:62a1c91a859a 253 void CalibrateHMC5843(void) {
gke 0:62a1c91a859a 254
gke 2:90292f8bd179 255 /*
gke 2:90292f8bd179 256 void magOffsetCalc()
gke 2:90292f8bd179 257 {
gke 2:90292f8bd179 258 int16 i, j ;
gke 2:90292f8bd179 259 static real32 tempMatrix[3] ;
gke 2:90292f8bd179 260 static real32 offsetSum[3] ;
gke 2:90292f8bd179 261
gke 2:90292f8bd179 262 // Compute magnetic field of the earth
gke 2:90292f8bd179 263
gke 2:90292f8bd179 264 magFieldEarth[0] = vectorDotProduct(3, &dcmMatrix[0], &magFieldBody[0]);
gke 2:90292f8bd179 265 magFieldEarth[1] = vectorDotProduct(3, &dcmMatrix[3], &magFieldBody[0]);
gke 2:90292f8bd179 266 magFieldEarth[2] = vectorDotProduct(3, &dcmMatrix[6], &magFieldBody[0]);
gke 2:90292f8bd179 267
gke 2:90292f8bd179 268 // First pass thru?
gke 2:90292f8bd179 269
gke 2:90292f8bd179 270 if ( firstPassMagOffset )
gke 2:90292f8bd179 271 {
gke 2:90292f8bd179 272 setPastValues(); // Yes, set initial values for previous values
gke 2:90292f8bd179 273 firstPassMagOffset = false; // Clear first pass flag
gke 2:90292f8bd179 274 }
gke 2:90292f8bd179 275
gke 2:90292f8bd179 276 // Compute the offsets in the magnetometer:
gke 2:90292f8bd179 277 vectorAdd(3, offsetSum , magFieldBody, magFieldBodyPrevious) ;
gke 2:90292f8bd179 278
gke 2:90292f8bd179 279 matrixMultiply(1, 3, 3, tempMatrix, magFieldEarthPrevious, dcmMatrix);
gke 2:90292f8bd179 280 vectorSubtract(3, offsetSum, offsetSum, tempMatrix);
gke 2:90292f8bd179 281 matrixMultiply(1, 3, 3, tempMatrix, magFieldEarth, dcmMatrixPrevious);
gke 2:90292f8bd179 282 vectorSubtract(3, offsetSum, offsetSum, tempMatrix) ;
gke 2:90292f8bd179 283
gke 2:90292f8bd179 284 for ( i = 0 ; i < 3 ; i++ )
gke 2:90292f8bd179 285 if ( abs(offsetSum[i] ) < 3 )
gke 2:90292f8bd179 286 offsetSum[i] = 0 ;
gke 2:90292f8bd179 287
gke 2:90292f8bd179 288 vectorAdd (3, magOffset, magOffset, offsetSum);
gke 2:90292f8bd179 289 setPastValues();
gke 2:90292f8bd179 290 }
gke 2:90292f8bd179 291
gke 2:90292f8bd179 292 void setPastValues()
gke 2:90292f8bd179 293 {
gke 2:90292f8bd179 294 static uint8 i;
gke 2:90292f8bd179 295
gke 2:90292f8bd179 296 for ( i = 0; i < 3; i++)
gke 2:90292f8bd179 297 {
gke 2:90292f8bd179 298 magFieldEarthPrevious[i] = magFieldEarth[i];
gke 2:90292f8bd179 299 magFieldBodyPrevious[i] = magFieldBody[i];
gke 2:90292f8bd179 300 }
gke 2:90292f8bd179 301
gke 2:90292f8bd179 302 for ( i = 0; i < 9; i++)
gke 2:90292f8bd179 303 dcmMatrixPrevious[i] = dcmMatrix[i];
gke 2:90292f8bd179 304
gke 2:90292f8bd179 305 }
gke 2:90292f8bd179 306 */
gke 2:90292f8bd179 307 } // CalibrateHMC5843
gke 2:90292f8bd179 308
gke 2:90292f8bd179 309 void GetHMC5843Parameters(void) {
gke 2:90292f8bd179 310
gke 2:90292f8bd179 311 static char CP[16];
gke 2:90292f8bd179 312 static uint8 i;
gke 2:90292f8bd179 313
gke 2:90292f8bd179 314 I2CCOMPASS.start();
gke 2:90292f8bd179 315 if ( I2CCOMPASS.write(HMC5843_WR) != I2C_ACK) goto HMC5843Error;
gke 2:90292f8bd179 316 if ( I2CCOMPASS.write(0x00) != I2C_ACK) goto HMC5843Error; // point to data
gke 2:90292f8bd179 317 I2CCOMPASS.stop();
gke 2:90292f8bd179 318
gke 2:90292f8bd179 319 if ( I2CCOMPASS.blockread(HMC5843_RD, CP, 13) ) goto HMC5843Error;
gke 2:90292f8bd179 320
gke 2:90292f8bd179 321 for ( i = 0; i < (uint8)13; i++ ) {
gke 2:90292f8bd179 322 TxVal32(i,0,0);
gke 2:90292f8bd179 323 TxString(":\t0x");
gke 2:90292f8bd179 324 TxValH(CP[i]);
gke 2:90292f8bd179 325 TxChar(HT);
gke 2:90292f8bd179 326 TxBin8(CP[i]);
gke 2:90292f8bd179 327 TxNextLine();
gke 2:90292f8bd179 328 }
gke 2:90292f8bd179 329
gke 2:90292f8bd179 330 return;
gke 2:90292f8bd179 331
gke 2:90292f8bd179 332 HMC5843Error:
gke 2:90292f8bd179 333 I2CCOMPASS.stop();
gke 2:90292f8bd179 334
gke 2:90292f8bd179 335 I2CError[HMC5843_ID]++;
gke 2:90292f8bd179 336
gke 2:90292f8bd179 337 } // GetHMC5843Parameters
gke 0:62a1c91a859a 338
gke 0:62a1c91a859a 339 void DoHMC5843Test(void) {
gke 2:90292f8bd179 340
gke 0:62a1c91a859a 341 TxString("\r\nCompass test (HMC5843)\r\n\r\n");
gke 0:62a1c91a859a 342
gke 0:62a1c91a859a 343 ReadHMC5843();
gke 0:62a1c91a859a 344
gke 2:90292f8bd179 345 GetHMC5843Parameters();
gke 2:90292f8bd179 346
gke 2:90292f8bd179 347 TxString("\r\nMag:\t");
gke 2:90292f8bd179 348 TxVal32(Mag[BF].V, 0, HT);
gke 0:62a1c91a859a 349 TxVal32(Mag[LR].V, 0, HT);
gke 0:62a1c91a859a 350 TxVal32(Mag[UD].V, 0, HT);
gke 0:62a1c91a859a 351 TxNextLine();
gke 0:62a1c91a859a 352 TxNextLine();
gke 0:62a1c91a859a 353
gke 2:90292f8bd179 354 TxVal32(MagHeading * RADDEG * 10.0, 1, ' ');
gke 2:90292f8bd179 355 TxString("deg (Magnetic)\r\n");
gke 2:90292f8bd179 356
gke 2:90292f8bd179 357 Heading = HeadingP = Make2Pi( MagHeading + MagDeviation - CompassOffset );
gke 2:90292f8bd179 358 TxVal32(Heading * RADDEG * 10.0, 1, ' ');
gke 2:90292f8bd179 359 TxString("deg (True)\r\n");
gke 2:90292f8bd179 360 } // DoHMC5843Test
gke 2:90292f8bd179 361
gke 2:90292f8bd179 362 boolean WriteByteHMC5843(uint8 a, uint8 d) {
gke 1:1e3318a30ddd 363
gke 2:90292f8bd179 364 I2CCOMPASS.start();
gke 2:90292f8bd179 365 if ( I2CCOMPASS.write(HMC5843_WR) != I2C_ACK ) goto HMC5843Error;
gke 2:90292f8bd179 366 if ( I2CCOMPASS.write(a) != I2C_ACK ) goto HMC5843Error;
gke 2:90292f8bd179 367 if ( I2CCOMPASS.write(d) != I2C_ACK ) goto HMC5843Error;
gke 2:90292f8bd179 368 I2CCOMPASS.stop();
gke 2:90292f8bd179 369
gke 2:90292f8bd179 370 return( false );
gke 2:90292f8bd179 371
gke 2:90292f8bd179 372 HMC5843Error:
gke 2:90292f8bd179 373 I2CCOMPASS.stop();
gke 2:90292f8bd179 374
gke 2:90292f8bd179 375 I2CError[HMC5843_ID]++;
gke 2:90292f8bd179 376
gke 2:90292f8bd179 377 return(true);
gke 2:90292f8bd179 378
gke 2:90292f8bd179 379 } // WriteByteHMC5843
gke 0:62a1c91a859a 380
gke 0:62a1c91a859a 381 void InitHMC5843(void) {
gke 2:90292f8bd179 382
gke 2:90292f8bd179 383 CompassMaxSlew = (COMPASS_SANITY_CHECK_RAD_S/HMC5843_UPDATE_S);
gke 2:90292f8bd179 384
gke 2:90292f8bd179 385 if ( WriteByteHMC5843(0x00, HMC5843_DR << 2) ) goto HMC5843Error; // rate, normal measurement mode
gke 2:90292f8bd179 386 if ( WriteByteHMC5843(0x02, 0x00) ) goto HMC5843Error; // mode continuous
gke 0:62a1c91a859a 387
gke 2:90292f8bd179 388 Delay1mS(50);
gke 2:90292f8bd179 389
gke 2:90292f8bd179 390 return;
gke 2:90292f8bd179 391
gke 2:90292f8bd179 392 HMC5843Error:
gke 0:62a1c91a859a 393 I2CCOMPASS.stop();
gke 0:62a1c91a859a 394
gke 2:90292f8bd179 395 I2CError[HMC5843_ID]++;
gke 2:90292f8bd179 396
gke 2:90292f8bd179 397 F.CompassValid = false;
gke 0:62a1c91a859a 398
gke 0:62a1c91a859a 399 } // InitHMC5843Magnetometer
gke 0:62a1c91a859a 400
gke 0:62a1c91a859a 401 boolean IsHMC5843Active(void) {
gke 1:1e3318a30ddd 402
gke 1:1e3318a30ddd 403 F.CompassValid = I2CCOMPASSAddressResponds( HMC5843_ID );
gke 1:1e3318a30ddd 404
gke 0:62a1c91a859a 405 if ( F.CompassValid )
gke 0:62a1c91a859a 406 TrackMinI2CRate(400000);
gke 0:62a1c91a859a 407
gke 0:62a1c91a859a 408 return ( F.CompassValid );
gke 0:62a1c91a859a 409
gke 0:62a1c91a859a 410 } // IsHMC5843Active
gke 0:62a1c91a859a 411
gke 0:62a1c91a859a 412 //________________________________________________________________________________________
gke 0:62a1c91a859a 413
gke 0:62a1c91a859a 414 // HMC6352 Compass
gke 0:62a1c91a859a 415
gke 0:62a1c91a859a 416 void ReadHMC6352(void);
gke 0:62a1c91a859a 417 uint8 WriteByteHMC6352(uint8);
gke 0:62a1c91a859a 418 void GetHMC6352Parameters(void);
gke 0:62a1c91a859a 419 void DoHMC6352Test(void);
gke 0:62a1c91a859a 420 void CalibrateHMC6352(void);
gke 0:62a1c91a859a 421 void InitHMC6352(void);
gke 0:62a1c91a859a 422 boolean IsHMC6352Active(void);
gke 0:62a1c91a859a 423
gke 0:62a1c91a859a 424 void ReadHMC6352(void) {
gke 0:62a1c91a859a 425 static i16u v;
gke 0:62a1c91a859a 426
gke 0:62a1c91a859a 427 I2CCOMPASS.start();
gke 2:90292f8bd179 428 if ( I2CCOMPASS.write(HMC6352_RD) != I2C_ACK ) goto HMC6352Error;
gke 0:62a1c91a859a 429 v.b1 = I2CCOMPASS.read(I2C_ACK);
gke 0:62a1c91a859a 430 v.b0 = I2CCOMPASS.read(I2C_NACK);
gke 0:62a1c91a859a 431 I2CCOMPASS.stop();
gke 0:62a1c91a859a 432
gke 0:62a1c91a859a 433 MagHeading = Make2Pi( ((real32)v.i16 * PI) / 1800.0 - CompassOffset ); // Radians
gke 2:90292f8bd179 434
gke 2:90292f8bd179 435 return;
gke 2:90292f8bd179 436
gke 2:90292f8bd179 437 HMC6352Error:
gke 2:90292f8bd179 438 I2CCOMPASS.stop();
gke 2:90292f8bd179 439
gke 2:90292f8bd179 440 if ( State == InFlight ) Stats[CompassFailS]++;
gke 2:90292f8bd179 441
gke 2:90292f8bd179 442 F.CompassMissRead = true;
gke 2:90292f8bd179 443
gke 0:62a1c91a859a 444 } // ReadHMC6352
gke 0:62a1c91a859a 445
gke 0:62a1c91a859a 446 uint8 WriteByteHMC6352(uint8 d) {
gke 2:90292f8bd179 447
gke 0:62a1c91a859a 448 I2CCOMPASS.start();
gke 2:90292f8bd179 449 if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
gke 2:90292f8bd179 450 if ( I2CCOMPASS.write(d) != I2C_ACK ) goto HMC6352Error;
gke 0:62a1c91a859a 451 I2CCOMPASS.stop();
gke 0:62a1c91a859a 452
gke 0:62a1c91a859a 453 return( I2C_ACK );
gke 2:90292f8bd179 454
gke 2:90292f8bd179 455 HMC6352Error:
gke 0:62a1c91a859a 456 I2CCOMPASS.stop();
gke 2:90292f8bd179 457
gke 2:90292f8bd179 458 I2CError[HMC6352_ID]++;
gke 2:90292f8bd179 459
gke 0:62a1c91a859a 460 return ( I2C_NACK );
gke 0:62a1c91a859a 461 } // WriteByteHMC6352
gke 0:62a1c91a859a 462
gke 0:62a1c91a859a 463 char CP[9];
gke 0:62a1c91a859a 464
gke 0:62a1c91a859a 465 #define TEST_COMP_OPMODE 0x70 // standby mode to reliably read EEPROM
gke 0:62a1c91a859a 466
gke 0:62a1c91a859a 467 void GetHMC6352Parameters(void) {
gke 2:90292f8bd179 468 int16 Temp;
gke 0:62a1c91a859a 469
gke 0:62a1c91a859a 470 I2CCOMPASS.start();
gke 2:90292f8bd179 471 if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
gke 2:90292f8bd179 472 if ( I2CCOMPASS.write('G') != I2C_ACK ) goto HMC6352Error;
gke 2:90292f8bd179 473 if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto HMC6352Error;
gke 2:90292f8bd179 474 if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto HMC6352Error;
gke 0:62a1c91a859a 475 I2CCOMPASS.stop();
gke 0:62a1c91a859a 476
gke 0:62a1c91a859a 477 Delay1mS(20);
gke 0:62a1c91a859a 478
gke 0:62a1c91a859a 479 for (r = 0; r <= (uint8)8; r++) { // must have this timing - not block read!
gke 0:62a1c91a859a 480
gke 0:62a1c91a859a 481 I2CCOMPASS.start();
gke 2:90292f8bd179 482 if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
gke 2:90292f8bd179 483 if ( I2CCOMPASS.write('r') != I2C_ACK ) goto HMC6352Error;
gke 2:90292f8bd179 484 if ( I2CCOMPASS.write(r) != I2C_ACK ) goto HMC6352Error;
gke 0:62a1c91a859a 485 I2CCOMPASS.stop();
gke 0:62a1c91a859a 486
gke 0:62a1c91a859a 487 Delay1mS(10);
gke 0:62a1c91a859a 488
gke 0:62a1c91a859a 489 I2CCOMPASS.start();
gke 2:90292f8bd179 490 if ( I2CCOMPASS.write(HMC6352_RD) != I2C_ACK ) goto HMC6352Error;
gke 0:62a1c91a859a 491 CP[r] = I2CCOMPASS.read(I2C_NACK);
gke 0:62a1c91a859a 492 I2CCOMPASS.stop();
gke 0:62a1c91a859a 493
gke 0:62a1c91a859a 494 Delay1mS(10);
gke 0:62a1c91a859a 495 }
gke 0:62a1c91a859a 496
gke 0:62a1c91a859a 497 TxString("\r\nRegisters\r\n");
gke 0:62a1c91a859a 498 TxString("\t0:\tI2C");
gke 0:62a1c91a859a 499 TxString("\t 0x");
gke 0:62a1c91a859a 500 TxValH(CP[0]);
gke 0:62a1c91a859a 501 if ( CP[0] != (uint8)0x42 )
gke 0:62a1c91a859a 502 TxString("\t Error expected 0x42 for HMC6352");
gke 0:62a1c91a859a 503 TxNextLine();
gke 0:62a1c91a859a 504
gke 0:62a1c91a859a 505 Temp = (CP[1]*256)|CP[2];
gke 0:62a1c91a859a 506 TxString("\t1:2:\tXOffset\t");
gke 0:62a1c91a859a 507 TxVal32((int32)Temp, 0, 0);
gke 0:62a1c91a859a 508 TxNextLine();
gke 0:62a1c91a859a 509
gke 0:62a1c91a859a 510 Temp = (CP[3]*256)|CP[4];
gke 0:62a1c91a859a 511 TxString("\t3:4:\tYOffset\t");
gke 0:62a1c91a859a 512 TxVal32((int32)Temp, 0, 0);
gke 0:62a1c91a859a 513 TxNextLine();
gke 0:62a1c91a859a 514
gke 0:62a1c91a859a 515 TxString("\t5:\tDelay\t");
gke 0:62a1c91a859a 516 TxVal32((int32)CP[5], 0, 0);
gke 0:62a1c91a859a 517 TxNextLine();
gke 0:62a1c91a859a 518
gke 0:62a1c91a859a 519 TxString("\t6:\tNSum\t");
gke 0:62a1c91a859a 520 TxVal32((int32)CP[6], 0, 0);
gke 0:62a1c91a859a 521 TxNextLine();
gke 0:62a1c91a859a 522
gke 0:62a1c91a859a 523 TxString("\t7:\tSW Ver\t");
gke 0:62a1c91a859a 524 TxString(" 0x");
gke 0:62a1c91a859a 525 TxValH(CP[7]);
gke 0:62a1c91a859a 526 TxNextLine();
gke 0:62a1c91a859a 527
gke 0:62a1c91a859a 528 TxString("\t8:\tOpMode:");
gke 0:62a1c91a859a 529 switch ( ( CP[8] >> 5 ) & 0x03 ) {
gke 0:62a1c91a859a 530 case 0:
gke 0:62a1c91a859a 531 TxString(" 1Hz");
gke 0:62a1c91a859a 532 break;
gke 0:62a1c91a859a 533 case 1:
gke 0:62a1c91a859a 534 TxString(" 5Hz");
gke 0:62a1c91a859a 535 break;
gke 0:62a1c91a859a 536 case 2:
gke 0:62a1c91a859a 537 TxString(" 10Hz");
gke 0:62a1c91a859a 538 break;
gke 0:62a1c91a859a 539 case 3:
gke 0:62a1c91a859a 540 TxString(" 20Hz");
gke 0:62a1c91a859a 541 break;
gke 0:62a1c91a859a 542 }
gke 0:62a1c91a859a 543
gke 0:62a1c91a859a 544 if ( CP[8] & 0x10 ) TxString(" S/R");
gke 0:62a1c91a859a 545
gke 0:62a1c91a859a 546 switch ( CP[8] & 0x03 ) {
gke 0:62a1c91a859a 547 case 0:
gke 0:62a1c91a859a 548 TxString(" Standby");
gke 0:62a1c91a859a 549 break;
gke 0:62a1c91a859a 550 case 1:
gke 0:62a1c91a859a 551 TxString(" Query");
gke 0:62a1c91a859a 552 break;
gke 0:62a1c91a859a 553 case 2:
gke 0:62a1c91a859a 554 TxString(" Continuous");
gke 0:62a1c91a859a 555 break;
gke 0:62a1c91a859a 556 case 3:
gke 0:62a1c91a859a 557 TxString(" Not-allowed");
gke 0:62a1c91a859a 558 break;
gke 0:62a1c91a859a 559 }
gke 0:62a1c91a859a 560 TxNextLine();
gke 0:62a1c91a859a 561
gke 2:90292f8bd179 562 return;
gke 2:90292f8bd179 563
gke 2:90292f8bd179 564 HMC6352Error:
gke 2:90292f8bd179 565 I2CCOMPASS.stop();
gke 2:90292f8bd179 566
gke 2:90292f8bd179 567 I2CError[HMC6352_ID]++;
gke 2:90292f8bd179 568
gke 2:90292f8bd179 569 TxString("FAIL\r\n");
gke 2:90292f8bd179 570
gke 2:90292f8bd179 571 } // GetHMC6352Parameters
gke 2:90292f8bd179 572
gke 2:90292f8bd179 573 void DoHMC6352Test(void) {
gke 2:90292f8bd179 574
gke 2:90292f8bd179 575 TxString("\r\nCompass test (HMC6352)\r\n");
gke 2:90292f8bd179 576
gke 2:90292f8bd179 577 I2CCOMPASS.start();
gke 2:90292f8bd179 578 if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
gke 2:90292f8bd179 579 if ( I2CCOMPASS.write('G') != I2C_ACK ) goto HMC6352Error;
gke 2:90292f8bd179 580 if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto HMC6352Error;
gke 2:90292f8bd179 581 if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto HMC6352Error;
gke 2:90292f8bd179 582 I2CCOMPASS.stop();
gke 2:90292f8bd179 583
gke 2:90292f8bd179 584 Delay1mS(1);
gke 2:90292f8bd179 585
gke 2:90292f8bd179 586 // I2CCOMPASS.start(); // Do Set/Reset now
gke 2:90292f8bd179 587 if ( WriteByteHMC6352('O') != I2C_ACK ) goto HMC6352Error;
gke 2:90292f8bd179 588
gke 2:90292f8bd179 589 Delay1mS(7);
gke 2:90292f8bd179 590
gke 2:90292f8bd179 591 GetHMC6352Parameters();
gke 2:90292f8bd179 592
gke 0:62a1c91a859a 593 InitCompass();
gke 2:90292f8bd179 594 if ( !F.CompassValid ) goto HMC6352Error;
gke 0:62a1c91a859a 595
gke 0:62a1c91a859a 596 Delay1mS(50);
gke 0:62a1c91a859a 597
gke 0:62a1c91a859a 598 ReadHMC6352();
gke 2:90292f8bd179 599 if ( F.CompassMissRead ) goto HMC6352Error;
gke 0:62a1c91a859a 600
gke 0:62a1c91a859a 601 TxNextLine();
gke 0:62a1c91a859a 602 TxVal32(MagHeading * RADDEG * 10.0, 1, 0);
gke 1:1e3318a30ddd 603 TxString(" deg (Magnetic)\r\n");
gke 2:90292f8bd179 604 Heading = HeadingP = Make2Pi( MagHeading - MagDeviation - CompassOffset );
gke 0:62a1c91a859a 605 TxVal32(Heading * RADDEG * 10.0, 1, 0);
gke 0:62a1c91a859a 606 TxString(" deg (True)\r\n");
gke 0:62a1c91a859a 607
gke 0:62a1c91a859a 608 return;
gke 2:90292f8bd179 609
gke 2:90292f8bd179 610 HMC6352Error:
gke 0:62a1c91a859a 611 I2CCOMPASS.stop();
gke 2:90292f8bd179 612
gke 2:90292f8bd179 613 I2CError[HMC6352_ID]++;
gke 2:90292f8bd179 614
gke 0:62a1c91a859a 615 TxString("FAIL\r\n");
gke 0:62a1c91a859a 616 } // DoHMC6352Test
gke 0:62a1c91a859a 617
gke 0:62a1c91a859a 618 void CalibrateHMC6352(void) { // calibrate the compass by rotating the ufo through 720 deg smoothly
gke 0:62a1c91a859a 619 TxString("\r\nCalib. compass - Press CONTINUE button (x) to Start\r\n");
gke 0:62a1c91a859a 620 while ( PollRxChar() != 'x' ); // UAVPSet uses 'x' for CONTINUE button
gke 0:62a1c91a859a 621
gke 0:62a1c91a859a 622 // Do Set/Reset now
gke 2:90292f8bd179 623 if ( WriteByteHMC6352('O') != I2C_ACK ) goto HMC6352Error;
gke 0:62a1c91a859a 624
gke 0:62a1c91a859a 625 Delay1mS(7);
gke 0:62a1c91a859a 626
gke 0:62a1c91a859a 627 // set Compass device to Calibration mode
gke 2:90292f8bd179 628 if ( WriteByteHMC6352('C') != I2C_ACK ) goto HMC6352Error;
gke 0:62a1c91a859a 629
gke 0:62a1c91a859a 630 TxString("\r\nRotate horizontally 720 deg in ~30 sec. - Press CONTINUE button (x) to Finish\r\n");
gke 0:62a1c91a859a 631 while ( PollRxChar() != 'x' );
gke 0:62a1c91a859a 632
gke 0:62a1c91a859a 633 // set Compass device to End-Calibration mode
gke 2:90292f8bd179 634 if ( WriteByteHMC6352('E') != I2C_ACK ) goto HMC6352Error;
gke 0:62a1c91a859a 635
gke 0:62a1c91a859a 636 TxString("\r\nCalibration complete\r\n");
gke 0:62a1c91a859a 637
gke 0:62a1c91a859a 638 Delay1mS(50);
gke 0:62a1c91a859a 639
gke 0:62a1c91a859a 640 InitCompass();
gke 0:62a1c91a859a 641
gke 0:62a1c91a859a 642 return;
gke 0:62a1c91a859a 643
gke 2:90292f8bd179 644 HMC6352Error:
gke 2:90292f8bd179 645 I2CCOMPASS.stop();
gke 2:90292f8bd179 646
gke 2:90292f8bd179 647 I2CError[HMC6352_ID]++;
gke 2:90292f8bd179 648
gke 0:62a1c91a859a 649 TxString("Calibration FAILED\r\n");
gke 0:62a1c91a859a 650 } // CalibrateHMC6352
gke 0:62a1c91a859a 651
gke 0:62a1c91a859a 652 void InitHMC6352(void) {
gke 0:62a1c91a859a 653
gke 0:62a1c91a859a 654 // 20Hz continuous read with periodic reset.
gke 0:62a1c91a859a 655 #ifdef SUPPRESS_COMPASS_SR
gke 0:62a1c91a859a 656 #define COMP_OPMODE 0x62
gke 0:62a1c91a859a 657 #else
gke 0:62a1c91a859a 658 #define COMP_OPMODE 0x72
gke 0:62a1c91a859a 659 #endif // SUPPRESS_COMPASS_SR
gke 0:62a1c91a859a 660
gke 2:90292f8bd179 661 CompassMaxSlew = (COMPASS_SANITY_CHECK_RAD_S/HMC6352_UPDATE_S);
gke 2:90292f8bd179 662
gke 0:62a1c91a859a 663 // Set device to Compass mode
gke 0:62a1c91a859a 664 I2CCOMPASS.start();
gke 2:90292f8bd179 665 if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto HMC6352Error;
gke 2:90292f8bd179 666 if ( I2CCOMPASS.write('G') != I2C_ACK ) goto HMC6352Error;
gke 2:90292f8bd179 667 if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto HMC6352Error;
gke 2:90292f8bd179 668 if ( I2CCOMPASS.write(COMP_OPMODE) != I2C_ACK ) goto HMC6352Error;
gke 0:62a1c91a859a 669 I2CCOMPASS.stop();
gke 0:62a1c91a859a 670
gke 2:90292f8bd179 671 Delay1mS(10);
gke 0:62a1c91a859a 672
gke 0:62a1c91a859a 673 // save operation mode in Flash
gke 2:90292f8bd179 674 if ( WriteByteHMC6352('L') != I2C_ACK ) goto HMC6352Error;
gke 0:62a1c91a859a 675
gke 2:90292f8bd179 676 Delay1mS(10);
gke 0:62a1c91a859a 677
gke 0:62a1c91a859a 678 // Do Bridge Offset Set/Reset now
gke 2:90292f8bd179 679 if ( WriteByteHMC6352('O') != I2C_ACK ) goto HMC6352Error;
gke 0:62a1c91a859a 680
gke 0:62a1c91a859a 681 Delay1mS(50);
gke 0:62a1c91a859a 682
gke 0:62a1c91a859a 683 F.CompassValid = true;
gke 0:62a1c91a859a 684
gke 0:62a1c91a859a 685 return;
gke 2:90292f8bd179 686
gke 2:90292f8bd179 687 HMC6352Error:
gke 2:90292f8bd179 688 I2CCOMPASS.stop();
gke 2:90292f8bd179 689
gke 2:90292f8bd179 690 I2CError[HMC6352_ID]++;
gke 2:90292f8bd179 691
gke 0:62a1c91a859a 692 F.CompassValid = false;
gke 2:90292f8bd179 693
gke 0:62a1c91a859a 694 F.CompassFailure = true;
gke 0:62a1c91a859a 695
gke 0:62a1c91a859a 696 } // InitHMC6352
gke 0:62a1c91a859a 697
gke 0:62a1c91a859a 698 boolean HMC6352Active(void) {
gke 0:62a1c91a859a 699
gke 1:1e3318a30ddd 700 F.CompassValid = I2CCOMPASSAddressResponds( HMC6352_ID );
gke 0:62a1c91a859a 701
gke 0:62a1c91a859a 702 if ( F.CompassValid )
gke 0:62a1c91a859a 703 TrackMinI2CRate(100000);
gke 0:62a1c91a859a 704
gke 0:62a1c91a859a 705 return ( F.CompassValid );
gke 0:62a1c91a859a 706
gke 0:62a1c91a859a 707 } // HMC6352Active