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