Rev 792 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 792 | Rev 793 | ||
|---|---|---|---|
| Line 1007... | Line 1007... | ||
| 1007 | 1007 | ||
| 1008 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1008 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1009 | // Yawing |
1009 | // Yawing |
| 1010 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1010 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1011 | if(abs(StickYaw) > 20 ) // yaw stick is activated |
- | |
| - | 1011 | if(abs(StickYaw) > 20 ) // yaw stick is activated |
|
| 1012 | { // if not fixed compass course is set update compass course for 1s |
1012 | { |
| 1013 | if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) UpdateCompassCourse = 500; |
1013 | if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) UpdateCompassCourse = 1; |
| 1014 | } |
1014 | } |
| 1015 | // exponential stick sensitivity in yawring rate |
1015 | // exponential stick sensitivity in yawring rate |
| 1016 | tmp_int = (int32_t) ParamSet.Yaw_P * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx² |
1016 | tmp_int = (int32_t) ParamSet.Yaw_P * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx² |
| 1017 | tmp_int += (ParamSet.Yaw_P * StickYaw) / 4; |
1017 | tmp_int += (ParamSet.Yaw_P * StickYaw) / 4; |
| Line 1024... | Line 1024... | ||
| 1024 | // Compass |
1024 | // Compass |
| 1025 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1025 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1026 | // compass code is used if Compass option or GPS option is selected |
1026 | // compass code is used if Compass option or GPS option is selected |
| 1027 | if((ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE) || (ParamSet.GlobalConfig & CFG_GPS_ACTIVE)) |
1027 | if((ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE) || (ParamSet.GlobalConfig & CFG_GPS_ACTIVE)) |
| 1028 | { |
1028 | { |
| 1029 | int16_t w,v; |
- | |
| 1030 | static uint8_t updCompass = 0; |
1029 | static uint8_t updCompass = 0; |
| Line 1031... | Line 1030... | ||
| 1031 | 1030 | ||
| 1032 | if (!updCompass--) |
1031 | if (!updCompass--) |
| 1033 | { |
1032 | { |
| Line 1037... | Line 1036... | ||
| 1037 | CompassHeading = MM3_Heading(); |
1036 | CompassHeading = MM3_Heading(); |
| 1038 | #endif |
1037 | #endif |
| 1039 | #ifdef USE_CMPS03 |
1038 | #ifdef USE_CMPS03 |
| 1040 | CompassHeading = CMPS03_Heading(); |
1039 | CompassHeading = CMPS03_Heading(); |
| 1041 | #endif |
1040 | #endif |
| 1042 | if (CompassHeading < 0) // no compass data available |
- | |
| 1043 | { |
- | |
| 1044 | CompassOffCourse = 0; // disables gyro compass correction |
1041 | if (CompassHeading < 0) CompassOffCourse = 0; // disable gyro compass correction on bad compass data |
| 1045 | } |
- | |
| 1046 | else // calculate OffCourse (angular deviation from heading to course) |
- | |
| 1047 | CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
1042 | else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; // calc course deviation |
| 1048 | } |
- | |
| 1049 | // reduce compass effect with increasing declination |
- | |
| 1050 | w = abs(IntegralPitch / 512); |
- | |
| 1051 | v = abs(IntegralRoll / 512); |
- | |
| 1052 | if(v > w) w = v; // get maximum declination |
- | |
| 1053 | // if declination is small enough then update compass course if neccessary |
- | |
| 1054 | if((w < 70) && UpdateCompassCourse && (CompassHeading>=0) ) // 70 corresponds to a declination of ~28 deg |
- | |
| 1055 | { |
- | |
| 1056 | CompassCourse = CompassHeading; |
- | |
| 1057 | CompassOffCourse = 0; // should be set immediatly because the CompassOffCourse is otherwise only refreshed if new compass heading is calulated |
- | |
| 1058 | } |
1043 | } |
| - | 1044 | ||
| 1059 | if(UpdateCompassCourse) UpdateCompassCourse--; |
1045 | if ((MaxStickPitch < 75) && (MaxStickRoll < 75)) |
| 1060 | w = (w * FCParam.CompassYawEffect) / 64; // (w=0 for 64->~25 deg, 128->~50 deg) for higher declinaions the compass drift compensation is disabled |
- | |
| 1061 | w = FCParam.CompassYawEffect - w; // reduce compass effect with increasing declination |
- | |
| 1062 | if(w > 0) // if there is any compass effect (avoid negative compass feedback) |
- | |
| 1063 | { |
1046 | { |
| - | 1047 | if(UpdateCompassCourse) |
|
| - | 1048 | { |
|
| - | 1049 | UpdateCompassCourse = 0; |
|
| - | 1050 | CompassCourse = CompassHeading; |
|
| - | 1051 | CompassOffCourse = 0; |
|
| - | 1052 | } |
|
| 1064 | Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32; |
1053 | Reading_IntegralGyroYaw += (CompassOffCourse * FCParam.CompassYawEffect) / 16; |
| 1065 | } |
1054 | } |
| 1066 | } |
1055 | } |
| 1067 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1056 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1068 | // GPS |
1057 | // GPS |
| 1069 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1058 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |