Rev 2466 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2466 | Rev 2471 | ||
|---|---|---|---|
| Line 302... | Line 302... | ||
| 302 | 302 | ||
| 303 | CalibrierMittelwert(); |
303 | CalibrierMittelwert(); |
| 304 | Delay_ms_Mess(100); |
304 | Delay_ms_Mess(100); |
| Line 305... | Line -... | ||
| 305 | CalibrierMittelwert(); |
- | |
| 306 | - | ||
| 307 | DebugOut.Analog[16] = 0; |
- | |
| 308 | DebugOut.Analog[17] = 0; |
- | |
| 309 | 305 | CalibrierMittelwert(); |
|
| 310 | 306 | ||
| 311 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
307 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
| 312 | { |
308 | { |
| Line 371... | Line 367... | ||
| 371 | Mess_IntegralNick = IntegralNick; |
367 | Mess_IntegralNick = IntegralNick; |
| 372 | Mess_IntegralRoll = IntegralRoll; |
368 | Mess_IntegralRoll = IntegralRoll; |
| 373 | Mess_Integral_Gier = 0; |
369 | Mess_Integral_Gier = 0; |
| 374 | KompassSollWert = KompassValue; |
370 | KompassSollWert = KompassValue; |
| 375 | KompassSignalSchlecht = 100; |
371 | KompassSignalSchlecht = 100; |
| 376 | beeptime = 50; |
- | |
| 377 | Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L; |
372 | Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L; |
| 378 | Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L; |
373 | Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L; |
| 379 | ExternHoehenValue = 0; |
374 | ExternHoehenValue = 0; |
| 380 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
375 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
| 381 | GierGyroFehler = 0; |
376 | GierGyroFehler = 0; |
| Line 453... | Line 448... | ||
| 453 | NaviAccRoll += AdWertAccRoll; |
448 | NaviAccRoll += AdWertAccRoll; |
| 454 | NaviCntAcc++; |
449 | NaviCntAcc++; |
| 455 | IntegralAccZ += Aktuell_az - NeutralAccZ; |
450 | IntegralAccZ += Aktuell_az - NeutralAccZ; |
| Line 456... | Line 451... | ||
| 456 | 451 | ||
| - | 452 | //++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 453 | HoehenWert = HoehenWert_Mess; |
|
| - | 454 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
|
| - | 455 | HoehenWertF = HoehenWertF_Mess; |
|
| - | 456 | #else |
|
| - | 457 | HoehenWertF = HoehenWert; |
|
| - | 458 | #endif |
|
| 457 | //++++++++++++++++++++++++++++++++++++++++++++++++ |
459 | //++++++++++++++++++++++++++++++++++++++++++++++++ |
| 458 | // ADC einschalten |
460 | // ADC einschalten |
| 459 | ANALOG_ON; |
461 | ANALOG_ON; |
| 460 | AdReady = 0; |
462 | AdReady = 0; |
| Line 1040... | Line 1042... | ||
| 1040 | // Einschalten |
1042 | // Einschalten |
| 1041 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1043 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1042 | if(CalibrationDone) FC_StatusFlags |= FC_STATUS_START; |
1044 | if(CalibrationDone) FC_StatusFlags |= FC_STATUS_START; |
| Line 1043... | Line 1045... | ||
| 1043 | 1045 | ||
| 1044 | StartLuftdruck = Luftdruck; |
- | |
| 1045 | HoehenWertF = 0; |
1046 | StartLuftdruck = Luftdruck; |
| - | 1047 | HoehenWert = 0; |
|
| - | 1048 | HoehenWert_Mess = 0; |
|
| - | 1049 | GasIsZeroCnt = 600; |
|
| - | 1050 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
|
| - | 1051 | HoehenWertF_Mess = 0; |
|
| 1046 | HoehenWert = 0; |
1052 | #endif |
| 1047 | SummenHoehe = 0; |
1053 | SummenHoehe = 0; |
| 1048 | if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -100 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 100) && EE_Parameter.MotorSafetySwitch == 0) delay_einschalten = 0; |
1054 | if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -100 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 100) && EE_Parameter.MotorSafetySwitch == 0) delay_einschalten = 0; |
| 1049 | if(++delay_einschalten > 253) |
1055 | if(++delay_einschalten > 253) |
| 1050 | { |
1056 | { |
| Line 1823... | Line 1829... | ||
| 1823 | } |
1829 | } |
| 1824 | WaypointTrimming = 0; |
1830 | WaypointTrimming = 0; |
| 1825 | } // gas stick is below hoover point |
1831 | } // gas stick is below hoover point |
| 1826 | else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) // Minus |
1832 | else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) // Minus |
| 1827 | { |
1833 | { |
| 1828 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) |
1834 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP || (HeightDeviation < -300)) |
| 1829 | { |
1835 | { |
| 1830 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
1836 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
| 1831 | SollHoehe = HoehenWertF; // update setpoint to current heigth |
1837 | SollHoehe = HoehenWertF; // update setpoint to current heigth |
| 1832 | } |
1838 | } |
| 1833 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
1839 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
| Line 1892... | Line 1898... | ||
| 1892 | { |
1898 | { |
| 1893 | if(HeightTrimming > 0) SollHoehe += EE_Parameter.Hoehe_Verstaerkung / 3; |
1899 | if(HeightTrimming > 0) SollHoehe += EE_Parameter.Hoehe_Verstaerkung / 3; |
| 1894 | else SollHoehe -= EE_Parameter.Hoehe_Verstaerkung / 3; |
1900 | else SollHoehe -= EE_Parameter.Hoehe_Verstaerkung / 3; |
| 1895 | } |
1901 | } |
| 1896 | HeightTrimming = 0; |
1902 | HeightTrimming = 0; |
| - | 1903 | LIMIT_MIN_MAX(HoehenWertF, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied |
|
| 1897 | LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied |
1904 | LIMIT_MIN_MAX(SollHoehe, (HoehenWertF-1024), (HoehenWertF+1024)); // max. 10m Unterschied |
| 1898 | if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100; |
1905 | if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100; |
| 1899 | //update hoover gas stick value when setpoint is shifted |
1906 | //update hoover gas stick value when setpoint is shifted |
| 1900 | if(FromNC_AltitudeSpeed == 0) CalcStickGasHover(); |
1907 | if(FromNC_AltitudeSpeed == 0) CalcStickGasHover(); |
| 1901 | /* |
1908 | /* |
| 1902 | if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0) |
1909 | if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0) |
| Line 1910... | Line 1917... | ||
| 1910 | } |
1917 | } |
| 1911 | if(BaroExpandActive) SollHoehe = HoehenWertF; // update setpoint to current altitude if Expanding is active |
1918 | if(BaroExpandActive) SollHoehe = HoehenWertF; // update setpoint to current altitude if Expanding is active |
| 1912 | } //if FCFlags & MKFCFLAG_FLY |
1919 | } //if FCFlags & MKFCFLAG_FLY |
| 1913 | else |
1920 | else |
| 1914 | { |
1921 | { |
| 1915 | SollHoehe = HoehenWert - 2000; |
- | |
| 1916 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
1922 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
| 1917 | ACC_AltitudeFusion(1); // init the ACC and Altitude fusion |
1923 | ACC_AltitudeFusion(1); // init the ACC and Altitude fusion |
| 1918 | #endif |
1924 | #endif |
| - | 1925 | SollHoehe = HoehenWertF - 2000; |
|
| - | 1926 | ||
| 1919 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint; |
1927 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint; |
| 1920 | else StickGasHover = 127; |
1928 | else StickGasHover = 127; |
| 1921 | HoverGas = GasMischanteil; |
1929 | HoverGas = GasMischanteil; |
| 1922 | VarioCharacter = '.'; |
1930 | VarioCharacter = '.'; |
| 1923 | } |
1931 | } |
| Line 2050... | Line 2058... | ||
| 2050 | tmp_long2 /= CosAttitude; |
2058 | tmp_long2 /= CosAttitude; |
| 2051 | HCGas = (int16_t)tmp_long2; |
2059 | HCGas = (int16_t)tmp_long2; |
| 2052 | // update height control gas averaging |
2060 | // update height control gas averaging |
| 2053 | FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
2061 | FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
| 2054 | // limit height control gas pd-control output |
2062 | // limit height control gas pd-control output |
| - | 2063 | int min; |
|
| - | 2064 | if(GasIsZeroCnt > 400 || (FC_StatusFlags2 & FC_STATUS2_WAIT_FOR_TAKEOFF)) min = EE_Parameter.Gas_Min; else min = EE_Parameter.Hoehe_MinGas; |
|
| 2055 | LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN); |
2065 | LIMIT_MIN_MAX(FilterHCGas, min * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN) |
| 2056 | // set GasMischanteil to HeightControlGasFilter |
2066 | // set GasMischanteil to HeightControlGasFilter |
| 2057 | if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) |
2067 | if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) |
| 2058 | { // old version |
2068 | { // old version |
| 2059 | LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
2069 | LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
| 2060 | GasMischanteil = FilterHCGas; |
2070 | GasMischanteil = FilterHCGas; |
| Line 2275... | Line 2285... | ||
| 2275 | } |
2285 | } |
| 2276 | } |
2286 | } |
| 2277 | #ifdef REDUNDANT_FC_MASTER |
2287 | #ifdef REDUNDANT_FC_MASTER |
| 2278 | if(Parameter_UserParam6 > 230) Motor[0].SetPoint = 0; |
2288 | if(Parameter_UserParam6 > 230) Motor[0].SetPoint = 0; |
| 2279 | #endif |
2289 | #endif |
| - | 2290 | ||
| 2280 | } |
2291 | } |
| 2281 | //DebugOut.Analog[16] |
2292 | //DebugOut.Analog[16] |