Rev 726 | Rev 733 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 726 | Rev 727 | ||
|---|---|---|---|
| Line 203... | Line 203... | ||
| 203 | CompassCourse = CompassHeading; |
203 | CompassCourse = CompassHeading; |
| 204 | BeepTime = 50; |
204 | BeepTime = 50; |
| 205 | TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
205 | TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
| 206 | TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
206 | TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
| 207 | ExternHightValue = 0; |
207 | ExternHightValue = 0; |
| - | 208 | GPS_Pitch = 0; |
|
| - | 209 | GPS_Roll = 0; |
|
| 208 | } |
210 | } |
| Line 209... | Line 211... | ||
| 209 | 211 | ||
| 210 | /************************************************************************/ |
212 | /************************************************************************/ |
| 211 | /* Averaging Measurement Readings */ |
213 | /* Averaging Measurement Readings */ |
| Line 1035... | Line 1037... | ||
| 1035 | } |
1037 | } |
| 1036 | w = (w * FCParam.CompassYawEffect) / 64; // scale to parameter |
1038 | w = (w * FCParam.CompassYawEffect) / 64; // scale to parameter |
| 1037 | w = FCParam.CompassYawEffect - w; // reduce commpass effect with increasing declination |
1039 | w = FCParam.CompassYawEffect - w; // reduce commpass effect with increasing declination |
| 1038 | if(w > 0) // if there is any compass effect (avoid negative compass feedback) |
1040 | if(w > 0) // if there is any compass effect (avoid negative compass feedback) |
| 1039 | { |
1041 | { |
| 1040 | Reading_IntegralGyroYaw -= (CompassOffCourse * w) / 32; |
1042 | Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32; |
| 1041 | } |
1043 | } |
| 1042 | } |
1044 | } |
| 1043 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1045 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1044 | // GPS |
1046 | // GPS |
| 1045 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1047 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 1121... | Line 1123... | ||
| 1121 | if(Reading_GyroYaw > MAX_SENSOR) Reading_GyroYaw = MAX_SENSOR; |
1123 | if(Reading_GyroYaw > MAX_SENSOR) Reading_GyroYaw = MAX_SENSOR; |
| 1122 | if(Reading_GyroYaw < -MAX_SENSOR) Reading_GyroYaw = -MAX_SENSOR; |
1124 | if(Reading_GyroYaw < -MAX_SENSOR) Reading_GyroYaw = -MAX_SENSOR; |
| Line 1123... | Line 1125... | ||
| 1123 | 1125 | ||
| 1124 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1126 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1125 | // Hight Control |
1127 | // Hight Control |
| 1126 | // The higth control algorithm reduces the thrust but does not increas the thrust. |
1128 | // The higth control algorithm reduces the thrust but does not increase the thrust. |
| 1127 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1129 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1128 | // If hight control is activated and no emergency landing is activre |
1130 | // If hight control is activated and no emergency landing is activre |
| 1129 | if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) ) |
1131 | if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) ) |
| 1130 | { |
1132 | { |