Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
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?
User | Revision | Line number | New 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 |