| 1025,11 → 1025,12 |
|---|
| if((ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE) || (ParamSet.GlobalConfig & CFG_GPS_ACTIVE)) |
| { |
| static uint8_t updCompass = 0; |
| int16_t w,v; |
| |
| if (!updCompass--) |
| { |
| updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
| // get current compass heading (angule between MK head and magnetic north) |
| // get current compass heading (angle between MK head and magnetic north) |
| #ifdef USE_MM3 |
| CompassHeading = MM3_Heading(); |
| #endif |
| 1040,7 → 1041,11 |
|---|
| else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; // calc course deviation |
| } |
| |
| if ((MaxStickPitch < 75) && (MaxStickRoll < 75)) |
| // get maximum attitude angle |
| w = abs(IntegralPitch/512); |
| v = abs(IntegralRoll /512); |
| if(v > w) w = v; |
| if (w < 25) |
| { |
| if(UpdateCompassCourse) |
| { |
| 1048,7 → 1053,9 |
|---|
| CompassCourse = CompassHeading; |
| CompassOffCourse = 0; |
| } |
| Reading_IntegralGyroYaw += (CompassOffCourse * FCParam.CompassYawEffect) / 16; |
| w = (w * FCParam.CompassYawEffect) / 64; |
| w = FCParam.CompassYawEffect - w; |
| if(w > 0) Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32; |
| } |
| } |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1060,7 → 1067,7 |
|---|
| GPS_P_Factor = FCParam.UserParam5; |
| GPS_D_Factor = FCParam.UserParam6; |
| if(EmergencyLanding) GPS_Main(230); // enables Comming Home |
| else GPS_Main(Poti3); |
| else GPS_Main(Poti3); // behavior controlled by Poti3 |
| } |
| else |
| { |