Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
compass.c@1:1e3318a30ddd, 2011-02-25 (annotated)
- Committer:
- gke
- Date:
- Fri Feb 25 01:35:24 2011 +0000
- Revision:
- 1:1e3318a30ddd
- Parent:
- 0:62a1c91a859a
- Child:
- 2:90292f8bd179
This version has broken I2C - posted for debugging involvement of Simon et al.
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 | 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 | 1:1e3318a30ddd | 69 | switch ( CompassType ) { |
gke | 0:62a1c91a859a | 70 | case HMC5843: |
gke | 1:1e3318a30ddd | 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 | 1:1e3318a30ddd | 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 | 1:1e3318a30ddd | 171 | r = I2CCOMPASS.write(HMC5843_WR); |
gke | 0:62a1c91a859a | 172 | r = I2CCOMPASS.write(0x03); // point to data |
gke | 0:62a1c91a859a | 173 | I2CCOMPASS.stop(); |
gke | 0:62a1c91a859a | 174 | |
gke | 1:1e3318a30ddd | 175 | I2CCOMPASS.blockread(HMC5843_RD, 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 | 1:1e3318a30ddd | 193 | DebugPin = true; |
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 | 1:1e3318a30ddd | 204 | DebugPin = false; |
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 | 1:1e3318a30ddd | 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 | 1:1e3318a30ddd | 238 | r = I2CCOMPASS.write(HMC5843_WR); |
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 | 1:1e3318a30ddd | 248 | |
gke | 1:1e3318a30ddd | 249 | F.CompassValid = I2CCOMPASSAddressResponds( HMC5843_ID ); |
gke | 1:1e3318a30ddd | 250 | |
gke | 0:62a1c91a859a | 251 | if ( F.CompassValid ) |
gke | 0:62a1c91a859a | 252 | TrackMinI2CRate(400000); |
gke | 0:62a1c91a859a | 253 | |
gke | 0:62a1c91a859a | 254 | return ( F.CompassValid ); |
gke | 0:62a1c91a859a | 255 | |
gke | 0:62a1c91a859a | 256 | } // IsHMC5843Active |
gke | 0:62a1c91a859a | 257 | |
gke | 0:62a1c91a859a | 258 | //________________________________________________________________________________________ |
gke | 0:62a1c91a859a | 259 | |
gke | 0:62a1c91a859a | 260 | // HMC6352 Compass |
gke | 0:62a1c91a859a | 261 | |
gke | 0:62a1c91a859a | 262 | void ReadHMC6352(void); |
gke | 0:62a1c91a859a | 263 | uint8 WriteByteHMC6352(uint8); |
gke | 0:62a1c91a859a | 264 | void GetHMC6352Parameters(void); |
gke | 0:62a1c91a859a | 265 | void DoHMC6352Test(void); |
gke | 0:62a1c91a859a | 266 | void CalibrateHMC6352(void); |
gke | 0:62a1c91a859a | 267 | void InitHMC6352(void); |
gke | 0:62a1c91a859a | 268 | boolean IsHMC6352Active(void); |
gke | 0:62a1c91a859a | 269 | |
gke | 0:62a1c91a859a | 270 | void ReadHMC6352(void) { |
gke | 0:62a1c91a859a | 271 | static i16u v; |
gke | 0:62a1c91a859a | 272 | |
gke | 0:62a1c91a859a | 273 | I2CCOMPASS.start(); |
gke | 1:1e3318a30ddd | 274 | F.CompassMissRead = I2CCOMPASS.write(HMC6352_RD) != I2C_ACK; |
gke | 0:62a1c91a859a | 275 | v.b1 = I2CCOMPASS.read(I2C_ACK); |
gke | 0:62a1c91a859a | 276 | v.b0 = I2CCOMPASS.read(I2C_NACK); |
gke | 0:62a1c91a859a | 277 | I2CCOMPASS.stop(); |
gke | 0:62a1c91a859a | 278 | |
gke | 0:62a1c91a859a | 279 | MagHeading = Make2Pi( ((real32)v.i16 * PI) / 1800.0 - CompassOffset ); // Radians |
gke | 0:62a1c91a859a | 280 | } // ReadHMC6352 |
gke | 0:62a1c91a859a | 281 | |
gke | 0:62a1c91a859a | 282 | uint8 WriteByteHMC6352(uint8 d) { |
gke | 0:62a1c91a859a | 283 | I2CCOMPASS.start(); |
gke | 1:1e3318a30ddd | 284 | if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto WError; |
gke | 0:62a1c91a859a | 285 | if ( I2CCOMPASS.write(d) != I2C_ACK ) goto WError; |
gke | 0:62a1c91a859a | 286 | I2CCOMPASS.stop(); |
gke | 0:62a1c91a859a | 287 | |
gke | 0:62a1c91a859a | 288 | return( I2C_ACK ); |
gke | 0:62a1c91a859a | 289 | WError: |
gke | 0:62a1c91a859a | 290 | I2CCOMPASS.stop(); |
gke | 0:62a1c91a859a | 291 | return ( I2C_NACK ); |
gke | 0:62a1c91a859a | 292 | } // WriteByteHMC6352 |
gke | 0:62a1c91a859a | 293 | |
gke | 0:62a1c91a859a | 294 | char CP[9]; |
gke | 0:62a1c91a859a | 295 | |
gke | 0:62a1c91a859a | 296 | #define TEST_COMP_OPMODE 0x70 // standby mode to reliably read EEPROM |
gke | 0:62a1c91a859a | 297 | |
gke | 0:62a1c91a859a | 298 | void GetHMC6352Parameters(void) { |
gke | 0:62a1c91a859a | 299 | uint8 r; |
gke | 0:62a1c91a859a | 300 | |
gke | 0:62a1c91a859a | 301 | I2CCOMPASS.start(); |
gke | 1:1e3318a30ddd | 302 | if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 303 | if ( I2CCOMPASS.write('G') != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 304 | if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 305 | if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 306 | I2CCOMPASS.stop(); |
gke | 0:62a1c91a859a | 307 | |
gke | 0:62a1c91a859a | 308 | Delay1mS(20); |
gke | 0:62a1c91a859a | 309 | |
gke | 0:62a1c91a859a | 310 | for (r = 0; r <= (uint8)8; r++) { // must have this timing - not block read! |
gke | 0:62a1c91a859a | 311 | |
gke | 0:62a1c91a859a | 312 | I2CCOMPASS.start(); |
gke | 1:1e3318a30ddd | 313 | if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 314 | if ( I2CCOMPASS.write('r') != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 315 | if ( I2CCOMPASS.write(r) != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 316 | I2CCOMPASS.stop(); |
gke | 0:62a1c91a859a | 317 | |
gke | 0:62a1c91a859a | 318 | Delay1mS(10); |
gke | 0:62a1c91a859a | 319 | |
gke | 0:62a1c91a859a | 320 | I2CCOMPASS.start(); |
gke | 1:1e3318a30ddd | 321 | if ( I2CCOMPASS.write(HMC6352_RD) != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 322 | CP[r] = I2CCOMPASS.read(I2C_NACK); |
gke | 0:62a1c91a859a | 323 | I2CCOMPASS.stop(); |
gke | 0:62a1c91a859a | 324 | |
gke | 0:62a1c91a859a | 325 | Delay1mS(10); |
gke | 0:62a1c91a859a | 326 | } |
gke | 0:62a1c91a859a | 327 | |
gke | 0:62a1c91a859a | 328 | return; |
gke | 0:62a1c91a859a | 329 | |
gke | 0:62a1c91a859a | 330 | CTerror: |
gke | 0:62a1c91a859a | 331 | I2CCOMPASS.stop(); |
gke | 0:62a1c91a859a | 332 | TxString("FAIL\r\n"); |
gke | 0:62a1c91a859a | 333 | |
gke | 0:62a1c91a859a | 334 | } // GetHMC6352Parameters |
gke | 0:62a1c91a859a | 335 | |
gke | 0:62a1c91a859a | 336 | void DoHMC6352Test(void) { |
gke | 0:62a1c91a859a | 337 | static real32 Temp; |
gke | 0:62a1c91a859a | 338 | |
gke | 0:62a1c91a859a | 339 | TxString("\r\nCompass test (HMC6352)\r\n"); |
gke | 0:62a1c91a859a | 340 | |
gke | 0:62a1c91a859a | 341 | I2CCOMPASS.start(); |
gke | 1:1e3318a30ddd | 342 | if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 343 | if ( I2CCOMPASS.write('G') != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 344 | if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 345 | if ( I2CCOMPASS.write(TEST_COMP_OPMODE) != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 346 | I2CCOMPASS.stop(); |
gke | 0:62a1c91a859a | 347 | |
gke | 0:62a1c91a859a | 348 | Delay1mS(1); |
gke | 0:62a1c91a859a | 349 | |
gke | 0:62a1c91a859a | 350 | // I2CCOMPASS.start(); // Do Set/Reset now |
gke | 0:62a1c91a859a | 351 | if ( WriteByteHMC6352('O') != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 352 | |
gke | 0:62a1c91a859a | 353 | Delay1mS(7); |
gke | 0:62a1c91a859a | 354 | |
gke | 0:62a1c91a859a | 355 | GetHMC6352Parameters(); |
gke | 0:62a1c91a859a | 356 | |
gke | 0:62a1c91a859a | 357 | TxString("\r\nRegisters\r\n"); |
gke | 0:62a1c91a859a | 358 | TxString("\t0:\tI2C"); |
gke | 0:62a1c91a859a | 359 | TxString("\t 0x"); |
gke | 0:62a1c91a859a | 360 | TxValH(CP[0]); |
gke | 0:62a1c91a859a | 361 | if ( CP[0] != (uint8)0x42 ) |
gke | 0:62a1c91a859a | 362 | TxString("\t Error expected 0x42 for HMC6352"); |
gke | 0:62a1c91a859a | 363 | TxNextLine(); |
gke | 0:62a1c91a859a | 364 | |
gke | 0:62a1c91a859a | 365 | Temp = (CP[1]*256)|CP[2]; |
gke | 0:62a1c91a859a | 366 | TxString("\t1:2:\tXOffset\t"); |
gke | 0:62a1c91a859a | 367 | TxVal32((int32)Temp, 0, 0); |
gke | 0:62a1c91a859a | 368 | TxNextLine(); |
gke | 0:62a1c91a859a | 369 | |
gke | 0:62a1c91a859a | 370 | Temp = (CP[3]*256)|CP[4]; |
gke | 0:62a1c91a859a | 371 | TxString("\t3:4:\tYOffset\t"); |
gke | 0:62a1c91a859a | 372 | TxVal32((int32)Temp, 0, 0); |
gke | 0:62a1c91a859a | 373 | TxNextLine(); |
gke | 0:62a1c91a859a | 374 | |
gke | 0:62a1c91a859a | 375 | TxString("\t5:\tDelay\t"); |
gke | 0:62a1c91a859a | 376 | TxVal32((int32)CP[5], 0, 0); |
gke | 0:62a1c91a859a | 377 | TxNextLine(); |
gke | 0:62a1c91a859a | 378 | |
gke | 0:62a1c91a859a | 379 | TxString("\t6:\tNSum\t"); |
gke | 0:62a1c91a859a | 380 | TxVal32((int32)CP[6], 0, 0); |
gke | 0:62a1c91a859a | 381 | TxNextLine(); |
gke | 0:62a1c91a859a | 382 | |
gke | 0:62a1c91a859a | 383 | TxString("\t7:\tSW Ver\t"); |
gke | 0:62a1c91a859a | 384 | TxString(" 0x"); |
gke | 0:62a1c91a859a | 385 | TxValH(CP[7]); |
gke | 0:62a1c91a859a | 386 | TxNextLine(); |
gke | 0:62a1c91a859a | 387 | |
gke | 0:62a1c91a859a | 388 | TxString("\t8:\tOpMode:"); |
gke | 0:62a1c91a859a | 389 | switch ( ( CP[8] >> 5 ) & 0x03 ) { |
gke | 0:62a1c91a859a | 390 | case 0: |
gke | 0:62a1c91a859a | 391 | TxString(" 1Hz"); |
gke | 0:62a1c91a859a | 392 | break; |
gke | 0:62a1c91a859a | 393 | case 1: |
gke | 0:62a1c91a859a | 394 | TxString(" 5Hz"); |
gke | 0:62a1c91a859a | 395 | break; |
gke | 0:62a1c91a859a | 396 | case 2: |
gke | 0:62a1c91a859a | 397 | TxString(" 10Hz"); |
gke | 0:62a1c91a859a | 398 | break; |
gke | 0:62a1c91a859a | 399 | case 3: |
gke | 0:62a1c91a859a | 400 | TxString(" 20Hz"); |
gke | 0:62a1c91a859a | 401 | break; |
gke | 0:62a1c91a859a | 402 | } |
gke | 0:62a1c91a859a | 403 | |
gke | 0:62a1c91a859a | 404 | if ( CP[8] & 0x10 ) TxString(" S/R"); |
gke | 0:62a1c91a859a | 405 | |
gke | 0:62a1c91a859a | 406 | switch ( CP[8] & 0x03 ) { |
gke | 0:62a1c91a859a | 407 | case 0: |
gke | 0:62a1c91a859a | 408 | TxString(" Standby"); |
gke | 0:62a1c91a859a | 409 | break; |
gke | 0:62a1c91a859a | 410 | case 1: |
gke | 0:62a1c91a859a | 411 | TxString(" Query"); |
gke | 0:62a1c91a859a | 412 | break; |
gke | 0:62a1c91a859a | 413 | case 2: |
gke | 0:62a1c91a859a | 414 | TxString(" Continuous"); |
gke | 0:62a1c91a859a | 415 | break; |
gke | 0:62a1c91a859a | 416 | case 3: |
gke | 0:62a1c91a859a | 417 | TxString(" Not-allowed"); |
gke | 0:62a1c91a859a | 418 | break; |
gke | 0:62a1c91a859a | 419 | } |
gke | 0:62a1c91a859a | 420 | TxNextLine(); |
gke | 0:62a1c91a859a | 421 | |
gke | 0:62a1c91a859a | 422 | InitCompass(); |
gke | 0:62a1c91a859a | 423 | if ( !F.CompassValid ) goto CTerror; |
gke | 0:62a1c91a859a | 424 | |
gke | 0:62a1c91a859a | 425 | Delay1mS(50); |
gke | 0:62a1c91a859a | 426 | |
gke | 0:62a1c91a859a | 427 | ReadHMC6352(); |
gke | 0:62a1c91a859a | 428 | if ( F.CompassMissRead ) goto CTerror; |
gke | 0:62a1c91a859a | 429 | |
gke | 0:62a1c91a859a | 430 | TxNextLine(); |
gke | 0:62a1c91a859a | 431 | TxVal32(MagHeading * RADDEG * 10.0, 1, 0); |
gke | 1:1e3318a30ddd | 432 | TxString(" deg (Magnetic)\r\n"); |
gke | 0:62a1c91a859a | 433 | Heading = Headingp = Make2Pi( MagHeading - MagDeviation - CompassOffset ); |
gke | 0:62a1c91a859a | 434 | TxVal32(Heading * RADDEG * 10.0, 1, 0); |
gke | 0:62a1c91a859a | 435 | TxString(" deg (True)\r\n"); |
gke | 0:62a1c91a859a | 436 | |
gke | 0:62a1c91a859a | 437 | return; |
gke | 0:62a1c91a859a | 438 | CTerror: |
gke | 0:62a1c91a859a | 439 | I2CCOMPASS.stop(); |
gke | 0:62a1c91a859a | 440 | TxString("FAIL\r\n"); |
gke | 0:62a1c91a859a | 441 | } // DoHMC6352Test |
gke | 0:62a1c91a859a | 442 | |
gke | 0:62a1c91a859a | 443 | void CalibrateHMC6352(void) { // calibrate the compass by rotating the ufo through 720 deg smoothly |
gke | 0:62a1c91a859a | 444 | TxString("\r\nCalib. compass - Press CONTINUE button (x) to Start\r\n"); |
gke | 0:62a1c91a859a | 445 | while ( PollRxChar() != 'x' ); // UAVPSet uses 'x' for CONTINUE button |
gke | 0:62a1c91a859a | 446 | |
gke | 0:62a1c91a859a | 447 | // Do Set/Reset now |
gke | 0:62a1c91a859a | 448 | if ( WriteByteHMC6352('O') != I2C_ACK ) goto CCerror; |
gke | 0:62a1c91a859a | 449 | |
gke | 0:62a1c91a859a | 450 | Delay1mS(7); |
gke | 0:62a1c91a859a | 451 | |
gke | 0:62a1c91a859a | 452 | // set Compass device to Calibration mode |
gke | 0:62a1c91a859a | 453 | if ( WriteByteHMC6352('C') != I2C_ACK ) goto CCerror; |
gke | 0:62a1c91a859a | 454 | |
gke | 0:62a1c91a859a | 455 | TxString("\r\nRotate horizontally 720 deg in ~30 sec. - Press CONTINUE button (x) to Finish\r\n"); |
gke | 0:62a1c91a859a | 456 | while ( PollRxChar() != 'x' ); |
gke | 0:62a1c91a859a | 457 | |
gke | 0:62a1c91a859a | 458 | // set Compass device to End-Calibration mode |
gke | 0:62a1c91a859a | 459 | if ( WriteByteHMC6352('E') != I2C_ACK ) goto CCerror; |
gke | 0:62a1c91a859a | 460 | |
gke | 0:62a1c91a859a | 461 | TxString("\r\nCalibration complete\r\n"); |
gke | 0:62a1c91a859a | 462 | |
gke | 0:62a1c91a859a | 463 | Delay1mS(50); |
gke | 0:62a1c91a859a | 464 | |
gke | 0:62a1c91a859a | 465 | InitCompass(); |
gke | 0:62a1c91a859a | 466 | |
gke | 0:62a1c91a859a | 467 | return; |
gke | 0:62a1c91a859a | 468 | |
gke | 0:62a1c91a859a | 469 | CCerror: |
gke | 0:62a1c91a859a | 470 | TxString("Calibration FAILED\r\n"); |
gke | 0:62a1c91a859a | 471 | } // CalibrateHMC6352 |
gke | 0:62a1c91a859a | 472 | |
gke | 0:62a1c91a859a | 473 | void InitHMC6352(void) { |
gke | 0:62a1c91a859a | 474 | |
gke | 0:62a1c91a859a | 475 | // 20Hz continuous read with periodic reset. |
gke | 0:62a1c91a859a | 476 | #ifdef SUPPRESS_COMPASS_SR |
gke | 0:62a1c91a859a | 477 | #define COMP_OPMODE 0x62 |
gke | 0:62a1c91a859a | 478 | #else |
gke | 0:62a1c91a859a | 479 | #define COMP_OPMODE 0x72 |
gke | 0:62a1c91a859a | 480 | #endif // SUPPRESS_COMPASS_SR |
gke | 0:62a1c91a859a | 481 | |
gke | 0:62a1c91a859a | 482 | // Set device to Compass mode |
gke | 0:62a1c91a859a | 483 | I2CCOMPASS.start(); |
gke | 1:1e3318a30ddd | 484 | if ( I2CCOMPASS.write(HMC6352_WR) != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 485 | if ( I2CCOMPASS.write('G') != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 486 | if ( I2CCOMPASS.write(0x74) != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 487 | if ( I2CCOMPASS.write(COMP_OPMODE) != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 488 | I2CCOMPASS.stop(); |
gke | 0:62a1c91a859a | 489 | |
gke | 0:62a1c91a859a | 490 | Delay1mS(1); |
gke | 0:62a1c91a859a | 491 | |
gke | 0:62a1c91a859a | 492 | // save operation mode in Flash |
gke | 0:62a1c91a859a | 493 | if ( WriteByteHMC6352('L') != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 494 | |
gke | 0:62a1c91a859a | 495 | Delay1mS(1); |
gke | 0:62a1c91a859a | 496 | |
gke | 0:62a1c91a859a | 497 | // Do Bridge Offset Set/Reset now |
gke | 0:62a1c91a859a | 498 | if ( WriteByteHMC6352('O') != I2C_ACK ) goto CTerror; |
gke | 0:62a1c91a859a | 499 | |
gke | 0:62a1c91a859a | 500 | Delay1mS(50); |
gke | 0:62a1c91a859a | 501 | |
gke | 0:62a1c91a859a | 502 | F.CompassValid = true; |
gke | 0:62a1c91a859a | 503 | |
gke | 0:62a1c91a859a | 504 | return; |
gke | 0:62a1c91a859a | 505 | CTerror: |
gke | 0:62a1c91a859a | 506 | F.CompassValid = false; |
gke | 0:62a1c91a859a | 507 | Stats[CompassFailS]++; |
gke | 0:62a1c91a859a | 508 | F.CompassFailure = true; |
gke | 0:62a1c91a859a | 509 | |
gke | 0:62a1c91a859a | 510 | I2CCOMPASS.stop(); |
gke | 0:62a1c91a859a | 511 | } // InitHMC6352 |
gke | 0:62a1c91a859a | 512 | |
gke | 0:62a1c91a859a | 513 | boolean HMC6352Active(void) { |
gke | 0:62a1c91a859a | 514 | |
gke | 1:1e3318a30ddd | 515 | F.CompassValid = I2CCOMPASSAddressResponds( HMC6352_ID ); |
gke | 0:62a1c91a859a | 516 | |
gke | 0:62a1c91a859a | 517 | if ( F.CompassValid ) |
gke | 0:62a1c91a859a | 518 | TrackMinI2CRate(100000); |
gke | 0:62a1c91a859a | 519 | |
gke | 0:62a1c91a859a | 520 | return ( F.CompassValid ); |
gke | 0:62a1c91a859a | 521 | |
gke | 0:62a1c91a859a | 522 | } // HMC6352Active |