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 | { |