Rev 712 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 712 | Rev 726 | ||
|---|---|---|---|
| Line 199... | Line 199... | ||
| 199 | Reading_GyroYaw = 0; |
199 | Reading_GyroYaw = 0; |
| 200 | StartAirPressure = AirPressure; |
200 | StartAirPressure = AirPressure; |
| 201 | HightD = 0; |
201 | HightD = 0; |
| 202 | Reading_Integral_Top = 0; |
202 | Reading_Integral_Top = 0; |
| 203 | CompassCourse = CompassHeading; |
203 | CompassCourse = CompassHeading; |
| 204 | GPS_Neutral(); |
- | |
| 205 | BeepTime = 50; |
204 | BeepTime = 50; |
| 206 | TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
205 | TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
| 207 | TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
206 | TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
| 208 | ExternHightValue = 0; |
207 | ExternHightValue = 0; |
| 209 | } |
208 | } |
| Line 390... | Line 389... | ||
| 390 | DebugOut.Analog[15] = Motor_Right; |
389 | DebugOut.Analog[15] = Motor_Right; |
| Line 391... | Line 390... | ||
| 391 | 390 | ||
| 392 | //Start I2C Interrupt Mode |
391 | //Start I2C Interrupt Mode |
| 393 | twi_state = 0; |
392 | twi_state = 0; |
| 394 | motor = 0; |
393 | motor = 0; |
| 395 | i2c_start(); |
394 | I2C_Start(); |
| Line 396... | Line 395... | ||
| 396 | } |
395 | } |
| Line 1016... | Line 1015... | ||
| 1016 | 1015 | ||
| 1017 | if (!updCompass--) |
1016 | if (!updCompass--) |
| 1018 | { |
1017 | { |
| 1019 | updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
1018 | updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
| 1020 | // get current compass heading (angule between MK head and magnetic north) |
1019 | // get current compass heading (angule between MK head and magnetic north) |
| 1021 | CompassHeading = MM3_heading(); |
1020 | CompassHeading = MM3_Heading(); |
| 1022 | // calculate OffCourse (angular deviation from heading to course) |
1021 | // calculate OffCourse (angular deviation from heading to course) |
| Line 1023... | Line 1022... | ||
| 1023 | CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
1022 | CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
| Line 1040... | Line 1039... | ||
| 1040 | { |
1039 | { |
| 1041 | Reading_IntegralGyroYaw -= (CompassOffCourse * w) / 32; |
1040 | Reading_IntegralGyroYaw -= (CompassOffCourse * w) / 32; |
| 1042 | } |
1041 | } |
| 1043 | } |
1042 | } |
| 1044 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1043 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| - | 1044 | // GPS |
|
| - | 1045 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 1046 | if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE) |
|
| - | 1047 | { |
|
| - | 1048 | GPS_P_Factor = FCParam.UserParam5; |
|
| - | 1049 | GPS_D_Factor = FCParam.UserParam6; |
|
| - | 1050 | GPS_Main(); // updates GPS_Pitch and GPS_Roll on new GPS data |
|
| - | 1051 | } |
|
| - | 1052 | else |
|
| - | 1053 | { |
|
| - | 1054 | GPS_Pitch = 0; |
|
| - | 1055 | GPS_Roll = 0; |
|
| - | 1056 | } |
|
| - | 1057 | ||
| - | 1058 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| 1045 | // Debugwerte zuordnen |
1059 | // Debugwerte zuordnen |
| 1046 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1060 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1047 | if(!TimerDebugOut--) |
1061 | if(!TimerDebugOut--) |
| 1048 | { |
1062 | { |
| 1049 | TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz) |
1063 | TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz) |