| 122,8 → 122,12 |
|---|
| int16_t CompassCourse = -1; |
| int16_t CompassOffCourse = 0; |
| uint8_t CompassCalState = 0; |
| int8_t CalculateCompassTimer = 100; |
| uint8_t CompassFusion = 32; |
| int16_t CompassYawSetPoint = 0; |
| |
| uint16_t BadCompassHeading = 50; |
| uint8_t FunnelCourse = 0; |
| uint16_t BadCompassHeading = 500; |
| int32_t YawGyroHeading; // Yaw Gyro Integral supported by compass |
| int16_t YawGyroDrift; |
| |
| 206,6 → 210,7 |
|---|
| DebugOut.Analog[20] = ServoNickValue; |
| DebugOut.Analog[22] = Capacity.ActualCurrent; |
| DebugOut.Analog[23] = Capacity.UsedCapacity; |
| DebugOut.Analog[27] = CompassCourse; |
| DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
| DebugOut.Analog[30] = GPSStickNick; |
| DebugOut.Analog[31] = GPSStickRoll; |
| 348,6 → 353,7 |
|---|
| |
| // update compass course to current heading |
| CompassCourse = CompassHeading; |
| BadCompassHeading = 100; |
| // Inititialize YawGyroIntegral value with current compass heading |
| YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR; |
| YawGyroDrift = 0; |
| 630,6 → 636,8 |
|---|
| /************************************************************************/ |
| void ParameterMapping(void) |
| { |
| static uint8_t CareFreeOld = 2; |
| |
| if(RC_Quality > 160) // do the mapping of RC-Potis only if the rc-signal is ok |
| // else the last updated values are used |
| { |
| 697,10 → 705,17 |
|---|
| if(!CareFree) ControlHeading = (((int16_t) ParamSet.OrientationAngle * 15 + CompassHeading) % 360) / 2; |
| #endif |
| CareFree = 1; |
| if(CareFreeOld == 0 && CareFree) BeepTime = 1000; |
| if(CareFreeOld == 1 && !CareFree) BeepTime = 200; |
| CareFreeOld = CareFree; |
| if(FromNaviCtrl.CompassHeading < 0) UART_VersionInfo.HardwareError[0] |= FC_ERROR0_CAREFREE; |
| else UART_VersionInfo.HardwareError[0] &= ~FC_ERROR0_CAREFREE; |
| } |
| else CareFree = 0; |
| else |
| { |
| CareFree = 0; |
| CareFreeOld = 0; |
| } |
| |
| if((FromNaviCtrl.CompassHeading < 0) && (BeepModulation == 0xFFFF) && CareFree && (FC_StatusFlags & FC_STATUS_MOTOR_RUN)) |
| { |
| 811,10 → 826,6 |
|---|
| StickYaw = 0; |
| ReadingIntegralGyroYaw = 0; |
| SetPointYaw = 0; |
| if(ModelIsFlying == 250) |
| { |
| UpdateCompassCourse = 1; |
| } |
| } |
| else FC_StatusFlags |= FC_STATUS_FLY; // set fly flag |
| |
| 985,6 → 996,7 |
|---|
| IPartNick = 0; |
| IPartRoll = 0; |
| ControlHeading = (((int16_t)ParamSet.OrientationAngle * 15 + CompassHeading) % 360) / 2; |
| UpdateCompassCourse = 100; // 2 seconds |
| } |
| else |
| { |
| 1065,7 → 1077,7 |
|---|
| |
| // mapping of yaw |
| StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
| #define YAW_DEAD_RANGE 2 |
| #define YAW_DEAD_RANGE 4 |
| // (range of -YAW_DEAD_RANGE .. YAW_DEAD_RANGE is set to zero, to avoid unwanted yaw trimming on compass correction) |
| if(ParamSet.Config0 & (CFG0_COMPASS_ACTIVE|CFG0_GPS_ACTIVE)) |
| { |
| 1216,7 → 1228,7 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(! LoopingNick && !LoopingRoll && ( (AdValueAccZ > 512) || (FC_StatusFlags & FC_STATUS_MOTOR_RUN) ) ) // if not lopping in any direction |
| { |
| if( FCParam.KalmanK != -1) |
| if( FCParam.KalmanK > 0) |
| { |
| // determine the deviation of gyro integral from averaged acceleration sensor |
| tmp_long1 = (int32_t)(IntegralGyroNick / ParamSet.GyroAccFactor - (int32_t)AccNick); |
| 1223,7 → 1235,7 |
|---|
| tmp_long1 = (tmp_long1 * FCParam.KalmanK) / (32 * 16); |
| tmp_long2 = (int32_t)(IntegralGyroRoll / ParamSet.GyroAccFactor - (int32_t)AccRoll); |
| tmp_long2 = (tmp_long2 * FCParam.KalmanK) / (32 * 16); |
| |
| CompassFusion = FCParam.KalmanK; |
| if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands |
| { |
| tmp_long1 /= 2; |
| 1256,7 → 1268,7 |
|---|
| tmp_long1 /= 3; |
| tmp_long2 /= 3; |
| } |
| |
| CompassFusion = 25; |
| #define BALANCE 32 |
| // limit correction effect |
| LIMIT_MIN_MAX(tmp_long1, -BALANCE, BALANCE); |
| 1357,7 → 1369,7 |
|---|
| else |
| { |
| cnt = 0; |
| BadCompassHeading = 1000; |
| BadCompassHeading = 100; |
| } |
| if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp; |
| if(FCParam.KalmanMaxDrift) if(cnt > FCParam.KalmanMaxDrift) cnt = FCParam.KalmanMaxDrift; |
| 1398,7 → 1410,7 |
|---|
| else |
| { |
| cnt = 0; |
| BadCompassHeading = 1000; |
| BadCompassHeading = 100; |
| } |
| // correct Gyro Offsets |
| if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp; |
| 1436,17 → 1448,18 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Yawing |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(abs(StickYaw) > 15 ) // yaw stick is activated |
| if(abs(StickYaw) > 3 ) // yaw stick is activated |
| { |
| BadCompassHeading = 1000; |
| //BadCompassHeading = 1000; |
| if(!(ParamSet.Config0 & CFG0_COMPASS_FIX)) |
| { |
| UpdateCompassCourse = 1; |
| UpdateCompassCourse = 50; // one second for login |
| } |
| } |
| // exponential stick sensitivity in yawring rate |
| tmp_int1 = (int32_t) ParamSet.StickYawP * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo y = ax + bx² |
| tmp_int1 += (ParamSet.StickYawP * StickYaw) / 4; |
| tmp_int1 += CompassYawSetPoint; |
| SetPointYaw = tmp_int1; |
| // trimm drift of ReadingIntegralGyroYaw with SetPointYaw(StickYaw) |
| ReadingIntegralGyroYaw -= tmp_int1; |
| 1456,66 → 1469,87 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // Compass |
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // compass code is used if Compass option is selected |
| if(ParamSet.Config0 & (CFG0_COMPASS_ACTIVE|CFG0_GPS_ACTIVE)) |
| // compass code is used only if compass option is selected |
| if( (CompassHeading >= 0) && (ParamSet.Config0 & (CFG0_COMPASS_ACTIVE|CFG0_GPS_ACTIVE))) |
| { |
| int16_t w, v, r,correction, error; |
| |
| if(CompassCalState && !(FC_StatusFlags & FC_STATUS_MOTOR_RUN) ) |
| if(CalculateCompassTimer-- == 1) |
| { |
| SetCompassCalState(); |
| } |
| else |
| { |
| // get maximum attitude angle |
| w = abs(IntegralGyroNick / 512); |
| v = abs(IntegralGyroRoll / 512); |
| if(v > w) w = v; |
| correction = w / 8 + 2; |
| // calculate the deviation of the yaw gyro heading and the compass heading |
| if (CompassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined |
| int16_t w, v, r,correction, error; |
| CalculateCompassTimer = 13; // in case of no Navi-Data |
| if(CompassCalState && !(FC_StatusFlags & FC_STATUS_MOTOR_RUN) ) |
| { |
| SetCompassCalState(); |
| } |
| else |
| { |
| // guessing max correction value |
| // get maximum attitude angle |
| w = abs(IntegralGyroNick / 512); |
| v = abs(IntegralGyroRoll / 512); |
| if(v > w) w = v; |
| correction = w / 4 + 1; |
| // calculate the deviation of the yaw gyro heading and the compass heading |
| error = ((540 + CompassHeading - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180; |
| //error += GyroYaw / 12; |
| } |
| if(!BadCompassHeading && w < 25) |
| { |
| YawGyroDrift += error; |
| if(UpdateCompassCourse) |
| // log in compass value |
| if(BadCompassHeading) BadCompassHeading--; |
| else if(w < 25) |
| { |
| //BeepTime = 200; |
| YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR; |
| CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR); |
| UpdateCompassCourse = 0; |
| YawGyroDrift += error; |
| if(UpdateCompassCourse) |
| { |
| if(--UpdateCompassCourse == 0) |
| { |
| YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR; |
| CompassCourse = (int16_t)(YawGyroHeading / GYRO_DEG_FACTOR); |
| } |
| } |
| } |
| } |
| YawGyroHeading += (error * 16) / correction; |
| w = (w * FCParam.CompassYawEffect) / 32; |
| w = FCParam.CompassYawEffect - w; |
| if(w >= 0) |
| { |
| if(!BadCompassHeading) |
| // data fusion |
| if(!BadCompassHeading) YawGyroHeading += (error * CompassFusion) / correction; |
| // yawing MK |
| if(!UpdateCompassCourse) |
| { |
| v = 64 + (MaxStickNick + MaxStickRoll) / 8; |
| // calc course deviation |
| r = ((540 + (YawGyroHeading / GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180; |
| v = (r * w) / v; // align to compass course |
| // limit yaw rate |
| w = 3 * FCParam.CompassYawEffect; |
| if (v > w) v = w; |
| else if (v < -w) v = -w; |
| ReadingIntegralGyroYaw += v; |
| DebugOut.Analog[19] = r; |
| v = r * (FCParam.CompassYawEffect/2); |
| CompassYawSetPoint = v / 16; |
| } |
| else CompassYawSetPoint = 0; |
| DebugOut.Analog[18] = CompassFusion; |
| DebugOut.Analog[25] = CompassYawSetPoint; |
| |
| /* |
| w = (w * FCParam.CompassYawEffect) / 32; |
| w = FCParam.CompassYawEffect - w; |
| if(w >= 0) |
| { |
| if(!BadCompassHeading) |
| { |
| v = 64 + (MaxStickNick + MaxStickRoll) / 8; |
| // calc course deviation |
| r = ((540 + (YawGyroHeading / GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180; |
| v = (r * w) / v; // align to compass course |
| // limit yaw rate |
| w = 3 * FCParam.CompassYawEffect; |
| if (v > w) v = w; |
| else if (v < -w) v = -w; |
| ReadingIntegralGyroYaw += v; |
| } |
| else |
| { // wait a while |
| BadCompassHeading--; |
| } |
| } |
| else |
| { // wait a while |
| BadCompassHeading--; |
| { // ignore compass at extreme attitudes for a while |
| BadCompassHeading = 500; |
| } |
| */ |
| } |
| else |
| { // ignore compass at extreme attitudes for a while |
| BadCompassHeading = 500; |
| } |
| } // EOF if(CalculateCompassTimer-- == 1) |
| else |
| { |
| CompassYawSetPoint = 0; |
| } |
| } |
| |