Rev 2048 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2048 | Rev 2051 | ||
|---|---|---|---|
| Line 51... | Line 51... | ||
| 51 | int32_t attitude[2]; |
51 | int32_t attitude[2]; |
| Line 52... | Line 52... | ||
| 52 | 52 | ||
| Line 53... | Line 53... | ||
| 53 | //int readingHeight = 0; |
53 | //int readingHeight = 0; |
| 54 | - | ||
| 55 | // Yaw angle and compass stuff. |
- | |
| 56 | - | ||
| 57 | // This is updated/written from MM3. Negative angle indicates invalid data. |
- | |
| 58 | int16_t magneticHeading = -1; |
- | |
| 59 | - | ||
| 60 | // This is NOT updated from MM3. Negative angle indicates invalid data. |
- | |
| 61 | // int16_t headingInDegrees = -1; |
54 | |
| Line 62... | Line 55... | ||
| 62 | 55 | // Yaw angle and compass stuff. |
|
| 63 | int32_t targetHeading; |
56 | int32_t headingError; |
| 64 | 57 | ||
| Line 65... | Line 58... | ||
| 65 | // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
58 | // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
| Line 66... | Line 59... | ||
| 66 | // Not necessary. Never read anywhere. |
59 | // Not necessary. Never read anywhere. |
| 67 | // int16_t compassOffCourse = 0; |
60 | // int16_t compassOffCourse = 0; |
| Line 68... | Line 61... | ||
| 68 | 61 | ||
| Line 201... | Line 194... | ||
| 201 | * Calculate yaw gyro integral (~ to rotation angle) |
194 | * Calculate yaw gyro integral (~ to rotation angle) |
| 202 | * Limit heading proportional to 0 deg to 360 deg |
195 | * Limit heading proportional to 0 deg to 360 deg |
| 203 | */ |
196 | */ |
| 204 | heading += ACYawRate; |
197 | heading += ACYawRate; |
| 205 | intervalWrap(&heading, YAWOVER360); |
198 | intervalWrap(&heading, YAWOVER360); |
| - | 199 | ||
| - | 200 | headingError += ACYawRate; |
|
| - | 201 | ||
| - | 202 | debugOut.analog[27] = heading / 100; |
|
| - | 203 | ||
| 206 | /* |
204 | /* |
| 207 | * Pitch axis integration and range boundary wrap. |
205 | * Pitch axis integration and range boundary wrap. |
| 208 | */ |
206 | */ |
| 209 | for (axis = PITCH; axis <= ROLL; axis++) { |
207 | for (axis = PITCH; axis <= ROLL; axis++) { |
| 210 | attitude[axis] += ACRate[axis]; |
208 | attitude[axis] += ACRate[axis]; |
| Line 344... | Line 342... | ||
| 344 | // Compass is invalid, skip. |
342 | // Compass is invalid, skip. |
| 345 | if (magneticHeading < 0) |
343 | if (magneticHeading < 0) |
| 346 | return; |
344 | return; |
| Line 347... | Line 345... | ||
| 347 | 345 | ||
| 348 | heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
346 | heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
| - | 347 | //targetHeading = heading; |
|
| Line 349... | Line 348... | ||
| 349 | targetHeading = heading; |
348 | headingError = 0; |
| 350 | 349 | ||
| Line 351... | Line 350... | ||
| 351 | debugOut.digital[0] ^= DEBUG_COMPASS; |
350 | debugOut.digital[0] ^= DEBUG_COMPASS; |
| 352 | } |
351 | } |
| Line 353... | Line -... | ||
| 353 | - | ||
| 354 | void correctHeadingToMagnetic(void) { |
- | |
| 355 | int32_t error; |
352 | |
| - | 353 | void correctHeadingToMagnetic(void) { |
|
| 356 | 354 | int32_t error; |
|
| - | 355 | ||
| Line 357... | Line 356... | ||
| 357 | debugOut.analog[27] = heading; |
356 | if (commands_isCalibratingCompass()) { |
| 358 | 357 | debugOut.analog[29] = 1; |
|
| 359 | if (commands_isCalibratingCompass()) |
358 | return; |
| 360 | return; |
359 | } |
| Line 361... | Line 360... | ||
| 361 | 360 | ||
| 362 | // Compass is off, skip. |
361 | // Compass is off, skip. |
| - | 362 | // Naaah this is assumed. |
|
| 363 | // Naaah this is assumed. |
363 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
| - | 364 | // return; |
|
| Line 364... | Line 365... | ||
| 364 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
365 | |
| 365 | // return; |
366 | // Compass is invalid, skip. |
| - | 367 | if (magneticHeading < 0) { |
|
| 366 | 368 | debugOut.analog[29] = 2; |
|
| - | 369 | return; |
|
| Line 367... | Line 370... | ||
| 367 | // Compass is invalid, skip. |
370 | } |
| 368 | if (magneticHeading < 0) |
371 | |
| 369 | return; |
372 | // Spinning fast, skip |
| - | 373 | if (abs(yawRate) > 128) { |
|
| 370 | 374 | debugOut.analog[29] = 3; |
|
| 371 | // Spinning fast, skip |
375 | return; |
| Line 372... | Line 376... | ||
| 372 | if (abs(yawRate) > 128) |
376 | } |
| 373 | return; |
377 | |
| - | 378 | // Otherwise invalidated, skip |
|
| - | 379 | if (ignoreCompassTimer) { |
|
| Line 374... | Line 380... | ||
| 374 | 380 | ignoreCompassTimer--; |
|
| 375 | // Otherwise invalidated, skip |
381 | debugOut.analog[29] = 4; |
| 376 | if (ignoreCompassTimer) { |
382 | return; |
| 377 | ignoreCompassTimer--; |
383 | } |
| Line 378... | Line 384... | ||
| 378 | return; |
384 | |
| - | 385 | // TODO: Find computational cost of this. |
|
| Line 379... | Line 386... | ||
| 379 | } |
386 | error = ((int32_t)magneticHeading*GYRO_DEG_FACTOR_YAW - heading); |
| 380 | 387 | if (error <= -YAWOVER180) error += YAWOVER360; |
|
| 381 | // TODO: Find computational cost of this. |
388 | else if (error > YAWOVER180) error -= YAWOVER360; |
| 382 | error = (magneticHeading*GYRO_DEG_FACTOR_YAW - heading) % GYRO_DEG_FACTOR_YAW; |
389 | |
| 383 | 390 | // We only correct errors larger than the resolution of the compass, or else we would keep rounding the |
|
| 384 | // We only correct errors larger than the resolution of the compass, or else we would keep rounding the |
391 | // better resolution of the gyros to the worse resolution of the compass all the time. |
| Line -... | Line 392... | ||
| - | 392 | // The correction should really only serve to compensate for gyro drift. |
|
| - | 393 | if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
|
| 385 | // better resolution of the gyros to the worse resolution of the compass all the time. |
394 | |
| 386 | // The correction should really only serve to compensate for gyro drift. |
395 | int32_t correction = (error * staticParams.compassYawCorrection) >> 8; |
| 387 | if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
- | |
| 388 | 396 | debugOut.analog[30] = correction; |
|
| 389 | int32_t correction = (error * (int32_t)dynamicParams.compassYawEffect) >> 8; |
397 | |
| Line 390... | Line 398... | ||
| 390 | 398 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
|
| 391 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
399 | // and to the target heading (the direction to which it maneuvers to point). That means, this correction has |
| 392 | // and to the target heading (the direction to which it maneuvers to point). That means, this correction has |
400 | // no effect on control at all!!! It only has effect on the values of the two variables. However, these values |