1052,7 → 1052,7 |
|
if (CompassHeading < 0) // no compass data available |
{ |
CompassOffCourse = 0; |
CompassOffCourse = 0; // disables gyro compass correction |
} |
else // calculate OffCourse (angular deviation from heading to course) |
CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
1062,13 → 1062,13 |
w = abs(IntegralPitch / 512); |
v = abs(IntegralRoll / 512); |
if(v > w) w = v; // get maximum declination |
// if declination is small enough update compass course if neccessary |
if((w < 35) && StoreNewCompassCourse && (CompassHeading>=0) ) |
// if declination is small enough then update compass course if neccessary |
if((w < 35) && StoreNewCompassCourse && (CompassHeading>=0) ) // 35 corresponds to a declination of ~14 deg |
{ |
CompassCourse = CompassHeading; |
StoreNewCompassCourse = 0; |
} |
w = (w * FCParam.CompassYawEffect) / 64; // scale to parameter |
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) |
{ |