Rev 826 | Rev 898 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 826 | Rev 844 | ||
|---|---|---|---|
| Line 1023... | Line 1023... | ||
| 1023 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1023 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1024 | // compass code is used if Compass option or GPS option is selected |
1024 | // compass code is used if Compass option or GPS option is selected |
| 1025 | if((ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE) || (ParamSet.GlobalConfig & CFG_GPS_ACTIVE)) |
1025 | if((ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE) || (ParamSet.GlobalConfig & CFG_GPS_ACTIVE)) |
| 1026 | { |
1026 | { |
| 1027 | static uint8_t updCompass = 0; |
1027 | static uint8_t updCompass = 0; |
| - | 1028 | int16_t w,v; |
|
| Line 1028... | Line 1029... | ||
| 1028 | 1029 | ||
| 1029 | if (!updCompass--) |
1030 | if (!updCompass--) |
| 1030 | { |
1031 | { |
| 1031 | updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
1032 | updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
| 1032 | // get current compass heading (angule between MK head and magnetic north) |
1033 | // get current compass heading (angle between MK head and magnetic north) |
| 1033 | #ifdef USE_MM3 |
1034 | #ifdef USE_MM3 |
| 1034 | CompassHeading = MM3_Heading(); |
1035 | CompassHeading = MM3_Heading(); |
| 1035 | #endif |
1036 | #endif |
| 1036 | #ifdef USE_CMPS03 |
1037 | #ifdef USE_CMPS03 |
| 1037 | CompassHeading = CMPS03_Heading(); |
1038 | CompassHeading = CMPS03_Heading(); |
| 1038 | #endif |
1039 | #endif |
| 1039 | if (CompassHeading < 0) CompassOffCourse = 0; // disable gyro compass correction on bad compass data |
1040 | if (CompassHeading < 0) CompassOffCourse = 0; // disable gyro compass correction on bad compass data |
| 1040 | else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; // calc course deviation |
1041 | else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; // calc course deviation |
| Line -... | Line 1042... | ||
| - | 1042 | } |
|
| - | 1043 | ||
| 1041 | } |
1044 | // get maximum attitude angle |
| - | 1045 | w = abs(IntegralPitch/512); |
|
| - | 1046 | v = abs(IntegralRoll /512); |
|
| 1042 | 1047 | if(v > w) w = v; |
|
| 1043 | if ((MaxStickPitch < 75) && (MaxStickRoll < 75)) |
1048 | if (w < 25) |
| 1044 | { |
1049 | { |
| 1045 | if(UpdateCompassCourse) |
1050 | if(UpdateCompassCourse) |
| 1046 | { |
1051 | { |
| 1047 | UpdateCompassCourse = 0; |
1052 | UpdateCompassCourse = 0; |
| 1048 | CompassCourse = CompassHeading; |
1053 | CompassCourse = CompassHeading; |
| - | 1054 | CompassOffCourse = 0; |
|
| - | 1055 | } |
|
| 1049 | CompassOffCourse = 0; |
1056 | w = (w * FCParam.CompassYawEffect) / 64; |
| 1050 | } |
1057 | w = FCParam.CompassYawEffect - w; |
| 1051 | Reading_IntegralGyroYaw += (CompassOffCourse * FCParam.CompassYawEffect) / 16; |
1058 | if(w > 0) Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32; |
| 1052 | } |
1059 | } |
| 1053 | } |
1060 | } |
| 1054 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1061 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 1058... | Line 1065... | ||
| 1058 | { |
1065 | { |
| 1059 | GPS_I_Factor = FCParam.UserParam2; |
1066 | GPS_I_Factor = FCParam.UserParam2; |
| 1060 | GPS_P_Factor = FCParam.UserParam5; |
1067 | GPS_P_Factor = FCParam.UserParam5; |
| 1061 | GPS_D_Factor = FCParam.UserParam6; |
1068 | GPS_D_Factor = FCParam.UserParam6; |
| 1062 | if(EmergencyLanding) GPS_Main(230); // enables Comming Home |
1069 | if(EmergencyLanding) GPS_Main(230); // enables Comming Home |
| 1063 | else GPS_Main(Poti3); |
1070 | else GPS_Main(Poti3); // behavior controlled by Poti3 |
| 1064 | } |
1071 | } |
| 1065 | else |
1072 | else |
| 1066 | { |
1073 | { |
| 1067 | GPS_Neutral(); |
1074 | GPS_Neutral(); |
| 1068 | } |
1075 | } |