Rev 2035 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2035 | Rev 2041 | ||
|---|---|---|---|
| Line 107... | Line 107... | ||
| 107 | int readingHeight = 0; |
107 | int readingHeight = 0; |
| Line 108... | Line 108... | ||
| 108 | 108 | ||
| Line 109... | Line 109... | ||
| 109 | // Yaw angle and compass stuff. |
109 | // Yaw angle and compass stuff. |
| 110 | 110 | ||
| Line 111... | Line 111... | ||
| 111 | // This is updated/written from MM3. Negative angle indicates invalid data. |
111 | // This is updated/written from MM3. Negative angle indicates invalid data. |
| 112 | int16_t compassHeading = -1; |
112 | int16_t magneticHeading = -1; |
| Line 113... | Line 113... | ||
| 113 | 113 | ||
| Line 185... | Line 185... | ||
| 185 | // reset gyro integrals to acc guessing |
185 | // reset gyro integrals to acc guessing |
| 186 | setStaticAttitudeAngles(); |
186 | setStaticAttitudeAngles(); |
| 187 | yawAngleDiff = 0; |
187 | yawAngleDiff = 0; |
| Line 188... | Line 188... | ||
| 188 | 188 | ||
| 189 | // update compass course to current heading |
189 | // update compass course to current heading |
| Line 190... | Line 190... | ||
| 190 | compassCourse = compassHeading; |
190 | compassCourse = magneticHeading; |
| 191 | 191 | ||
| Line 192... | Line 192... | ||
| 192 | // Inititialize YawGyroIntegral value with current compass heading |
192 | // Inititialize YawGyroIntegral value with current compass heading |
| 193 | yawGyroHeading = (int32_t) compassHeading * GYRO_DEG_FACTOR_YAW; |
193 | yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
| Line 194... | Line 194... | ||
| 194 | 194 | ||
| Line 433... | Line 433... | ||
| 433 | v = abs(angle[ROLL] / 512); |
433 | v = abs(angle[ROLL] / 512); |
| 434 | if (v > w) |
434 | if (v > w) |
| 435 | w = v; |
435 | w = v; |
| 436 | correction = w / 8 + 1; |
436 | correction = w / 8 + 1; |
| 437 | // calculate the deviation of the yaw gyro heading and the compass heading |
437 | // calculate the deviation of the yaw gyro heading and the compass heading |
| 438 | if (compassHeading < 0) |
438 | if (magneticHeading < 0) |
| 439 | error = 0; // disable yaw drift compensation if compass heading is undefined |
439 | error = 0; // disable yaw drift compensation if compass heading is undefined |
| 440 | else if (abs(yawRate) > 128) { // spinning fast |
440 | else if (abs(yawRate) > 128) { // spinning fast |
| 441 | error = 0; |
441 | error = 0; |
| 442 | } else { |
442 | } else { |
| 443 | // compassHeading - yawGyroHeading, on a -180..179 deg interval. |
443 | // compassHeading - yawGyroHeading, on a -180..179 deg interval. |
| 444 | error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) |
444 | error = ((540 + magneticHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) |
| 445 | % 360) - 180; |
445 | % 360) - 180; |
| 446 | } |
446 | } |
| 447 | if (!ignoreCompassTimer && w < 25) { |
447 | if (!ignoreCompassTimer && w < 25) { |
| 448 | yawGyroDrift += error; |
448 | yawGyroDrift += error; |
| 449 | // Basically this gets set if we are in "fix" mode, and when starting. |
449 | // Basically this gets set if we are in "fix" mode, and when starting. |
| 450 | if (updateCompassCourse) { |
450 | if (updateCompassCourse) { |
| 451 | beep(200); |
451 | beep(200); |
| 452 | yawGyroHeading = (int32_t) compassHeading * GYRO_DEG_FACTOR_YAW; |
452 | yawGyroHeading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
| 453 | compassCourse = compassHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
453 | compassCourse = magneticHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
| 454 | updateCompassCourse = 0; |
454 | updateCompassCourse = 0; |
| 455 | } |
455 | } |
| 456 | } |
456 | } |
| 457 | yawGyroHeading += (error * 8) / correction; |
457 | yawGyroHeading += (error * 8) / correction; |