Rev 765 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 765 | Rev 770 | ||
|---|---|---|---|
| Line 1050... | Line 1050... | ||
| 1050 | CompassHeading = CMPS03_Heading(); |
1050 | CompassHeading = CMPS03_Heading(); |
| 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; |
1055 | CompassOffCourse = 0; // disables gyro compass correction |
| 1056 | } |
1056 | } |
| 1057 | else // calculate OffCourse (angular deviation from heading to course) |
1057 | else // calculate OffCourse (angular deviation from heading to course) |
| 1058 | CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
1058 | CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
| Line 1059... | Line 1059... | ||
| 1059 | } |
1059 | } |
| 1060 | 1060 | ||
| 1061 | // reduce compass effect with increasing declination |
1061 | // reduce compass effect with increasing declination |
| 1062 | w = abs(IntegralPitch / 512); |
1062 | w = abs(IntegralPitch / 512); |
| 1063 | v = abs(IntegralRoll / 512); |
1063 | v = abs(IntegralRoll / 512); |
| 1064 | if(v > w) w = v; // get maximum declination |
1064 | if(v > w) w = v; // get maximum declination |
| 1065 | // if declination is small enough update compass course if neccessary |
1065 | // if declination is small enough then update compass course if neccessary |
| 1066 | if((w < 35) && StoreNewCompassCourse && (CompassHeading>=0) ) |
1066 | if((w < 35) && StoreNewCompassCourse && (CompassHeading>=0) ) // 35 corresponds to a declination of ~14 deg |
| 1067 | { |
1067 | { |
| 1068 | CompassCourse = CompassHeading; |
1068 | CompassCourse = CompassHeading; |
| 1069 | StoreNewCompassCourse = 0; |
1069 | StoreNewCompassCourse = 0; |
| 1070 | } |
1070 | } |
| 1071 | w = (w * FCParam.CompassYawEffect) / 64; // scale to parameter |
1071 | w = (w * FCParam.CompassYawEffect) / 64; // (w=0 for 64->~25 deg, 128->~50 deg) for higher declinaions the compass drift compensation is disabled |
| 1072 | w = FCParam.CompassYawEffect - w; // reduce compass effect with increasing declination |
1072 | w = FCParam.CompassYawEffect - w; // reduce compass effect with increasing declination |
| 1073 | if(w > 0) // if there is any compass effect (avoid negative compass feedback) |
1073 | if(w > 0) // if there is any compass effect (avoid negative compass feedback) |
| 1074 | { |
1074 | { |