| 138,7 → 138,7 |
|---|
| // MK flags |
| uint16_t ModelIsFlying = 0; |
| |
| volatile uint8_t FC_StatusFlags = 0; |
| volatile uint8_t FC_StatusFlags = 0, FC_StatusFlags2 = 0; |
| volatile uint8_t FC_ErrorFlags = 0; |
| |
| int8_t VarioCharacter = ' '; |
| 212,7 → 212,7 |
|---|
| DebugOut.Analog[20] = ServoNickValue; |
| DebugOut.Analog[22] = Capacity.ActualCurrent; |
| DebugOut.Analog[23] = Capacity.UsedCapacity; |
| DebugOut.Analog[24] = SetPointHeight/5; |
| //DebugOut.Analog[24] = SetPointHeight/5; |
| DebugOut.Analog[27] = CompassCourse; |
| DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
| DebugOut.Analog[30] = GPSStickNick; |
| 614,10 → 614,6 |
|---|
| { |
| Motor[i].SetPoint = MotorTest[i]; |
| Motor[i].SetPointLowerBits = 0; |
| /* |
| Motor[i].SetPoint = MotorTest[i] / 4; |
| Motor[i].SetPointLowerBits = MotorTest[i] % 4; |
| */ |
| } |
| } |
| if(MotorTest_Active) MotorTest_Active--; |
| 702,36 → 698,46 |
|---|
| Ki = 10300 / ( FCParam.IFactor + 1 ); |
| |
| CHK_POTI(tmp,ParamSet.OrientationModeControl); |
| if(tmp > 50) |
| if(tmp > 50) // care free should be set on |
| { |
| #ifdef SWITCH_LEARNS_CAREFREE |
| 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; |
| // check dependencies like valid compass heading |
| if((FromNaviCtrl.CompassHeading < 0) && (FC_StatusFlags & FC_STATUS_MOTOR_RUN)) |
| { |
| // indicate orientation lost |
| if(BeepModulation == 0xFFFF) |
| { |
| BeepTime = 15000; // 1.5 seconds |
| BeepModulation = 0xA400; |
| } |
| // and disable care free |
| CareFree = 0; |
| FC_StatusFlags2 &= ~FC_STATUS2_CAREFREE; |
| UART_VersionInfo.HardwareError[0] |= FC_ERROR0_CAREFREE; |
| } |
| else // everthing ok |
| { |
| #ifdef SWITCH_LEARNS_CAREFREE |
| if(!CareFree) ControlHeading = (((int16_t) ParamSet.OrientationAngle * 15 + CompassHeading) % 360) / 2; |
| #endif |
| CareFree = 1; |
| if(FCParam.AxisCoupling1 < 210) FCParam.AxisCoupling1 += 30; |
| FC_StatusFlags2 |= FC_STATUS2_CAREFREE; |
| UART_VersionInfo.HardwareError[0] &= ~FC_ERROR0_CAREFREE; // reset error flag |
| } |
| } |
| else |
| else // care free off |
| { |
| CareFree = 0; |
| CareFreeOld = 0; |
| FC_StatusFlags2 &= ~FC_STATUS2_CAREFREE; |
| UART_VersionInfo.HardwareError[0] &= ~FC_ERROR0_CAREFREE; // reset error flag |
| } |
| |
| if((FromNaviCtrl.CompassHeading < 0) && (BeepModulation == 0xFFFF) && CareFree && (FC_StatusFlags & FC_STATUS_MOTOR_RUN)) |
| // accoustic signal when CareFree state transition |
| if(CareFree != CareFreeOld) |
| { |
| BeepTime = 15000; // 1.5 seconds |
| BeepModulation = 0xA400; |
| CareFree = 0; |
| if(CareFree) BeepTime = 1500; // has been enabled |
| else BeepTime = 200; // has been disabled |
| CareFreeOld = CareFree; // resync |
| } |
| |
| if(CareFree) |
| { |
| if(FCParam.AxisCoupling1 < 210) FCParam.AxisCoupling1 += 30; |
| } |
| |
| } |
| } |
| |
| 1512,42 → 1518,10 |
|---|
| if(!UpdateCompassCourse) |
| { |
| r = ((540 + CompassCourse - (YawGyroHeading / GYRO_DEG_FACTOR)) % 360) - 180; |
| DebugOut.Analog[19] = r; |
| v = r * (FCParam.CompassYawEffect/2); |
| if(abs(r) > 20) v *= 2; // over 20° course deviation -> twice as fast |
| 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 |
| { // ignore compass at extreme attitudes for a while |
| BadCompassHeading = 500; |
| } |
| */ |
| } |
| } // EOF if(CalculateCompassTimer-- == 1) |
| else |
| 1731,6 → 1705,8 |
|---|
| |
| if(HCActive && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
| { |
| FC_StatusFlags2 |= FC_STATUS2_ALTITUDE_CONTROL; |
| |
| if((ParamSet.Config2 & CFG2_HEIGHT_LIMIT) || !(ParamSet.Config0 & CFG0_HEIGHT_SWITCH)) |
| { |
| // Holgers original version |
| 1762,6 → 1738,7 |
|---|
| FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
| HeightTrimming += abs(StickGas - (StickGasHover + HC_STICKTHRESHOLD)); |
| VarioCharacter = '+'; |
| WaypointTrimming= 0; |
| } // gas stick is below hover point |
| else if(StickGas < (StickGasHover - HC_STICKTHRESHOLD) && !(BaroFlags & BARO_LIMIT_MIN)) |
| { |
| 1773,6 → 1750,7 |
|---|
| FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
| HeightTrimming -= abs(StickGas - (StickGasHover - HC_STICKTHRESHOLD)); |
| VarioCharacter = '-'; |
| WaypointTrimming= 0; |
| } |
| else // gas stick in hover range |
| { |
| 1783,6 → 1761,11 |
|---|
| HeightTrimming += NCAltitudeSpeed; |
| WaypointTrimming = 10; |
| VarioCharacter = '^'; |
| if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) // changed from sinking to rising |
| { |
| FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN; |
| SetPointHeight = ReadingHeight; // update setpoint to current height |
| } |
| } |
| else if(NCAltitudeSpeed && (NCSetPointHeight < SetPointHeight)) |
| { |
| 1790,6 → 1773,11 |
|---|
| HeightTrimming -= NCAltitudeSpeed; |
| WaypointTrimming = -10; |
| VarioCharacter = 'v'; |
| if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) // changed from rising to sinking |
| { |
| FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
| SetPointHeight = ReadingHeight; // update setpoint to current height |
| } |
| } |
| else if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
| { |
| 1951,6 → 1939,7 |
|---|
| FilterHCGas = GasMixFraction; // init filter for HCGas witch current gas mix fraction |
| // set undefined state to indicate vario off |
| FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
| FC_StatusFlags2 &= ~FC_STATUS2_ALTITUDE_CONTROL; |
| } // EOF HC not active |
| |
| // ----------------- Hover Gas Estimation -------------------------------- |
| 1975,7 → 1964,7 |
|---|
| HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L); |
| HoverGasFilter += 4L * tmp_long2; |
| } |
| else if(abs(ReadingVario) < 100) // later on, only on small vertical speed |
| else if((abs(ReadingVario) < 100) && ( abs(ReadingHeight - SetPointHeight) < 256)) // later on, only on small vertical speed, and less setpoit deviation |
| { |
| HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE; |
| HoverGasFilter += tmp_long2; |