Rev 266 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 266 | Rev 290 | ||
---|---|---|---|
Line 79... | Line 79... | ||
79 | volatile int KompassStartwert = 0; |
79 | volatile int KompassStartwert = 0; |
80 | volatile int KompassRichtung = 0; |
80 | volatile int KompassRichtung = 0; |
81 | unsigned char MAX_GAS,MIN_GAS; |
81 | unsigned char MAX_GAS,MIN_GAS; |
82 | unsigned char Notlandung = 0; |
82 | unsigned char Notlandung = 0; |
83 | unsigned char HoehenReglerAktiv = 0; |
83 | unsigned char HoehenReglerAktiv = 0; |
84 | //Salvo 4.10.2007 |
84 | //Salvo 12.10.2007 |
85 | uint8_t magkompass_ok=0; |
85 | uint8_t magkompass_ok=0; |
86 | uint8_t gps_cmd = GPS_CMD_STOP; |
86 | uint8_t gps_cmd = GPS_CMD_STOP; |
- | 87 | static int ubat_cnt =0; |
|
- | 88 | static int gas_actual,gas_mittel; //Parameter fuer Gasreduzierung bei unterspannung |
|
87 | //Salvo End |
89 | //Salvo End |
Line 88... | Line 90... | ||
88 | 90 | ||
89 | //Salvo 2.9.2007 Ersatzkompass |
91 | //Salvo 2.9.2007 Ersatzkompass |
90 | volatile long GyroKomp_Int,GyroKomp_Int2; |
92 | volatile long GyroKomp_Int,GyroKomp_Int2; |
Line 181... | Line 183... | ||
181 | StartLuftdruck = Luftdruck; |
183 | StartLuftdruck = Luftdruck; |
182 | HoeheD = 0; |
184 | HoeheD = 0; |
183 | Mess_Integral_Hoch = 0; |
185 | Mess_Integral_Hoch = 0; |
184 | KompassStartwert = KompassValue; |
186 | KompassStartwert = KompassValue; |
185 | beeptime = 50; |
187 | beeptime = 50; |
186 | //Salvo 4.9.2007 Ersatzkompass |
188 | //Salvo 13.10.2007 Ersatzkompass |
187 | GyroKomp_Int = 0; |
189 | GyroKomp_Int = 0; |
- | 190 | gas_mittel = 30; |
|
- | 191 | gas_actual = gas_mittel; |
|
188 | // Salvo End |
192 | // Salvo End |
189 | } |
193 | } |
Line 190... | Line 194... | ||
190 | 194 | ||
191 | //############################################################################ |
195 | //############################################################################ |
Line 324... | Line 328... | ||
324 | EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
328 | EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
325 | EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
329 | EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
326 | EE_Parameter.KompassWirkung = 64; // Wert : 0-250 |
330 | EE_Parameter.KompassWirkung = 64; // Wert : 0-250 |
327 | EE_Parameter.Gyro_P = 120; //80 // Wert : 0-250 |
331 | EE_Parameter.Gyro_P = 120; //80 // Wert : 0-250 |
328 | EE_Parameter.Gyro_I = 150; // Wert : 0-250 |
332 | EE_Parameter.Gyro_I = 150; // Wert : 0-250 |
329 | EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250 |
333 | EE_Parameter.UnterspannungsWarnung = 102; // Wert : 0-250 |
330 | EE_Parameter.NotGas = 100; // Wert : 0-250 // Gaswert bei Empangsverlust |
334 | EE_Parameter.NotGas = 100; // Wert : 0-250 // Gaswert bei Empangsverlust |
331 | EE_Parameter.NotGasZeit = 60; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
335 | EE_Parameter.NotGasZeit = 60; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
332 | EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
336 | EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
333 | EE_Parameter.I_Faktor = 5; |
337 | EE_Parameter.I_Faktor = 5; |
334 | EE_Parameter.UserParam1 = 0; //zur freien Verwendung, derzeit P-Anteil GPS |
338 | EE_Parameter.UserParam1 = 16; //zur freien Verwendung, derzeit P-Anteil GPS |
335 | EE_Parameter.UserParam2 = 0; //zur freien Verwendung, derzeit I-Anteil GPS |
339 | EE_Parameter.UserParam2 = 2; //zur freien Verwendung, derzeit I-Anteil GPS |
336 | EE_Parameter.UserParam3 = 0; //zur freien Verwendung, derzeit D-Anteil GPS |
340 | EE_Parameter.UserParam3 = 12; //zur freien Verwendung, derzeit D-Anteil GPS |
337 | EE_Parameter.UserParam4 = 0; //zur freien Verwendung |
341 | EE_Parameter.UserParam4 = 0; //zur freien Verwendung |
338 | EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
342 | EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
339 | EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
343 | EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
340 | EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
344 | EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
341 | EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
345 | EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
Line 367... | Line 371... | ||
367 | EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
371 | EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
368 | EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
372 | EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
369 | EE_Parameter.KompassWirkung = 64; // Wert : 0-250 |
373 | EE_Parameter.KompassWirkung = 64; // Wert : 0-250 |
370 | EE_Parameter.Gyro_P = 175; //80 // Wert : 0-250 |
374 | EE_Parameter.Gyro_P = 175; //80 // Wert : 0-250 |
371 | EE_Parameter.Gyro_I = 175; // Wert : 0-250 |
375 | EE_Parameter.Gyro_I = 175; // Wert : 0-250 |
372 | EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250 |
376 | EE_Parameter.UnterspannungsWarnung = 102; // Wert : 0-250 |
373 | EE_Parameter.NotGas = 100; // Wert : 0-250 // Gaswert bei Empangsverlust |
377 | EE_Parameter.NotGas = 100; // Wert : 0-250 // Gaswert bei Empangsverlust |
374 | EE_Parameter.NotGasZeit = 60; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
378 | EE_Parameter.NotGasZeit = 60; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
375 | EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
379 | EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
376 | EE_Parameter.I_Faktor = 5; |
380 | EE_Parameter.I_Faktor = 5; |
377 | EE_Parameter.UserParam1 = 0; //zur freien Verwendung |
381 | EE_Parameter.UserParam1 = 12; //zur freien Verwendung |
378 | EE_Parameter.UserParam2 = 0; //zur freien Verwendung |
382 | EE_Parameter.UserParam2 = 2; //zur freien Verwendung |
379 | EE_Parameter.UserParam3 = 0; //zur freien Verwendung |
383 | EE_Parameter.UserParam3 = 16; //zur freien Verwendung |
380 | EE_Parameter.UserParam4 = 0; //zur freien Verwendung |
384 | EE_Parameter.UserParam4 = 0; //zur freien Verwendung |
381 | EE_Parameter.UserParam3 = 0; //zur freien Verwendung |
385 | EE_Parameter.UserParam3 = 0; //zur freien Verwendung |
382 | EE_Parameter.UserParam4 = 0; //zur freien Verwendung |
386 | EE_Parameter.UserParam4 = 0; //zur freien Verwendung |
383 | EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
387 | EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
384 | EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
388 | EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
Line 455... | Line 459... | ||
455 | //******PROVISORISCH*************** |
459 | //******PROVISORISCH*************** |
456 | GRN_ON; |
460 | GRN_ON; |
Line 457... | Line 461... | ||
457 | 461 | ||
458 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
462 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
459 | // Gaswert ermitteln |
463 | // Gaswert ermitteln |
460 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
464 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 465 | GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
|
- | 466 | //Salvo 13.10.2007 langsame Gasreduktion bei Unterspannung. Als Ausgangswert wird der bei UBAT=k gemessen Mittelwert genommen |
|
- | 467 | // und dieser dann langsam zwangsweise reduziert |
|
- | 468 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
|
- | 469 | if (UBat <= EE_Parameter.UnterspannungsWarnung - 2) //Unterhalb der Piepser Schwelle aktivieren |
|
- | 470 | { |
|
- | 471 | if (ubat_cnt > 800) |
|
- | 472 | { |
|
- | 473 | ubat_cnt = 0; |
|
- | 474 | if (gas_actual > ((gas_mittel*13)/15)) gas_actual--; |
|
- | 475 | } |
|
- | 476 | else ubat_cnt++; |
|
- | 477 | if (GasMischanteil > gas_actual) GasMischanteil = gas_actual; |
|
- | 478 | } |
|
- | 479 | else //Falls UBAT wieder ok ist |
|
- | 480 | { |
|
- | 481 | if (ubat_cnt > 1000) |
|
- | 482 | { |
|
- | 483 | gas_mittel = ((gas_mittel*9) + GasMischanteil)/10; //Filtern |
|
- | 484 | gas_actual = GasMischanteil; |
|
- | 485 | } |
|
- | 486 | else |
|
- | 487 | { |
|
- | 488 | ubat_cnt++; |
|
- | 489 | if ((ubat_cnt % 10) == 0) |
|
- | 490 | { |
|
- | 491 | if (gas_actual < GasMischanteil) gas_actual++; |
|
- | 492 | else gas_actual = GasMischanteil; |
|
- | 493 | } |
|
- | 494 | } |
|
- | 495 | GasMischanteil = gas_actual; |
|
- | 496 | } |
|
- | 497 | ANALOG_ON; // ADC einschalten |
|
461 | GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
498 | // Salvo End |
Line 462... | Line 499... | ||
462 | if(GasMischanteil < 0) GasMischanteil = 0; |
499 | if(GasMischanteil < 0) GasMischanteil = 0; |
463 | 500 | ||
464 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
501 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
465 | // Emfang schlecht |
502 | // Empfang schlecht |
466 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
503 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
467 | if(SenderOkay < 100) |
504 | if(SenderOkay < 100) |
468 | { |
505 | { |
Line 831... | Line 868... | ||
831 | if(!TimerWerteausgabe--) |
868 | if(!TimerWerteausgabe--) |
832 | { |
869 | { |
833 | TimerWerteausgabe = 49; |
870 | TimerWerteausgabe = 49; |
834 | // DebugOut.Analog[0] = MesswertNick; |
871 | // DebugOut.Analog[0] = MesswertNick; |
835 | // DebugOut.Analog[1] = MesswertRoll; |
872 | // DebugOut.Analog[1] = MesswertRoll; |
836 | DebugOut.Analog[0] = gps_sub_state; |
873 | // DebugOut.Analog[0] = gps_sub_state; |
Line 837... | Line 874... | ||
837 | 874 | ||
838 | DebugOut.Analog[1] = dist_2home; |
875 | /* DebugOut.Analog[1] = dist_2home; |
- | 876 | DebugOut.Analog[2] = hdng_2home; |
|
Line 839... | Line -... | ||
839 | DebugOut.Analog[2] = hdng_2home; |
- | |
840 | - | ||
841 | - | ||
842 | DebugOut.Analog[3] = gps_rel_hold_position.utm_east; |
- | |
843 | DebugOut.Analog[4] = gps_rel_hold_position.utm_north; |
- | |
844 | 877 | */ |
|
845 | /* DebugOut.Analog[2] = MesswertGier; |
878 | |
- | 879 | DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
|
- | 880 | DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
|
- | 881 | /* DebugOut.Analog[3] = gps_rel_hold_position.utm_east; |
|
846 | DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
882 | DebugOut.Analog[4] = gps_rel_hold_position.utm_north; |
847 | DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
883 | */ |
848 | DebugOut.Analog[2] = Mittelwert_AccNick; |
884 | DebugOut.Analog[2] = Mittelwert_AccNick; |
849 | DebugOut.Analog[3] = Mittelwert_AccRoll; |
885 | DebugOut.Analog[3] = Mittelwert_AccRoll; |
850 | DebugOut.Analog[4] = MesswertGier; |
886 | DebugOut.Analog[4] = MesswertGier; |
- | 887 | DebugOut.Analog[5] = HoehenWert; |
|
851 | DebugOut.Analog[5] = HoehenWert; |
888 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
852 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
889 | // DebugOut.Analog[2] = MesswertGier; |
- | 890 | ||
853 | */ |
891 | |
854 | /* |
892 | |
855 | DebugOut.Analog[7] = GasMischanteil; |
893 | DebugOut.Analog[7] = GasMischanteil; |
856 | */ |
894 | |
Line 857... | Line 895... | ||
857 | DebugOut.Analog[7] = dist_flown; |
895 | // DebugOut.Analog[7] = dist_flown; |
Line 858... | Line 896... | ||
858 | DebugOut.Analog[8] = KompassValue; |
896 | DebugOut.Analog[8] = KompassValue; |
Line 933... | Line 971... | ||
933 | { |
971 | { |
934 | if(Parameter_MaxHoehe < 50) |
972 | if(Parameter_MaxHoehe < 50) |
935 | { |
973 | { |
936 | //Salvo 10.10.2007 Um Absacken beim Einschalten zu verhindern |
974 | //Salvo 10.10.2007 Um Absacken beim Einschalten zu verhindern |
937 | // SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
975 | // SollHoehe = HoehenWert - 20; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
938 | SollHoehe = HoehenWert -10; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
976 | SollHoehe = HoehenWert - 0; // Parameter_MaxHoehe ist der PPM-Wert des Schalters |
939 | // Salvo End |
977 | // Salvo End |
940 | HoehenReglerAktiv = 0; |
978 | HoehenReglerAktiv = 0; |
941 | } |
979 | } |
942 | else |
980 | else |
943 | HoehenReglerAktiv = 1; |
981 | HoehenReglerAktiv = 1; |