Rev 2466 | Rev 2481 | 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] |