| 201,7 → 201,6 |
|---|
| HightD = 0; |
| Reading_Integral_Top = 0; |
| CompassCourse = CompassHeading; |
| GPS_Neutral(); |
| BeepTime = 50; |
| TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L; |
| TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L; |
| 392,7 → 391,7 |
|---|
| //Start I2C Interrupt Mode |
| twi_state = 0; |
| motor = 0; |
| i2c_start(); |
| I2C_Start(); |
| } |
| |
| |
| 1018,7 → 1017,7 |
|---|
| { |
| updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
| // get current compass heading (angule between MK head and magnetic north) |
| CompassHeading = MM3_heading(); |
| CompassHeading = MM3_Heading(); |
| // calculate OffCourse (angular deviation from heading to course) |
| CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
| |
| 1042,6 → 1041,21 |
|---|
| } |
| } |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // GPS |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE) |
| { |
| GPS_P_Factor = FCParam.UserParam5; |
| GPS_D_Factor = FCParam.UserParam6; |
| GPS_Main(); // updates GPS_Pitch and GPS_Roll on new GPS data |
| } |
| else |
| { |
| GPS_Pitch = 0; |
| GPS_Roll = 0; |
| } |
| |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Debugwerte zuordnen |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(!TimerDebugOut--) |