Mercurial > public > hwos_code
annotate src/compass.c @ 656:8af5aefbcdaf default tip
Update to 3.31 beta
| author | heinrichsweikamp |
|---|---|
| date | Thu, 27 Nov 2025 18:32:58 +0100 |
| parents | 75e90cd0c2c3 |
| children |
| rev | line source |
|---|---|
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
214
diff
changeset
|
1 ////////////////////////////////////////////////////////////////////////////// |
|
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
214
diff
changeset
|
2 /// compass.c |
| 623 | 3 /// Compute north direction from magnetic/acceleration measures V3.02.1 |
| 604 | 4 /// |
| 654 | 5 /// Copyright (c) 2012-2015, JD Gascuel, heinrichs weikamp gmbh, all right reserved. |
| 0 | 6 ////////////////////////////////////////////////////////////////////////////// |
| 7 // HISTORY | |
| 8 // 2012-12-01 [jDG] Creation | |
| 9 // 2012-12-23 [jDG] Added filtering. | |
| 10 // 2012-12-30 [jDG] Added calibration (spherical best fit). | |
|
282
7d9edd3b8c86
Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
214
diff
changeset
|
11 // 2015-05-22 [jDG] Minor cleanups. Smaller calibration code. |
| 0 | 12 |
| 623 | 13 #include "configuration.inc" |
| 0 | 14 #include "compass.h" |
| 15 | |
| 623 | 16 |
| 17 #ifdef _compass | |
| 18 | |
| 19 | |
| 0 | 20 ////////////////////////////////////////////////////////////////////////////// |
| 650 | 21 // |
| 22 // Put compass data into bank 13 (stack) and bank 9 (variables) | |
| 23 // | |
| 0 | 24 #ifndef UNIX |
| 650 | 25 # pragma udata overlay bank13=0xd00 |
| 26 static char C_STACK[256]; // overlay C-code data stack here | |
| 27 # define C_STACK_ADDR C_STACK | |
| 28 # define RESET_C_STACK \ | |
| 29 _asm \ | |
| 30 LFSR 1,C_STACK_ADDR \ | |
| 31 LFSR 2,C_STACK_ADDR \ | |
| 604 | 32 _endasm |
| 650 | 33 # pragma udata bank9a = 0x900 |
| 604 | 34 # pragma code compass_run |
| 650 | 35 # define PARAMETER static |
| 36 # define OVERLAY overlay | |
| 0 | 37 #else |
| 604 | 38 # define RESET_C_STACK |
| 650 | 39 # define PARAMETER |
| 40 # define OVERLAY | |
| 0 | 41 #endif |
| 42 | |
| 650 | 43 |
| 0 | 44 ////////////////////////////////////////////////////////////////////////////// |
| 45 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error | |
| 46 // | |
| 604 | 47 #define K1 (5701) // needs K1/2**16 |
| 48 #define K2 (1645) // needs K2/2**48 WAS NEGATIV | |
| 49 #define K3 ( 446) // needs K3/2**80 | |
| 50 | |
| 0 | 51 |
| 52 ////////////////////////////////////////////////////////////////////////////// | |
| 604 | 53 // Interface to assembler multiplies |
| 54 | |
| 0 | 55 Int16 umul(PARAMETER Int16 a, PARAMETER Int16 b) |
| 56 { | |
| 604 | 57 extern Int16 compass_umul(void); |
| 58 extern Int16 compass_a, compass_b; | |
| 59 compass_a = a; | |
| 60 compass_b = b; | |
| 61 return compass_umul(); | |
| 0 | 62 } |
| 63 | |
| 64 Int16 imul(PARAMETER Int16 a, PARAMETER Int16 b) | |
| 65 { | |
| 604 | 66 extern Int16 compass_imul(void); |
| 67 extern Int16 compass_a, compass_b; | |
| 68 compass_a = a; | |
| 69 compass_b = b; | |
| 70 return compass_imul(); | |
| 0 | 71 } |
| 72 | |
| 73 ////////////////////////////////////////////////////////////////////////////// | |
| 74 /// Returns a / b * 2**16 | |
| 75 /// | |
| 604 | 76 /// A 16/16 -> 16 bits divide, returning a scaled result. |
| 0 | 77 /// Used to multiply fractional numbers in the range 0..1, |
| 78 /// represented as 0..32767. | |
| 604 | 79 |
| 0 | 80 Int16 udiv(PARAMETER Int16 a, PARAMETER Int16 b) |
| 81 { | |
| 604 | 82 OVERLAY Int16 d, r; |
| 623 | 83 OVERLAY char failsafe=250; |
| 84 | |
| 604 | 85 //---- Pre-scale both numerator and denominator -------------------------- |
| 86 while( (((a>>8) | (b>>8)) & 0xC0) == 0 ) | |
| 87 { | |
| 623 | 88 failsafe--; |
| 89 if( failsafe == 0 ) break; | |
| 90 | |
| 604 | 91 a <<= 1; |
| 92 b <<= 1; | |
| 93 } | |
| 0 | 94 |
| 604 | 95 //---- Make division trials ---------------------------------------------- |
| 96 d = 0x4000; // start with 0.5, because 1.0 is sign bit | |
| 97 b >>= 1; // hence pre-shift b | |
| 98 r = 0; | |
| 99 do { | |
| 100 if( a >= b ) // a is big enough ? | |
| 101 { | |
| 102 a -= b; // then count d times b out of it | |
| 103 r |= d; // and accumulate that bit | |
| 104 } | |
| 105 b >>= 1; // then loop trying twice smaller | |
| 106 d >>= 1; | |
| 107 } while( b ); | |
| 108 return r; | |
| 0 | 109 } |
| 110 | |
| 111 ////////////////////////////////////////////////////////////////////////////// | |
| 112 /// Computes atan(y/x) in Angle, for x, y in range 0..32767 | |
| 113 /// | |
| 114 /// Results a single quadrant Angle, in the range 0 .. Q_PI/2 | |
| 604 | 115 |
| 0 | 116 Angle utan(PARAMETER Int16 y, PARAMETER Int16 x) |
| 117 { | |
| 604 | 118 OVERLAY Int16 ratio, angle, x2, x3; |
| 0 | 119 |
| 604 | 120 //---- Handle zero divisor ----------------------------------------------- |
| 121 if( x == 0 ) | |
| 122 return (y == 0) ? 0 : Q_PIO2; | |
| 0 | 123 |
| 604 | 124 //---- Make it half-quadrant : 0 .. 45 deg ------------------------------- |
| 125 ratio = (x > y) ? udiv(y, x) : udiv(x, y); | |
| 0 | 126 |
| 604 | 127 //---- Then apply the polynomial approximation --------------------------- |
| 128 angle = umul(K1, ratio); // r*K1 / 2**16 | |
| 129 x2 = umul(ratio, ratio); // r**2 / 2**16 | |
| 130 x3 = umul(x2, ratio); // r**3 / 2**32 | |
| 131 angle -= umul(x3, K2); // K2*r**3 / 2**48: NEGATIV. | |
| 0 | 132 |
| 604 | 133 x3 = umul(x3, x2); // r**5 / 2**64 |
| 134 angle += umul(x3, K3); // K3*r**5 / 2**80 | |
| 0 | 135 |
| 604 | 136 //---- Recover the full quadrant ----------------------------------------- |
| 137 return (x < y) ? (Angle)(Q_PIO2 - angle) | |
| 138 : (Angle)(angle); | |
| 0 | 139 } |
| 140 | |
| 141 ////////////////////////////////////////////////////////////////////////////// | |
| 142 /// Computes atan2(y/x) in Angle, for x, y in range -32768 to 32767 | |
| 143 /// | |
| 144 /// Results a four quadrant Angle, in the range -Q_PI .. +Q_PI | |
| 604 | 145 |
| 0 | 146 Angle itan(PARAMETER Int16 y, PARAMETER Int16 x) |
| 147 { | |
| 604 | 148 // Beware: -32768 is not properly handled (sign error) |
| 149 if( x == -32768 ) x = -32767; | |
| 150 if( y == -32768 ) y = -32767; | |
| 0 | 151 |
| 604 | 152 if( x >= 0 ) |
| 153 if( y >= 0 ) // first quadrant: 0..90 deg. | |
| 154 return utan(y,x); | |
| 155 else // fourth quadrant: 0..-90 deg | |
| 156 return -utan(-y,x); | |
| 157 else | |
| 158 if( y >= 0 ) // second quadrant: 90..180 deg | |
| 159 return Q_PI - utan(y, -x); | |
| 160 else // third quadrant: -90..-180 deg; | |
| 161 return -Q_PI + utan(-y, -x); | |
| 0 | 162 } |
| 163 | |
| 164 ////////////////////////////////////////////////////////////////////////////// | |
| 165 /// Computes cos(theta) = sqrtf(x2/h2), | |
| 166 /// when theta = atan(y/x) and h2=x*x+y*y | |
| 167 /// | |
| 604 | 168 |
| 0 | 169 Int16 cosxh(PARAMETER Int16 x2, PARAMETER Int16 h2) |
| 170 { | |
| 604 | 171 OVERLAY Int16 r = 0; |
| 172 OVERLAY Int16 d = 0x4000; | |
| 0 | 173 |
| 604 | 174 do { |
| 175 OVERLAY Int16 a = r + d; | |
| 176 a = umul(a, a); | |
| 177 a = umul(a, h2); | |
| 178 if( a <= x2 ) r += d; | |
| 179 d >>= 1; | |
| 180 } while( d ); | |
| 0 | 181 |
| 604 | 182 return r; |
| 0 | 183 } |
| 184 | |
| 185 ////////////////////////////////////////////////////////////////////////////// | |
| 186 /// Computes both sin and cos of angle y/x, | |
| 187 /// with h = sqrt(x**2+y**2). | |
| 188 /// | |
| 604 | 189 |
| 0 | 190 void sincos(PARAMETER Int16 x, PARAMETER Int16 y, Int16* sin, Int16* cos) |
| 191 { | |
| 604 | 192 OVERLAY Int16 x2, y2, h2; |
| 623 | 193 OVERLAY char failsafe = 250; |
| 194 | |
| 195 //---- Fold into one quadrant -------------------------------------------- | |
| 604 | 196 OVERLAY char neg = 0; |
| 197 if( x < 0 ) | |
| 198 { | |
| 199 neg |= 1; | |
| 200 x = -x; | |
| 201 } | |
| 202 if( y < 0 ) | |
| 203 { | |
| 204 neg |= 2; | |
| 205 y = -y; | |
| 206 } | |
| 623 | 207 |
| 208 //---- Pre-scale both numerator and denominator ---------------------- | |
| 604 | 209 while( (((x>>8) | (y>>8)) & 0xE0) == 0 ) |
| 210 { | |
| 623 | 211 failsafe--; |
| 212 if( failsafe == 0 ) break; | |
| 213 | |
| 604 | 214 x <<= 1; |
| 215 y <<= 1; | |
| 216 } | |
| 0 | 217 |
| 604 | 218 //---- Uses trig() to do the stuff one on quadrant ------------------- |
| 219 x2 = umul(x,x); | |
| 220 y2 = umul(y,y); | |
| 221 h2 = x2 + y2; | |
| 222 x2 = cosxh(x2, h2); | |
| 0 | 223 |
| 604 | 224 //---- Results back in four quadrants -------------------------------- |
| 225 *cos = (neg & 1) ? -x2 : x2; | |
| 226 y2 = cosxh(y2, h2); | |
| 227 *sin = (neg & 2) ? -y2 : y2; | |
| 0 | 228 } |
| 229 | |
| 230 ////////////////////////////////////////////////////////////////////////////// | |
| 231 // | |
| 232 | |
| 233 void compass(void) | |
| 234 { | |
| 604 | 235 OVERLAY Int16 sin, cos; |
| 236 OVERLAY Int16 iBfx, iBfy, Gz; | |
| 237 OVERLAY Int16 iBpx, iBpy, iBpz; | |
| 238 | |
| 239 RESET_C_STACK; | |
| 0 | 240 |
| 623 | 241 //---- Detect uncalibrated compass --------------------------------------- |
| 242 if( !compass_CX_f && !compass_CY_f && !compass_CZ_f ) | |
| 243 { | |
| 244 // no usable compass is signaled by bit 15 set to 1 | |
| 245 compass_heading_new = 32768; | |
| 246 return; | |
| 247 } | |
| 248 | |
| 604 | 249 //---- Make hard iron correction ----------------------------------------- |
| 250 // Measured magnetometer orientation, measured ok | |
| 251 // From matthias drawing: (X,Y,Z) --> (X,Y,Z) : no rotation | |
| 252 iBpx = compass_DX_f - compass_CX_f; // X | |
| 253 iBpy = compass_DY_f - compass_CY_f; // Y | |
| 254 iBpz = compass_DZ_f - compass_CZ_f; // Z | |
| 0 | 255 |
| 604 | 256 //---- Calculate sine and cosine of roll angle Phi ----------------------- |
| 257 sincos(accel_DZ_f, accel_DY_f, &sin, &cos); | |
| 650 | 258 |
| 604 | 259 //---- rotate by roll angle (-Phi) --------------------------------------- |
| 260 iBfy = imul(iBpy, cos) - imul(iBpz, sin); | |
| 261 iBpz = imul(iBpy, sin) + imul(iBpz, cos); | |
| 262 Gz = imul(accel_DY_f, sin) + imul(accel_DZ_f, cos); | |
| 0 | 263 |
| 604 | 264 //---- calculate sin and cosine of pitch angle Theta --------------------- |
| 265 sincos(Gz, -accel_DX_f, &sin, &cos); // NOTE: changed sin sign | |
| 628 | 266 |
| 604 | 267 /* correct cosine if pitch not in range -90 to 90 degrees */ |
| 268 if( cos < 0 ) cos = -cos; | |
| 0 | 269 |
| 604 | 270 ///---- de-rotate by pitch angle Theta ----------------------------------- |
| 271 iBfx = imul(iBpx, cos) + imul(iBpz, sin); | |
| 0 | 272 |
| 604 | 273 //---- calculate current yaw = e-compass angle Psi ----------------------- |
| 274 // Result in degree (no need of 0.01 deg precision... | |
| 623 | 275 compass_heading_new = itan(-iBfy, iBfx) / 100; |
| 0 | 276 |
| 604 | 277 // Result in 0..360 range: |
| 623 | 278 if( compass_heading_new < 0 ) |
| 279 compass_heading_new += 360; | |
| 0 | 280 } |
| 623 | 281 |
| 282 #endif // _compass |