Rev 189 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 189 | Rev 330 | ||
---|---|---|---|
Line 50... | Line 50... | ||
50 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
50 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
51 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
51 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
52 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
52 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
53 | // + POSSIBILITY OF SUCH DAMAGE. |
53 | // + POSSIBILITY OF SUCH DAMAGE. |
54 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
54 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 55 | // Aenderungen von Peter Muehlenbrock ("Salvo") Stand 6.10.2007 |
|
- | 56 | /* |
|
- | 57 | Driftkompensation fuer Gyros verbessert |
|
- | 58 | Linearsensor mit fixem Neutralwert |
|
- | 59 | Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion |
|
- | 60 | */ |
|
Line 55... | Line 61... | ||
55 | 61 | ||
Line 56... | Line 62... | ||
56 | #include "main.h" |
62 | #include "main.h" |
57 | 63 | ||
Line 73... | Line 79... | ||
73 | volatile int KompassStartwert = 0; |
79 | volatile int KompassStartwert = 0; |
74 | volatile int KompassRichtung = 0; |
80 | volatile int KompassRichtung = 0; |
75 | unsigned char MAX_GAS,MIN_GAS; |
81 | unsigned char MAX_GAS,MIN_GAS; |
76 | unsigned char Notlandung = 0; |
82 | unsigned char Notlandung = 0; |
77 | unsigned char HoehenReglerAktiv = 0; |
83 | unsigned char HoehenReglerAktiv = 0; |
- | 84 | //Salvo 12.10.2007 |
|
- | 85 | uint8_t magkompass_ok=0; |
|
- | 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 |
|
- | 89 | //Salvo End |
|
78 | 90 | ||
- | 91 | //Salvo 2.9.2007 Ersatzkompass |
|
- | 92 | volatile long GyroKomp_Int,GyroKomp_Int2; |
|
- | 93 | volatile int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass |
|
- | 94 | // Salvo End |
|
79 | float GyroFaktor; |
95 | float GyroFaktor; |
80 | float IntegralFaktor; |
96 | float IntegralFaktor; |
Line 81... | Line 97... | ||
81 | 97 | ||
82 | volatile int DiffNick,DiffRoll; |
98 | volatile int DiffNick,DiffRoll; |
Line 88... | Line 104... | ||
88 | char MotorenEin = 0; |
104 | char MotorenEin = 0; |
89 | int HoehenWert = 0; |
105 | int HoehenWert = 0; |
90 | int SollHoehe = 0; |
106 | int SollHoehe = 0; |
91 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
107 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
Line -... | Line 108... | ||
- | 108 | ||
- | 109 | ||
- | 110 | int w,v; |
|
92 | 111 | ||
93 | float Kp = FAKTOR_P; |
112 | float Kp = FAKTOR_P; |
Line 94... | Line 113... | ||
94 | float Ki = FAKTOR_I; |
113 | float Ki = FAKTOR_I; |
95 | 114 | ||
Line 130... | Line 149... | ||
130 | NeutralAccY = 0; |
149 | NeutralAccY = 0; |
131 | NeutralAccZ = 0; |
150 | NeutralAccZ = 0; |
132 | AdNeutralNick = 0; |
151 | AdNeutralNick = 0; |
133 | AdNeutralRoll = 0; |
152 | AdNeutralRoll = 0; |
134 | AdNeutralGier = 0; |
153 | AdNeutralGier = 0; |
- | 154 | GPS_Neutral(); |
|
135 | CalibrierMittelwert(); |
155 | CalibrierMittelwert(); |
136 | timer = SetDelay(5); |
156 | timer = SetDelay(5); |
137 | while (!CheckDelay(timer)); |
157 | while (!CheckDelay(timer)); |
138 | CalibrierMittelwert(); |
158 | CalibrierMittelwert(); |
139 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
159 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
Line 151... | Line 171... | ||
151 | { |
171 | { |
152 | AdNeutralNick= abs(MesswertNick) / 2; |
172 | AdNeutralNick= abs(MesswertNick) / 2; |
153 | AdNeutralRoll= abs(MesswertRoll) / 2; |
173 | AdNeutralRoll= abs(MesswertRoll) / 2; |
154 | AdNeutralGier= abs(MesswertGier) / 2; |
174 | AdNeutralGier= abs(MesswertGier) / 2; |
155 | } |
175 | } |
- | 176 | // Salvo 1.9.2007 ACC mit festen neutralwerten ***** |
|
- | 177 | if (ACC_NEUTRAL_FIXED > 0) |
|
- | 178 | { |
|
- | 179 | NeutralAccX = ACC_NICK_NEUTRAL; |
|
- | 180 | NeutralAccY = ACC_ROLL_NEUTRAL; |
|
- | 181 | } |
|
- | 182 | else |
|
- | 183 | { // Automatisch bei Offsetabgleich ermitteln |
|
156 | NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
184 | NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY; |
157 | NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY; |
185 | NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
- | 186 | } |
|
- | 187 | // Salvo End |
|
158 | NeutralAccZ = Aktuell_az; |
188 | NeutralAccZ = Aktuell_az; |
Line 159... | Line 189... | ||
159 | 189 | ||
160 | Mess_IntegralNick = 0; |
190 | Mess_IntegralNick = 0; |
161 | Mess_IntegralNick2 = 0; |
191 | Mess_IntegralNick2 = 0; |
Line 167... | Line 197... | ||
167 | MesswertGier = 0; |
197 | MesswertGier = 0; |
168 | StartLuftdruck = Luftdruck; |
198 | StartLuftdruck = Luftdruck; |
169 | HoeheD = 0; |
199 | HoeheD = 0; |
170 | Mess_Integral_Hoch = 0; |
200 | Mess_Integral_Hoch = 0; |
171 | KompassStartwert = KompassValue; |
201 | KompassStartwert = KompassValue; |
172 | GPS_Neutral(); |
- | |
- | 202 | ||
173 | beeptime = 50; |
203 | beeptime = 50; |
- | 204 | //Salvo 13.10.2007 Ersatzkompass |
|
- | 205 | GyroKomp_Int = 0; |
|
- | 206 | gas_mittel = 30; |
|
- | 207 | gas_actual = gas_mittel; |
|
- | 208 | // Salvo End |
|
174 | } |
209 | } |
Line 175... | Line 210... | ||
175 | 210 | ||
176 | //############################################################################ |
211 | //############################################################################ |
177 | // Bildet den Mittelwert aus den Messwerten |
212 | // Bildet den Mittelwert aus den Messwerten |
Line 302... | Line 337... | ||
302 | EE_Parameter.MaxHoehe = 251; // Wert : 0-32 251 -> Poti1 |
337 | EE_Parameter.MaxHoehe = 251; // Wert : 0-32 251 -> Poti1 |
303 | EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
338 | EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
304 | EE_Parameter.Luftdruck_D = 50; // Wert : 0-250 |
339 | EE_Parameter.Luftdruck_D = 50; // Wert : 0-250 |
305 | EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250 |
340 | EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250 |
306 | EE_Parameter.Hoehe_Verstaerkung = 4; // Wert : 0-50 |
341 | EE_Parameter.Hoehe_Verstaerkung = 4; // Wert : 0-50 |
307 | EE_Parameter.Stick_P = 4; //2 // Wert : 1-6 |
342 | EE_Parameter.Stick_P = 2; //2 // Wert : 1-6 |
308 | EE_Parameter.Stick_D = 8; //8 // Wert : 0-64 |
343 | EE_Parameter.Stick_D = 4; //8 // Wert : 0-64 |
309 | EE_Parameter.Gier_P = 16; // Wert : 1-20 |
344 | EE_Parameter.Gier_P = 16; // Wert : 1-20 |
310 | EE_Parameter.Gas_Min = 15; // Wert : 0-32 |
345 | EE_Parameter.Gas_Min = 15; // Wert : 0-32 |
311 | EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
346 | EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
312 | EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
347 | EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
313 | EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
348 | EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
314 | EE_Parameter.Gyro_P = 120; //80 // Wert : 0-250 |
349 | EE_Parameter.Gyro_P = 120; //80 // Wert : 0-250 |
315 | EE_Parameter.Gyro_I = 150; // Wert : 0-250 |
350 | EE_Parameter.Gyro_I = 150; // Wert : 0-250 |
316 | EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250 |
351 | EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250 |
317 | EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
352 | EE_Parameter.NotGas = 100; // Wert : 0-250 // Gaswert bei Empangsverlust |
318 | EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
353 | EE_Parameter.NotGasZeit = 60; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
319 | EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
354 | EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
320 | EE_Parameter.I_Faktor = 0; |
355 | EE_Parameter.I_Faktor = 0; |
321 | EE_Parameter.UserParam1 = 0; //zur freien Verwendung |
356 | EE_Parameter.UserParam1 = 8; //zur freien Verwendung |
322 | EE_Parameter.UserParam2 = 0; //zur freien Verwendung |
357 | EE_Parameter.UserParam2 = 2; //zur freien Verwendung |
323 | EE_Parameter.UserParam3 = 0; //zur freien Verwendung |
358 | EE_Parameter.UserParam3 = 12; //zur freien Verwendung |
324 | EE_Parameter.UserParam4 = 0; //zur freien Verwendung |
359 | EE_Parameter.UserParam4 = 0; //zur freien Verwendung |
325 | EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
360 | EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
326 | EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
361 | EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
327 | EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
362 | EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
328 | EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
363 | EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
Line 349... | Line 384... | ||
349 | EE_Parameter.MaxHoehe = 251; // Wert : 0-32 251 -> Poti1 |
384 | EE_Parameter.MaxHoehe = 251; // Wert : 0-32 251 -> Poti1 |
350 | EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
385 | EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
351 | EE_Parameter.Luftdruck_D = 50; // Wert : 0-250 |
386 | EE_Parameter.Luftdruck_D = 50; // Wert : 0-250 |
352 | EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250 |
387 | EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250 |
353 | EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50 |
388 | EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50 |
354 | EE_Parameter.Stick_P = 4; //2 // Wert : 1-6 |
389 | EE_Parameter.Stick_P = 2; //2 // Wert : 1-6 |
355 | EE_Parameter.Stick_D = 0; //8 // Wert : 0-64 |
390 | EE_Parameter.Stick_D = 0; //8 // Wert : 0-64 |
356 | EE_Parameter.Gier_P = 16; // Wert : 1-20 |
391 | EE_Parameter.Gier_P = 16; // Wert : 1-20 |
357 | EE_Parameter.Gas_Min = 15; // Wert : 0-32 |
392 | EE_Parameter.Gas_Min = 15; // Wert : 0-32 |
358 | EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
393 | EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
359 | EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
394 | EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
360 | EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
395 | EE_Parameter.KompassWirkung = 64; // Wert : 0-250 |
361 | EE_Parameter.Gyro_P = 175; //80 // Wert : 0-250 |
396 | EE_Parameter.Gyro_P = 175; //80 // Wert : 0-250 |
362 | EE_Parameter.Gyro_I = 175; // Wert : 0-250 |
397 | EE_Parameter.Gyro_I = 175; // Wert : 0-250 |
363 | EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250 |
398 | EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250 |
364 | EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
399 | EE_Parameter.NotGas = 100; // Wert : 0-250 // Gaswert bei Empangsverlust |
365 | EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
400 | EE_Parameter.NotGasZeit = 60; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
366 | EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
401 | EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
367 | EE_Parameter.I_Faktor = 0; |
402 | EE_Parameter.I_Faktor = 5; |
368 | EE_Parameter.UserParam1 = 0; //zur freien Verwendung |
403 | EE_Parameter.UserParam1 = 12; //zur freien Verwendung |
369 | EE_Parameter.UserParam2 = 0; //zur freien Verwendung |
404 | EE_Parameter.UserParam2 = 2; //zur freien Verwendung |
370 | EE_Parameter.UserParam3 = 0; //zur freien Verwendung |
405 | EE_Parameter.UserParam3 = 16; //zur freien Verwendung |
371 | EE_Parameter.UserParam4 = 0; //zur freien Verwendung |
406 | EE_Parameter.UserParam4 = 0; //zur freien Verwendung |
372 | EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
407 | EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
373 | EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
408 | EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
374 | EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
409 | EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
375 | EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
410 | EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
Line 429... | Line 464... | ||
429 | static unsigned int modell_fliegt = 0; |
464 | static unsigned int modell_fliegt = 0; |
430 | static int hoehenregler = 0; |
465 | static int hoehenregler = 0; |
431 | static char TimerWerteausgabe = 0; |
466 | static char TimerWerteausgabe = 0; |
432 | static char NeueKompassRichtungMerken = 0; |
467 | static char NeueKompassRichtungMerken = 0; |
433 | Mittelwert(); |
468 | Mittelwert(); |
- | 469 | //****** GPS Daten holen *************** |
|
- | 470 | short int n; |
|
- | 471 | if (gps_alive_cnt > 0) gps_alive_cnt--; //Dekrementieren. Wenn 0 kommen keine ausreichenden GPS Meldungen (Timeout) |
|
- | 472 | n = Get_Rel_Position(); |
|
- | 473 | if (n == 0) |
|
434 | 474 | { |
|
- | 475 | ROT_ON; //led blitzen lassen |
|
- | 476 | } |
|
- | 477 | //******PROVISORISCH*************** |
|
435 | GRN_ON; |
478 | GRN_ON; |
Line 436... | Line 479... | ||
436 | 479 | ||
437 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
480 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
438 | // Gaswert ermitteln |
481 | // Gaswert ermitteln |
439 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
482 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 483 | GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
|
- | 484 | //Salvo 13.10.2007 langsame Gasreduktion bei Unterspannung. Als Ausgangswert wird der bei UBAT=k gemessen Mittelwert genommen |
|
- | 485 | // und dieser dann langsam zwangsweise reduziert |
|
- | 486 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
|
- | 487 | if (UBat <= EE_Parameter.UnterspannungsWarnung - 2) //Unterhalb der Piepser Schwelle aktivieren |
|
- | 488 | { |
|
- | 489 | if (ubat_cnt > 700) |
|
- | 490 | { |
|
- | 491 | ubat_cnt = 0; |
|
- | 492 | if (gas_actual > ((gas_mittel*12)/15)) gas_actual--; |
|
- | 493 | } |
|
- | 494 | else ubat_cnt++; |
|
- | 495 | if (GasMischanteil > gas_actual) GasMischanteil = gas_actual; |
|
- | 496 | } |
|
- | 497 | else //Falls UBAT wieder ok ist |
|
- | 498 | { |
|
- | 499 | if (ubat_cnt > 1000) |
|
- | 500 | { |
|
- | 501 | gas_mittel = ((gas_mittel*9) + GasMischanteil)/10; //Filtern |
|
- | 502 | gas_actual = GasMischanteil; |
|
- | 503 | } |
|
- | 504 | else |
|
- | 505 | { |
|
- | 506 | ubat_cnt++; |
|
- | 507 | if ((ubat_cnt % 10) == 0) |
|
- | 508 | { |
|
- | 509 | if (gas_actual < GasMischanteil) gas_actual++; |
|
- | 510 | else gas_actual = GasMischanteil; |
|
- | 511 | } |
|
- | 512 | } |
|
- | 513 | GasMischanteil = gas_actual; |
|
- | 514 | } |
|
- | 515 | ANALOG_ON; // ADC einschalten |
|
440 | GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
516 | // Salvo End |
441 | if(GasMischanteil < 0) GasMischanteil = 0; |
517 | if(GasMischanteil < 0) GasMischanteil = 0; |
442 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
518 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
443 | // Emfang schlecht |
519 | // Empfang schlecht |
444 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
520 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
445 | if(SenderOkay < 100) |
521 | if(SenderOkay < 100) |
446 | { |
522 | { |
447 | if(!PcZugriff) |
523 | if(!PcZugriff) |
Line 497... | Line 573... | ||
497 | { |
573 | { |
498 | unsigned char setting; |
574 | unsigned char setting; |
499 | if(++delay_neutral > 200) // nicht sofort |
575 | if(++delay_neutral > 200) // nicht sofort |
500 | { |
576 | { |
501 | GRN_OFF; |
577 | GRN_OFF; |
- | 578 | SetNeutral(); |
|
502 | MotorenEin = 0; |
579 | MotorenEin = 0; |
503 | delay_neutral = 0; |
580 | delay_neutral = 0; |
504 | modell_fliegt = 0; |
581 | modell_fliegt = 0; |
505 | if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70) |
582 | if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70) |
506 | { |
583 | { |
Line 509... | Line 586... | ||
509 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3; |
586 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3; |
510 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4; |
587 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4; |
511 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5; |
588 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5; |
512 | eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken |
589 | eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); // aktiven Datensatz merken |
513 | } |
590 | } |
514 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
- | |
515 | { |
- | |
516 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
- | |
517 | } |
- | |
- | 591 | ||
- | 592 | ||
- | 593 | ||
- | 594 | ||
518 | ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
595 | ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE); |
519 | SetNeutral(); |
- | |
- | 596 | ||
520 | Piep(GetActiveParamSetNumber()); |
597 | Piep(GetActiveParamSetNumber()); |
- | 598 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
|
- | 599 | { |
|
- | 600 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
|
521 | } |
601 | } |
- | 602 | GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar |
|
- | 603 | if (gps_home_position.status > 0 ) |
|
- | 604 | { |
|
- | 605 | Delay_ms(1000); //akustisch verkuenden dass GPS Home Daten da sind |
|
- | 606 | beeptime = 2000; |
|
- | 607 | Delay_ms(500); |
|
- | 608 | } |
|
- | 609 | } |
|
522 | } |
610 | } |
523 | else delay_neutral = 0; |
611 | else delay_neutral = 0; |
524 | } |
612 | } |
525 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
613 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
526 | // Gas ist unten |
614 | // Gas ist unten |
Line 650... | Line 738... | ||
650 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
738 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
651 | // Gyro-Drift kompensieren |
739 | // Gyro-Drift kompensieren |
652 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
740 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
653 | #define DRIFT_FAKTOR 3 |
741 | #define DRIFT_FAKTOR 3 |
654 | if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
742 | if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
- | 743 | { |
|
- | 744 | // Salvo 12.9.2007 Ersatzkompass ******* |
|
- | 745 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
|
- | 746 | if (GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT >=360 ) GyroKomp_Int = 0; |
|
- | 747 | ANALOG_ON; // ADC einschalten |
|
- | 748 | ROT_OFF; |
|
655 | { |
749 | // Salvo End |
- | 750 | ||
656 | IntegralFehlerNick = IntegralNick2 - IntegralNick; |
751 | IntegralFehlerNick = IntegralNick2 - IntegralNick; |
657 | IntegralFehlerRoll = IntegralRoll2 - IntegralRoll; |
752 | IntegralFehlerRoll = IntegralRoll2 - IntegralRoll; |
658 | ZaehlMessungen = 0; |
753 | ZaehlMessungen = 0; |
- | 754 | // Salvo 1.9.2007 ************************* |
|
- | 755 | // Abgleich Roll und Nick Gyro vollsteandig nur, wenn nahezu waagrechte Lage, ansonsten abschwaechen |
|
- | 756 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
|
- | 757 | w = (abs(Mittelwert_AccNick)); |
|
- | 758 | v = (abs(Mittelwert_AccRoll)); |
|
- | 759 | ANALOG_ON; // ADC einschalten |
|
- | 760 | // Abgleich Roll und Nick Gyro nur wenn auch keine wilden Manoever stattfinden 26.9.2007 |
|
- | 761 | if ((abs(StickNick - GPS_Nick) < 30) && (abs(StickRoll - GPS_Roll) < 30)) |
|
- | 762 | { |
|
- | 763 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) |
|
- | 764 | { |
|
659 | if(IntegralFehlerNick > 500/DRIFT_FAKTOR) AdNeutralNick++; |
765 | if(IntegralFehlerNick > 500/DRIFT_FAKTOR) AdNeutralNick++; |
660 | if(IntegralFehlerNick < -500/DRIFT_FAKTOR) AdNeutralNick--; |
766 | if(IntegralFehlerNick < -500/DRIFT_FAKTOR) AdNeutralNick--; |
661 | if(IntegralFehlerRoll > 500/DRIFT_FAKTOR) AdNeutralRoll++; |
767 | if(IntegralFehlerRoll > 500/DRIFT_FAKTOR) AdNeutralRoll++; |
662 | if(IntegralFehlerRoll < -500/DRIFT_FAKTOR) AdNeutralRoll--; |
768 | if(IntegralFehlerRoll < -500/DRIFT_FAKTOR) AdNeutralRoll--; |
- | 769 | } |
|
- | 770 | else if ((w < 2*ACC_WAAGRECHT_LIMIT) && (v < 2*ACC_WAAGRECHT_LIMIT)) // langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht |
|
- | 771 | { |
|
- | 772 | if(IntegralFehlerNick > 500*2/DRIFT_FAKTOR) AdNeutralNick++; |
|
- | 773 | if(IntegralFehlerNick < -500*2/DRIFT_FAKTOR) AdNeutralNick--; |
|
- | 774 | if(IntegralFehlerRoll > 500*2/DRIFT_FAKTOR) AdNeutralRoll++; |
|
- | 775 | if(IntegralFehlerRoll < -500*2/DRIFT_FAKTOR) AdNeutralRoll--; |
|
- | 776 | } |
|
- | 777 | /* else // noch langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht |
|
- | 778 | { |
|
- | 779 | if(IntegralFehlerNick > 500*4/DRIFT_FAKTOR) AdNeutralNick++; |
|
663 | // if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; // macht nur mit Referenz (Kompass Sinn) |
780 | if(IntegralFehlerNick < -500*4/DRIFT_FAKTOR) AdNeutralNick--; |
- | 781 | if(IntegralFehlerRoll > 500*4/DRIFT_FAKTOR) AdNeutralRoll++; |
|
664 | // if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; // macht nur mit Referenz (Kompass Sinn) |
782 | if(IntegralFehlerRoll < -500*4/DRIFT_FAKTOR) AdNeutralRoll--; |
- | 783 | } |
|
- | 784 | */ } |
|
- | 785 | ||
- | 786 | // Salvo End |
|
- | 787 | ||
665 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
788 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
666 | Mess_IntegralNick2 = IntegralNick; |
789 | Mess_IntegralNick2 = IntegralNick; |
667 | Mess_IntegralRoll2 = IntegralRoll; |
790 | Mess_IntegralRoll2 = IntegralRoll; |
668 | Mess_Integral_Gier2 = Integral_Gier; |
791 | Mess_Integral_Gier2 = Integral_Gier; |
669 | ANALOG_ON; // ADC einschalten |
792 | ANALOG_ON; // ADC einschalten |
670 | } |
793 | } |
671 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
794 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
672 | // Integrale auf ACC-Signal abgleichen |
795 | // Integrale auf ACC-Signal abgleichen |
673 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
796 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
674 | if(IntegralFaktor && !Looping_Nick && !Looping_Roll) |
797 | if(IntegralFaktor && !Looping_Nick && !Looping_Roll) |
Line 690... | Line 813... | ||
690 | { |
813 | { |
691 | tmp_long = 0; |
814 | tmp_long = 0; |
692 | tmp_long2 = 0; |
815 | tmp_long2 = 0; |
693 | } |
816 | } |
694 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
817 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
- | 818 | // Salvo 1.9.2007 Volle Korrektur nur wenn waagrechte Lage, sonst abgeschawecht ***************** |
|
- | 819 | ||
- | 820 | w = (abs(Mittelwert_AccNick)); |
|
- | 821 | v = (abs(Mittelwert_AccRoll)); |
|
- | 822 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) |
|
- | 823 | { |
|
695 | Mess_IntegralNick -= tmp_long; |
824 | Mess_IntegralNick -= tmp_long; |
696 | Mess_IntegralRoll -= tmp_long2; |
825 | Mess_IntegralRoll -= tmp_long2; |
- | 826 | } |
|
- | 827 | else if ((w < 2 * ACC_WAAGRECHT_LIMIT) && (v < 2 * ACC_WAAGRECHT_LIMIT)) |
|
- | 828 | { |
|
- | 829 | Mess_IntegralNick -= tmp_long/2; //Vorher 8 |
|
- | 830 | Mess_IntegralRoll -= tmp_long2/2; |
|
- | 831 | } |
|
- | 832 | else if ((w < 4 * ACC_WAAGRECHT_LIMIT) && (v < 4 * ACC_WAAGRECHT_LIMIT)) |
|
- | 833 | { |
|
- | 834 | Mess_IntegralNick -= tmp_long/4; |
|
- | 835 | Mess_IntegralRoll -= tmp_long2/4; |
|
- | 836 | } |
|
- | 837 | else |
|
- | 838 | { |
|
- | 839 | Mess_IntegralNick -= tmp_long/8; |
|
- | 840 | Mess_IntegralRoll -= tmp_long2/8; |
|
- | 841 | } |
|
- | 842 | // Salvo End *********************** |
|
697 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
843 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
698 | // Gieren |
844 | // Gieren |
699 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
845 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
700 | if(abs(StickGier) > 20) // war 35 |
846 | if(abs(StickGier) > 20) // war 35 |
701 | { |
847 | { |
Line 706... | Line 852... | ||
706 | sollGier = tmp_int; |
852 | sollGier = tmp_int; |
707 | Mess_Integral_Gier -= tmp_int; |
853 | Mess_Integral_Gier -= tmp_int; |
708 | if(Mess_Integral_Gier > 25000) Mess_Integral_Gier = 25000; // begrenzen |
854 | if(Mess_Integral_Gier > 25000) Mess_Integral_Gier = 25000; // begrenzen |
709 | if(Mess_Integral_Gier <-25000) Mess_Integral_Gier =-25000; |
855 | if(Mess_Integral_Gier <-25000) Mess_Integral_Gier =-25000; |
Line -... | Line 856... | ||
- | 856 | ||
- | 857 | // Salvo Gewolltes Gieren ignorieren 30.8.2007 ********************** |
|
- | 858 | Mess_Integral_Gier2 -= tmp_int; |
|
710 | 859 | // Salvo End ************************* |
|
Line -... | Line 860... | ||
- | 860 | ANALOG_ON; // ADC einschalten |
|
- | 861 | ||
- | 862 | // Salvo Ersatzkompass 26.9.2007 ********************** |
|
- | 863 | if ((Kompass_Neuer_Wert > 0)) |
|
- | 864 | { |
|
- | 865 | Kompass_Neuer_Wert = 0; |
|
- | 866 | w = (abs(Mittelwert_AccNick)); |
|
- | 867 | v = (abs(Mittelwert_AccRoll)); |
|
- | 868 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass nur mit Magnetkompass aktualisieren wenn alle sok |
|
- | 869 | { |
|
- | 870 | if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen |
|
- | 871 | { |
|
- | 872 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
|
- | 873 | magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet |
|
- | 874 | GyroKomp_Int = (GyroKomp_Int )/GYROKOMP_INC_GRAD_DEFAULT; |
|
- | 875 | w = KompassValue - GyroKomp_Int; |
|
- | 876 | if ((w > 0) && (w < 180)) |
|
- | 877 | { |
|
- | 878 | ++GyroKomp_Int; |
|
- | 879 | } |
|
- | 880 | else if ((w > 0) && (w >= 180)) |
|
- | 881 | { |
|
- | 882 | --GyroKomp_Int; |
|
- | 883 | } |
|
- | 884 | else if ((w < 0) && (w >= -180)) |
|
- | 885 | { |
|
- | 886 | --GyroKomp_Int; |
|
- | 887 | } |
|
- | 888 | else if ((w < 0) && (w < -180)) |
|
- | 889 | { |
|
- | 890 | ++GyroKomp_Int; |
|
- | 891 | } |
|
- | 892 | if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + 360; |
|
- | 893 | GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern |
|
- | 894 | ANALOG_ON; // ADC einschalten |
|
- | 895 | } |
|
- | 896 | } |
|
- | 897 | else magkompass_ok = 0; |
|
- | 898 | } |
|
- | 899 | // Salvo End ************************* |
|
- | 900 | ||
- | 901 | // Salvo 6.10.2007 |
|
- | 902 | // GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist |
|
- | 903 | //GPS Hold Aktiveren wenn Knueppel in Ruhelage sind |
|
- | 904 | if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (abs(StickRoll) < GPS_STICK_HOLDOFF) && (abs(StickNick) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0)) |
|
- | 905 | { |
|
- | 906 | if (Parameter_MaxHoehe > 200) |
|
- | 907 | { |
|
- | 908 | if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
|
- | 909 | else gps_cmd = GPS_CMD_REQ_HOME; |
|
- | 910 | n = GPS_CRTL(gps_cmd); |
|
- | 911 | } |
|
- | 912 | else |
|
- | 913 | { |
|
- | 914 | if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet |
|
- | 915 | else gps_cmd = GPS_CMD_REQ_HOLD; |
|
- | 916 | n= GPS_CRTL(gps_cmd); |
|
- | 917 | } |
|
- | 918 | } |
|
711 | ANALOG_ON; // ADC einschalten |
919 | else (n= GPS_CRTL(GPS_CMD_STOP)); //GPS Lageregelung beenden |
712 | 920 | ||
713 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
921 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
714 | // Kompass |
922 | // Kompass |
715 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
923 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
716 | if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
- | |
717 | { |
- | |
718 | int w,v; |
- | |
719 | static int SignalSchlecht = 0; |
- | |
720 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
924 | if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (Kompass_present > 0)) |
- | 925 | { |
|
- | 926 | if(v > w) w = v; // grösste Neigung ermitteln |
|
721 | v = abs(IntegralRoll /512); |
927 | |
722 | if(v > w) w = v; // grösste Neigung ermitteln |
928 | // Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert |
723 | if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht) |
929 | if ((magkompass_ok > 0) && NeueKompassRichtungMerken) |
724 | { |
930 | { |
725 | KompassStartwert = KompassValue; |
931 | KompassStartwert = KompassValue; |
- | 932 | NeueKompassRichtungMerken = 0; |
|
- | 933 | } |
|
- | 934 | // Salvo 13.9.2007 |
|
726 | NeueKompassRichtungMerken = 0; |
935 | w=0; |
727 | } |
936 | // Salvo End |
728 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
937 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
729 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
938 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
730 | if(w > 0) |
939 | if(w > 0) |
- | 940 | { |
|
- | 941 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
|
731 | { |
942 | |
- | 943 | // Salvo Kompasssteuerung ********************** |
|
732 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
944 | if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
733 | if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
- | |
734 | ANALOG_ON; // ADC einschalten |
945 | // Salvo End |
735 | if(SignalSchlecht) SignalSchlecht--; |
- | |
- | 946 | ANALOG_ON; // ADC einschalten |
|
736 | } |
947 | } |
737 | else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
948 | |
Line 738... | Line 949... | ||
738 | } |
949 | } |
739 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
950 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 750... | Line 961... | ||
750 | DebugOut.Analog[3] = Mittelwert_AccRoll; |
961 | DebugOut.Analog[3] = Mittelwert_AccRoll; |
751 | DebugOut.Analog[4] = MesswertGier; |
962 | DebugOut.Analog[4] = MesswertGier; |
752 | DebugOut.Analog[5] = HoehenWert; |
963 | DebugOut.Analog[5] = HoehenWert; |
753 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
964 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
754 | DebugOut.Analog[8] = KompassValue; |
965 | DebugOut.Analog[8] = KompassValue; |
- | 966 | DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
|
Line 755... | Line 967... | ||
755 | 967 | ||
756 | /* DebugOut.Analog[16] = motor_rx[0]; |
968 | /* DebugOut.Analog[16] = motor_rx[0]; |
757 | DebugOut.Analog[17] = motor_rx[1]; |
969 | DebugOut.Analog[17] = motor_rx[1]; |
758 | DebugOut.Analog[18] = motor_rx[2]; |
970 | DebugOut.Analog[18] = motor_rx[2]; |