Rev 764 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 764 | Rev 765 | ||
|---|---|---|---|
| Line 1051... | Line 1051... | ||
| 1051 | #endif |
1051 | #endif |
| Line 1052... | Line 1052... | ||
| 1052 | 1052 | ||
| 1053 | if (CompassHeading < 0) // no compass data available |
1053 | if (CompassHeading < 0) // no compass data available |
| 1054 | { |
1054 | { |
| 1055 | CompassOffCourse = 0; |
- | |
| 1056 | if(!BeepTime) BeepTime = 100; // make noise at 10 Hz to signal the compass problem |
1055 | CompassOffCourse = 0; |
| 1057 | } |
1056 | } |
| 1058 | else // calculate OffCourse (angular deviation from heading to course) |
1057 | else // calculate OffCourse (angular deviation from heading to course) |
| 1059 | CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
1058 | CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
| Line 1060... | Line 1059... | ||
| 1060 | } |
1059 | } |
| 1061 | 1060 | ||
| 1062 | // reduce compass effect with increasing declination |
1061 | // reduce compass effect with increasing declination |
| 1063 | w = abs(IntegralPitch / 512); |
1062 | w = abs(IntegralPitch / 512); |
| 1064 | v = abs(IntegralRoll / 512); |
1063 | v = abs(IntegralRoll / 512); |
| 1065 | if(v > w) w = v; // get maximum declination |
1064 | if(v > w) w = v; // get maximum declination |
| 1066 | // if declination is small enough update compass course if neccessary |
1065 | // if declination is small enough update compass course if neccessary |
| 1067 | if(w < 35 && StoreNewCompassCourse) |
1066 | if((w < 35) && StoreNewCompassCourse && (CompassHeading>=0) ) |
| 1068 | { |
1067 | { |
| 1069 | CompassCourse = CompassHeading; |
1068 | CompassCourse = CompassHeading; |
| 1070 | StoreNewCompassCourse = 0; |
1069 | StoreNewCompassCourse = 0; |