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