Rev 2309 | Rev 2318 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2309 | Rev 2316 | ||
---|---|---|---|
Line 65... | Line 65... | ||
65 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
65 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
66 | int Mittelwert_AccNick, Mittelwert_AccRoll; |
66 | int Mittelwert_AccNick, Mittelwert_AccRoll; |
67 | unsigned int NeutralAccX=0, NeutralAccY=0; |
67 | unsigned int NeutralAccX=0, NeutralAccY=0; |
68 | int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
68 | int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
69 | int NeutralAccZ = 0; |
69 | int NeutralAccZ = 0; |
- | 70 | signed char NeutralAccZfine = 0; |
|
70 | unsigned char ControlHeading = 0;// in 2° |
71 | unsigned char ControlHeading = 0;// in 2° |
71 | long IntegralNick = 0,IntegralNick2 = 0; |
72 | long IntegralNick = 0,IntegralNick2 = 0; |
72 | long IntegralRoll = 0,IntegralRoll2 = 0; |
73 | long IntegralRoll = 0,IntegralRoll2 = 0; |
73 | long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
74 | long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
74 | long Integral_Gier = 0; |
75 | long Integral_Gier = 0; |
Line 209... | Line 210... | ||
209 | DebugOut.Analog[27] = KompassSollWert; |
210 | DebugOut.Analog[27] = KompassSollWert; |
210 | DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
211 | DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
211 | DebugOut.Analog[30] = GPS_Nick; |
212 | DebugOut.Analog[30] = GPS_Nick; |
212 | DebugOut.Analog[31] = GPS_Roll; |
213 | DebugOut.Analog[31] = GPS_Roll; |
213 | if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe; |
214 | if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe; |
214 | - | ||
215 | DebugOut.Analog[16] = Variance; |
215 | //DebugOut.Analog[16] = Variance; |
216 | DebugOut.Analog[17] = VerticalVelocity; |
216 | //DebugOut.Analog[17] = VarioMeter; |
217 | DebugOut.Analog[18] = HoehenWertF; |
217 | //DebugOut.Analog[18] = HoehenWertF; |
218 | DebugOut.Analog[25] = Parameter_Hoehe_P; |
218 | //DebugOut.Analog[25] = Parameter_Hoehe_P; |
219 | DebugOut.Analog[26] = Parameter_Luftdruck_D; |
219 | //DebugOut.Analog[26] = Parameter_Luftdruck_D; |
220 | - | ||
221 | } |
220 | } |
Line 222... | Line 221... | ||
222 | 221 | ||
223 | 222 | ||
Line 270... | Line 269... | ||
270 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
269 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
271 | } |
270 | } |
Line 272... | Line 271... | ||
272 | 271 | ||
273 | //############################################################################ |
272 | //############################################################################ |
274 | // Nullwerte ermitteln |
273 | // Nullwerte ermitteln |
275 | void SetNeutral(unsigned char AccAdjustment) |
274 | unsigned char SetNeutral(unsigned char AccAdjustment) // retuns: "sucess" |
276 | //############################################################################ |
275 | //############################################################################ |
277 | { |
276 | { |
278 | unsigned char i; |
277 | unsigned char i, sucess = 1; |
279 | unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0; |
278 | unsigned int gier_neutral = 0, nick_neutral = 0, roll_neutral = 0, acc_z_neutral = 0; |
280 | VersionInfo.HardwareError[0] = 0; |
279 | VersionInfo.HardwareError[0] = 0; |
281 | // HEF4017Reset_ON; |
280 | // HEF4017Reset_ON; |
282 | NeutralAccX = 0; |
281 | NeutralAccX = 0; |
283 | NeutralAccY = 0; |
282 | NeutralAccY = 0; |
- | 283 | NeutralAccZ = 0; |
|
Line 284... | Line 284... | ||
284 | NeutralAccZ = 0; |
284 | NeutralAccZfine = 0; |
285 | 285 | ||
286 | AdNeutralNick = 0; |
286 | AdNeutralNick = 0; |
Line 306... | Line 306... | ||
306 | { |
306 | { |
307 | Delay_ms_Mess(10); |
307 | Delay_ms_Mess(10); |
308 | gier_neutral += AdWertGier; |
308 | gier_neutral += AdWertGier; |
309 | nick_neutral += AdWertNick; |
309 | nick_neutral += AdWertNick; |
310 | roll_neutral += AdWertRoll; |
310 | roll_neutral += AdWertRoll; |
- | 311 | acc_z_neutral += Aktuell_az; |
|
311 | } |
312 | } |
312 | AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
313 | AdNeutralNick = (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
313 | AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
314 | AdNeutralRoll = (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8); |
314 | AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
315 | AdNeutralGier = (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
- | 316 | NeutralAccZ = (acc_z_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER); |
|
Line 315... | Line 317... | ||
315 | 317 | ||
316 | StartNeutralRoll = AdNeutralRoll; |
318 | StartNeutralRoll = AdNeutralRoll; |
Line 317... | Line 319... | ||
317 | StartNeutralNick = AdNeutralNick; |
319 | StartNeutralNick = AdNeutralNick; |
318 | 320 | ||
319 | if(AccAdjustment) |
321 | if(AccAdjustment) |
320 | { |
322 | { |
321 | NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
- | |
322 | NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
- | |
323 | NeutralAccZ = Aktuell_az; |
323 | NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
324 | 324 | NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
|
325 | // Save ACC neutral settings to eeprom |
325 | // Save ACC neutral settings to eeprom |
326 | SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX); |
326 | SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX); |
327 | SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
327 | SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
328 | SetParamWord(PID_ACC_TOP, (uint16_t)NeutralAccZ); |
328 | SetParamWord(PID_ACC_TOP, (uint16_t)NeutralAccZ); |
329 | } |
329 | } |
330 | else |
330 | else |
331 | { |
331 | { |
332 | // restore from eeprom |
332 | // restore from eeprom |
333 | NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK); |
- | |
334 | NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL); |
333 | NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK); |
335 | NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP); |
334 | NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL); |
336 | // strange settings? |
335 | // strange settings? |
337 | if(((unsigned int) NeutralAccX > 2048) || ((unsigned int) NeutralAccY > 2048) || ((unsigned int) NeutralAccZ > 1024)) |
336 | if(((unsigned int) NeutralAccX > 2048) || ((unsigned int) NeutralAccY > 2048)/* || ((unsigned int) NeutralAccZ > 1024)*/) |
338 | { |
337 | { |
339 | printf("\n\rACC not calibrated!\r\n"); |
338 | printf("\n\rACC not calibrated!\r\n"); |
340 | NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
339 | NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY); |
341 | NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
340 | NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY); |
342 | NeutralAccZ = Aktuell_az; |
341 | sucess = 0; |
343 | } |
- | |
344 | } |
342 | } |
345 | 343 | } |
|
346 | MesswertNick = 0; |
344 | MesswertNick = 0; |
347 | MesswertRoll = 0; |
345 | MesswertRoll = 0; |
348 | MesswertGier = 0; |
346 | MesswertGier = 0; |
Line 354... | Line 352... | ||
354 | Mess_IntegralNick = IntegralNick; |
352 | Mess_IntegralNick = IntegralNick; |
355 | Mess_IntegralRoll = IntegralRoll; |
353 | Mess_IntegralRoll = IntegralRoll; |
356 | Mess_Integral_Gier = 0; |
354 | Mess_Integral_Gier = 0; |
357 | StartLuftdruck = Luftdruck; |
355 | StartLuftdruck = Luftdruck; |
358 | VarioMeter = 0; |
356 | VarioMeter = 0; |
359 | VerticalVelocitySum = 0; |
- | |
360 | SummenHoehe = 0; Mess_Integral_Hoch = 0; |
357 | SummenHoehe = 0; Mess_Integral_Hoch = 0; |
361 | KompassSollWert = KompassValue; |
358 | KompassSollWert = KompassValue; |
362 | KompassSignalSchlecht = 100; |
359 | KompassSignalSchlecht = 100; |
363 | beeptime = 50; |
360 | beeptime = 50; |
364 | Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L; |
361 | Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L; |
Line 385... | Line 382... | ||
385 | // if(EE_Parameter.ServoCompInvert & SERVO_NICK_INV) NickServoValue = ((128 + 60) * 4 * 16); // neutral position = upper 1/4 |
382 | // if(EE_Parameter.ServoCompInvert & SERVO_NICK_INV) NickServoValue = ((128 + 60) * 4 * 16); // neutral position = upper 1/4 |
386 | // else |
383 | // else |
387 | NickServoValue = ((128 - 60) * 4 * 16); // neutral position = lower 1/4 |
384 | NickServoValue = ((128 - 60) * 4 * 16); // neutral position = lower 1/4 |
388 | } |
385 | } |
Line -... | Line 386... | ||
- | 386 | ||
- | 387 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
|
- | 388 | signed int tilt1, tilt2; |
|
- | 389 | tilt1 = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
|
- | 390 | tilt2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
|
- | 391 | tilt1 = (int16_t)ihypot(tilt1,tilt2); // tilt angle over all |
|
- | 392 | CosAttitude = c_cos_8192(tilt1); |
|
- | 393 | NeutralAccZ = (long)((long) (NeutralAccZ - 512) * 8192 + 4096) / CosAttitude + 512; |
|
- | 394 | if(tilt1 > 20) sucess = 0; // calibration must be within 20° Tilt angle |
|
- | 395 | if(ACC_AltitudeControl) if((NeutralAccZ < 682 - 25) || (NeutralAccZ > 682 + 25)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; sucess = 0;}; |
|
- | 396 | ||
- | 397 | #else |
|
- | 398 | NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP); |
|
- | 399 | #endif |
|
389 | 400 | ||
390 | if((AdNeutralNick < 150 * 16) || (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; }; |
401 | if((AdNeutralNick < 150 * 16) || (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; }; |
391 | if((AdNeutralRoll < 150 * 16) || (AdNeutralRoll > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_ROLL; }; |
402 | if((AdNeutralRoll < 150 * 16) || (AdNeutralRoll > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_ROLL; }; |
392 | if((AdNeutralGier < 150 * 2) || (AdNeutralGier > 850 * 2)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; }; |
403 | if((AdNeutralGier < 150 * 2) || (AdNeutralGier > 850 * 2)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; }; |
393 | if((NeutralAccX < 300) || (NeutralAccX > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_NICK; }; |
404 | if((NeutralAccX < 300) || (NeutralAccX > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_NICK; }; |
394 | if((NeutralAccY < 300) || (NeutralAccY > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_ROLL; }; |
405 | if((NeutralAccY < 300) || (NeutralAccY > 750)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_ROLL; }; |
- | 406 | if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; }; |
|
395 | if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; }; |
407 | if(VersionInfo.HardwareError[0]) sucess = 0; |
396 | carefree_old = 70; |
408 | carefree_old = 70; |
397 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
409 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
- | 410 | LIBFC_HoTT_Clear(); |
|
398 | LIBFC_HoTT_Clear(); |
411 | ACC_AltitudeFusion(2); // initalisation |
- | 412 | #endif |
|
399 | #endif |
413 | return(sucess); |
Line 400... | Line 414... | ||
400 | } |
414 | } |
401 | 415 | ||
Line 861... | Line 875... | ||
861 | if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
875 | if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
862 | { |
876 | { |
863 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
877 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
864 | } |
878 | } |
865 | // ServoActive = 0; |
879 | // ServoActive = 0; |
866 | SetNeutral(0); |
- | |
867 | CalibrationDone = 1; |
880 | CalibrationDone = SetNeutral(0); |
868 | ServoActive = 1; |
881 | ServoActive = 1; |
869 | DDRD |=0x80; // enable J7 -> Servo signal |
882 | DDRD |=0x80; // enable J7 -> Servo signal |
870 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
883 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
871 | if(abs(Aktuell_az - NeutralAccZ) > 5 && ACC_AltitudeControl) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; }; |
- | |
872 | if(VersionInfo.HardwareError[0]) SpeakHoTT = SPEAK_ERR_SENSOR; |
884 | if(VersionInfo.HardwareError[0]) SpeakHoTT = SPEAK_ERR_SENSOR; |
- | 885 | else |
|
- | 886 | if(!CalibrationDone) SpeakHoTT = SPEAK_ERR_CALIBARTION; |
|
873 | else SpeakHoTT = SPEAK_CALIBRATE; |
887 | else SpeakHoTT = SPEAK_CALIBRATE; |
874 | #endif |
888 | #endif |
875 | Piep(GetActiveParamSet(),120); |
889 | Piep(GetActiveParamSet(),120); |
876 | } |
890 | } |
877 | } |
891 | } |
Line 882... | Line 896... | ||
882 | if(++delay_neutral > 200) // nicht sofort |
896 | if(++delay_neutral > 200) // nicht sofort |
883 | { |
897 | { |
884 | MotorenEin = 0; |
898 | MotorenEin = 0; |
885 | delay_neutral = 0; |
899 | delay_neutral = 0; |
886 | modell_fliegt = 0; |
900 | modell_fliegt = 0; |
887 | SetNeutral(1); |
- | |
888 | CalibrationDone = 1; |
901 | CalibrationDone = SetNeutral(1); |
889 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
902 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
- | 903 | if(VersionInfo.HardwareError[0]) SpeakHoTT = SPEAK_ERR_SENSOR; |
|
- | 904 | else |
|
- | 905 | if(!CalibrationDone) SpeakHoTT = SPEAK_ERR_CALIBARTION; |
|
890 | SpeakHoTT = SPEAK_CALIBRATE; |
906 | else SpeakHoTT = SPEAK_CALIBRATE; |
891 | #endif |
907 | #endif |
892 | Piep(GetActiveParamSet(),120); |
908 | Piep(GetActiveParamSet(),120); |
893 | } |
909 | } |
894 | } |
910 | } |
895 | else delay_neutral = 0; |
911 | else delay_neutral = 0; |
Line 1528... | Line 1544... | ||
1528 | } |
1544 | } |
1529 | } |
1545 | } |
1530 | else // delay, because of expanding the Baro-Range |
1546 | else // delay, because of expanding the Baro-Range |
1531 | { |
1547 | { |
1532 | // now clear the D-values |
1548 | // now clear the D-values |
- | 1549 | VarioMeter = 0; |
|
1533 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
1550 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
1534 | if(ACC_AltitudeControl) SummenHoehe = HoehenWert * SA_FILTER; |
1551 | if(ACC_AltitudeControl) ACC_AltitudeFusion(1); // init |
1535 | else SummenHoehe = HoehenWert * SM_FILTER; |
1552 | else SummenHoehe = HoehenWert * SM_FILTER; |
1536 | #else |
1553 | #else |
1537 | SummenHoehe = HoehenWert * SM_FILTER; |
1554 | SummenHoehe = HoehenWert * SM_FILTER; |
1538 | #endif |
1555 | #endif |
1539 | - | ||
1540 | VerticalVelocitySum = 0; |
- | |
1541 | VarioMeter = 0; |
- | |
1542 | BaroExpandActive--; |
1556 | BaroExpandActive--; |
1543 | } |
1557 | } |
1544 | - | ||
1545 | // if height control is activated by an rc channel |
1558 | // if height control is activated by an rc channel |
1546 | if(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1559 | if(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1547 | { // check if parameter is less than activation threshold |
1560 | { // check if parameter is less than activation threshold |
1548 | if(Parameter_HoehenSchalter < 50) // for 3 or 2-state switch height control is disabled in lowest position |
1561 | if(Parameter_HoehenSchalter < 50) // for 3 or 2-state switch height control is disabled in lowest position |
1549 | { //height control not active |
1562 | { //height control not active |
Line 1720... | Line 1733... | ||
1720 | } |
1733 | } |
1721 | HCGas = HoverGas; // take hover gas (neutral point) |
1734 | HCGas = HoverGas; // take hover gas (neutral point) |
1722 | } |
1735 | } |
1723 | if(HoehenWertF > SollHoehe || !(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT)) |
1736 | if(HoehenWertF > SollHoehe || !(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT)) |
1724 | { |
1737 | { |
1725 | if(!ACC_AltitudeControl) |
1738 | if(!ACC_AltitudeControl) |
1726 | { |
1739 | { |
1727 | // from this point the Heigth Control Algorithm is identical for both versions |
1740 | // from this point the Heigth Control Algorithm is identical for both versions |
1728 | if(BaroExpandActive) // baro range expanding active |
1741 | if(BaroExpandActive) // baro range expanding active |
1729 | { |
1742 | { |
1730 | HCGas = HoverGas; // hover while expanding baro adc range |
1743 | HCGas = HoverGas; // hover while expanding baro adc range |
1731 | HeightDeviation = 0; |
1744 | HeightDeviation = 0; |
Line 1804... | Line 1817... | ||
1804 | { // old version |
1817 | { // old version |
1805 | LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
1818 | LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
1806 | GasMischanteil = FilterHCGas; |
1819 | GasMischanteil = FilterHCGas; |
1807 | } |
1820 | } |
1808 | else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode |
1821 | else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode |
1809 | } |
1822 | } |
1810 | else |
1823 | else // ACC-Altitude control |
1811 | { |
1824 | { |
1812 | // from this point the Heigth Control Algorithm is identical for both versions |
1825 | // from this point the Heigth Control Algorithm is identical for both versions |
1813 | if(BaroExpandActive) // baro range expanding active |
1826 | if(BaroExpandActive) // baro range expanding active |
1814 | { |
1827 | { |
1815 | HCGas = HoverGas; // hover while expanding baro adc range |
1828 | HCGas = HoverGas; // hover while expanding baro adc range |
1816 | HeightDeviation = 0; |
1829 | HeightDeviation = 0; |
Line 1823... | Line 1836... | ||
1823 | HeightDeviation = (int)(tmp_long); // positive when too high |
1836 | HeightDeviation = (int)(tmp_long); // positive when too high |
1824 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part |
1837 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part |
1825 | LIMIT_MIN_MAX(tmp_long, -511 * STICK_GAIN, 512 * STICK_GAIN); // more than full range makes sense |
1838 | LIMIT_MIN_MAX(tmp_long, -511 * STICK_GAIN, 512 * STICK_GAIN); // more than full range makes sense |
1826 | GasReduction = tmp_long; |
1839 | GasReduction = tmp_long; |
1827 | // ------------------------ D-Part: ACC-Z Integral ------------------------ |
1840 | // ------------------------ D-Part: ACC-Z Integral ------------------------ |
1828 | tmp_long = VerticalVelocity + AdWertAccHoch * Parameter_Hoehe_ACC_Wirkung/256; |
1841 | tmp_long = VarioMeter + (AdWertAccHoch * Parameter_Hoehe_ACC_Wirkung)/256; |
1829 | // ------------------------- D-Part: Vario Meter ---------------------------- |
1842 | // ------------------------- D-Part: Vario Meter ---------------------------- |
1830 | if(WaypointTrimming) { |
1843 | if(WaypointTrimming) { |
1831 | Variance = AltitudeSetpointTrimming * 8; |
1844 | Variance = AltitudeSetpointTrimming * 8; |
1832 | } else { |
1845 | } else { |
1833 | Variance = AltitudeSetpointTrimming * EE_Parameter.Hoehe_Verstaerkung*9/32; |
1846 | Variance = AltitudeSetpointTrimming * EE_Parameter.Hoehe_Verstaerkung*9/32; |
Line 1835... | Line 1848... | ||
1835 | tmp_long -= (long)Variance; |
1848 | tmp_long -= (long)Variance; |
1836 | tmp_long = (tmp_long * (long)Parameter_Luftdruck_D) / 32; // scale to d-gain parameter |
1849 | tmp_long = (tmp_long * (long)Parameter_Luftdruck_D) / 32; // scale to d-gain parameter |
1837 | LIMIT_MIN_MAX(tmp_long,-511 * STICK_GAIN, 512 * STICK_GAIN); |
1850 | LIMIT_MIN_MAX(tmp_long,-511 * STICK_GAIN, 512 * STICK_GAIN); |
1838 | GasReduction += tmp_long; |
1851 | GasReduction += tmp_long; |
1839 | } // EOF no baro range expanding |
1852 | } // EOF no baro range expanding |
1840 | DebugOut.Analog[19] = -GasReduction; |
- | |
1841 | HCGas -= GasReduction; |
1853 | HCGas -= GasReduction; |
1842 | LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limits gas around hover point |
1854 | LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limits gas around hover point |
1843 | // strech control output by inverse attitude projection 1/cos |
1855 | // strech control output by inverse attitude projection 1/cos |
1844 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
1856 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
1845 | tmp_long2 = (int32_t)HCGas; |
1857 | tmp_long2 = (int32_t)HCGas; |
Line 1854... | Line 1866... | ||
1854 | if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) |
1866 | if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) |
1855 | { // old version |
1867 | { // old version |
1856 | LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
1868 | LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
1857 | GasMischanteil = FilterHCGas; |
1869 | GasMischanteil = FilterHCGas; |
1858 | } |
1870 | } |
1859 | else GasMischanteil = FilterHCGas;// + (GasMischanteil - HoverGas) / 4; // Qopter: geändert |
1871 | else GasMischanteil = FilterHCGas; |
1860 | } |
- | |
- | 1872 | } // end of ACC-Altitude control |
|
1861 | } |
1873 | } |
1862 | }// EOF height control active |
1874 | }// EOF height control active |
1863 | else // HC not active |
1875 | else // HC not active |
1864 | { |
1876 | { |
1865 | //update hoover gas stick value when HC is not active |
1877 | //update hoover gas stick value when HC is not active |
Line 1877... | Line 1889... | ||
1877 | } |
1889 | } |
1878 | // Hover gas estimation by averaging gas control output on small z-velocities |
1890 | // Hover gas estimation by averaging gas control output on small z-velocities |
1879 | // this is done only if height contol option is selected in global config and aircraft is flying |
1891 | // this is done only if height contol option is selected in global config and aircraft is flying |
1880 | if((FC_StatusFlags & FC_STATUS_FLY))// && !(FC_SatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
1892 | if((FC_StatusFlags & FC_STATUS_FLY))// && !(FC_SatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
1881 | { |
1893 | { |
1882 | //if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
1894 | //if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
1883 | if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(HoverGas); // Qopter: geändert |
1895 | if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(HoverGas); // Qopter: geändert |
1884 | if(StartTrigger == 1) StartTrigger = 2; |
1896 | if(StartTrigger == 1) StartTrigger = 2; |
1885 | tmp_long2 = (int32_t)GasMischanteil; // take current thrust |
1897 | tmp_long2 = (int32_t)GasMischanteil; // take current thrust |
1886 | tmp_long2 *= CosAttitude; // apply attitude projection |
1898 | tmp_long2 *= CosAttitude; // apply attitude projection |
1887 | tmp_long2 /= 8192; |
1899 | tmp_long2 /= 8192; |
1888 | // average vertical projected thrust |
1900 | // average vertical projected thrust |
Line 1895... | Line 1907... | ||
1895 | { // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
1907 | { // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
1896 | HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L); |
1908 | HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L); |
1897 | HoverGasFilter += 4L * tmp_long2; |
1909 | HoverGasFilter += 4L * tmp_long2; |
1898 | } |
1910 | } |
1899 | else //later |
1911 | else //later |
1900 | //if(abs(VerticalVelocity) < 50 && abs(HoehenWertF - SollHoehe) < 256) // Qopter: geändert, Anpassung nötig? |
- | |
1901 | if(abs(VarioMeter) < 100 && abs(HoehenWert - SollHoehe) < 256) // only on small vertical speed & difference is small (only descending) |
1912 | if(abs(VarioMeter) < 100 && abs(HoehenWertF - SollHoehe) < 256) // only on small vertical speed & difference is small (only descending) |
1902 | { |
1913 | { |
1903 | HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE; |
1914 | HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE; |
1904 | HoverGasFilter += tmp_long2; |
1915 | HoverGasFilter += tmp_long2; |
1905 | } |
1916 | } |
1906 | HoverGas = (int16_t)(HoverGasFilter/HOVER_GAS_AVERAGE); |
1917 | HoverGas = (int16_t)(HoverGasFilter/HOVER_GAS_AVERAGE); |