1009,8 → 1009,8 |
// Yawing |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(abs(StickYaw) > 20 ) // yaw stick is activated |
{ // if not fixed compass course is set update compass course for 1s |
if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) UpdateCompassCourse = 500; |
{ |
if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) UpdateCompassCourse = 1; |
} |
// exponential stick sensitivity in yawring rate |
tmp_int = (int32_t) ParamSet.Yaw_P * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx² |
1026,7 → 1026,6 |
// compass code is used if Compass option or GPS option is selected |
if((ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE) || (ParamSet.GlobalConfig & CFG_GPS_ACTIVE)) |
{ |
int16_t w,v; |
static uint8_t updCompass = 0; |
|
if (!updCompass--) |
1039,30 → 1038,20 |
#ifdef USE_CMPS03 |
CompassHeading = CMPS03_Heading(); |
#endif |
if (CompassHeading < 0) // no compass data available |
if (CompassHeading < 0) CompassOffCourse = 0; // disable gyro compass correction on bad compass data |
else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; // calc course deviation |
} |
|
if ((MaxStickPitch < 75) && (MaxStickRoll < 75)) |
{ |
if(UpdateCompassCourse) |
{ |
CompassOffCourse = 0; // disables gyro compass correction |
UpdateCompassCourse = 0; |
CompassCourse = CompassHeading; |
CompassOffCourse = 0; |
} |
else // calculate OffCourse (angular deviation from heading to course) |
CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
Reading_IntegralGyroYaw += (CompassOffCourse * FCParam.CompassYawEffect) / 16; |
} |
// reduce compass effect with increasing declination |
w = abs(IntegralPitch / 512); |
v = abs(IntegralRoll / 512); |
if(v > w) w = v; // get maximum declination |
// if declination is small enough then update compass course if neccessary |
if((w < 70) && UpdateCompassCourse && (CompassHeading>=0) ) // 70 corresponds to a declination of ~28 deg |
{ |
CompassCourse = CompassHeading; |
CompassOffCourse = 0; // should be set immediatly because the CompassOffCourse is otherwise only refreshed if new compass heading is calulated |
} |
if(UpdateCompassCourse) UpdateCompassCourse--; |
w = (w * FCParam.CompassYawEffect) / 64; // (w=0 for 64->~25 deg, 128->~50 deg) for higher declinaions the compass drift compensation is disabled |
w = FCParam.CompassYawEffect - w; // reduce compass effect with increasing declination |
if(w > 0) // if there is any compass effect (avoid negative compass feedback) |
{ |
Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32; |
} |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// GPS |