Rev 1374 | Rev 1391 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1374 | Rev 1377 | ||
---|---|---|---|
Line 90... | Line 90... | ||
90 | int ErsatzKompassInGrad; // Kompasswert in Grad |
90 | int ErsatzKompassInGrad; // Kompasswert in Grad |
91 | int GierGyroFehler = 0; |
91 | int GierGyroFehler = 0; |
92 | char GyroFaktor,GyroFaktorGier; |
92 | char GyroFaktor,GyroFaktorGier; |
93 | char IntegralFaktor,IntegralFaktorGier; |
93 | char IntegralFaktor,IntegralFaktorGier; |
94 | int DiffNick,DiffRoll; |
94 | int DiffNick,DiffRoll; |
95 | int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
95 | //int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
- | 96 | unsigned char Poti[9] = {0,0,0,0,0,0,0,0}; |
|
96 | volatile unsigned char SenderOkay = 0; |
97 | volatile unsigned char SenderOkay = 0; |
97 | volatile unsigned char SenderRSSI = 0; |
98 | volatile unsigned char SenderRSSI = 0; |
98 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
99 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
99 | char MotorenEin = 0; |
100 | char MotorenEin = 0; |
100 | long HoehenWert = 0; |
101 | long HoehenWert = 0; |
Line 261... | Line 262... | ||
261 | LED_Init(); |
262 | LED_Init(); |
262 | MikroKopterFlags |= FLAG_CALIBRATE; |
263 | MikroKopterFlags |= FLAG_CALIBRATE; |
263 | FromNaviCtrl_Value.Kalman_K = -1; |
264 | FromNaviCtrl_Value.Kalman_K = -1; |
264 | FromNaviCtrl_Value.Kalman_MaxDrift = 0; |
265 | FromNaviCtrl_Value.Kalman_MaxDrift = 0; |
265 | FromNaviCtrl_Value.Kalman_MaxFusion = 32; |
266 | FromNaviCtrl_Value.Kalman_MaxFusion = 32; |
266 | Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110; |
- | |
- | 267 | ||
267 | Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110; |
268 | for(i=0;i<8;i++) |
- | 269 | { |
|
268 | Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110; |
270 | Poti[i] = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 110; |
269 | Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110; |
- | |
- | 271 | } |
|
270 | SenderOkay = 100; |
272 | SenderOkay = 100; |
271 | if(ServoActive) |
273 | if(ServoActive) |
272 | { |
274 | { |
273 | HEF4017R_ON; |
275 | HEF4017R_ON; |
274 | DDRD |=0x80; // enable J7 -> Servo signal |
276 | DDRD |=0x80; // enable J7 -> Servo signal |
Line 281... | Line 283... | ||
281 | //############################################################################ |
283 | //############################################################################ |
282 | { |
284 | { |
283 | static signed long tmpl,tmpl2,tmpl3,tmpl4; |
285 | static signed long tmpl,tmpl2,tmpl3,tmpl4; |
284 | static signed int oldNick, oldRoll, d2Roll, d2Nick; |
286 | static signed int oldNick, oldRoll, d2Roll, d2Nick; |
285 | signed long winkel_nick, winkel_roll; |
287 | signed long winkel_nick, winkel_roll; |
286 | - | ||
- | 288 | unsigned char i; |
|
287 | MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
289 | MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
288 | // MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier; |
290 | // MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier; |
289 | MesswertNick = (signed int) AdWertNickFilter / 8; |
291 | MesswertNick = (signed int) AdWertNickFilter / 8; |
290 | MesswertRoll = (signed int) AdWertRollFilter / 8; |
292 | MesswertRoll = (signed int) AdWertRollFilter / 8; |
291 | RohMesswertNick = MesswertNick; |
293 | RohMesswertNick = MesswertNick; |
Line 422... | Line 424... | ||
422 | if(RohMesswertNick > 256) MesswertNick += 1 * (RohMesswertNick - 256); |
424 | if(RohMesswertNick > 256) MesswertNick += 1 * (RohMesswertNick - 256); |
423 | else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256); |
425 | else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256); |
424 | if(RohMesswertRoll > 256) MesswertRoll += 1 * (RohMesswertRoll - 256); |
426 | if(RohMesswertRoll > 256) MesswertRoll += 1 * (RohMesswertRoll - 256); |
425 | else if(RohMesswertRoll < -256) MesswertRoll += 1 * (RohMesswertRoll + 256); |
427 | else if(RohMesswertRoll < -256) MesswertRoll += 1 * (RohMesswertRoll + 256); |
426 | } |
428 | } |
- | 429 | for(i=0;i<8;i++) |
|
427 | 430 | { |
|
428 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
- | |
429 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
- | |
430 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
- | |
431 | if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
- | |
432 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
431 | int tmp; |
433 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
432 | tmp = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 110; |
434 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
433 | if(tmp > 255) tmp = 255; else if(tmp < 0) tmp = 0; |
435 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
434 | if(Poti[i] > tmp) Poti[i]--; else if(Poti[i] < tmp) Poti[i]++; |
- | 435 | } |
|
436 | } |
436 | } |
Line 437... | Line 437... | ||
437 | 437 | ||
438 | //############################################################################ |
438 | //############################################################################ |
439 | // Messwerte beim Ermitteln der Nullage |
439 | // Messwerte beim Ermitteln der Nullage |
440 | void CalibrierMittelwert(void) |
440 | void CalibrierMittelwert(void) |
441 | //############################################################################ |
441 | //############################################################################ |
- | 442 | { |
|
442 | { |
443 | unsigned char i; |
443 | if(PlatinenVersion == 13) SucheGyroOffset(); |
444 | if(PlatinenVersion == 13) SucheGyroOffset(); |
444 | // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
445 | // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
445 | ANALOG_OFF; |
446 | ANALOG_OFF; |
446 | MesswertNick = AdWertNick; |
447 | MesswertNick = AdWertNick; |
Line 449... | Line 450... | ||
449 | Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
450 | Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
450 | Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
451 | Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
451 | Mittelwert_AccHoch = (long)AdWertAccHoch; |
452 | Mittelwert_AccHoch = (long)AdWertAccHoch; |
452 | // ADC einschalten |
453 | // ADC einschalten |
453 | ANALOG_ON; |
454 | ANALOG_ON; |
454 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
- | |
455 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
- | |
456 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
- | |
457 | if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
- | |
458 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
455 | for(i=0;i<8;i++) |
- | 456 | { |
|
- | 457 | int tmp; |
|
459 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
458 | tmp = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 110; |
460 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
459 | if(tmp > 255) tmp = 255; else if(tmp < 0) tmp = 0; |
461 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
460 | if(Poti[i] > tmp) Poti[i]--; else if(Poti[i] < tmp) Poti[i]++; |
462 | 461 | } |
|
463 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
462 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
464 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
463 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
465 | } |
464 | } |
Line 466... | Line 465... | ||
466 | 465 | ||
Line 498... | Line 497... | ||
498 | //############################################################################ |
497 | //############################################################################ |
499 | // Trägt ggf. das Poti als Parameter ein |
498 | // Trägt ggf. das Poti als Parameter ein |
500 | void ParameterZuordnung(void) |
499 | void ParameterZuordnung(void) |
501 | //############################################################################ |
500 | //############################################################################ |
502 | { |
501 | { |
503 | #define CHK_POTI_MM(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;} |
502 | // #define CHK_POTI(b,a) {if(a < 247) b = a; else switch(a) { case 248: b = Poti8; break; case 249: b = Poti7; break; case 250: b = Poti6; break; case 251: b = Poti5; break; case 252: b = Poti4; break; case 253: b = Poti3; break; case 254: b = Poti2; break; case 255: b = Poti1; break; };} |
- | 503 | #define CHK_POTI(b,a) {if(a < 247) b = a; else b = Poti[255 - a];} |
|
- | 504 | #define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); if(b <= min) b = min; else if(b >= max) b = max;} |
|
504 | #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; } |
505 | // #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; } |
505 | CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255); |
- | |
- | 506 | ||
506 | CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100); |
507 | CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100); |
507 | CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100); |
508 | CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100); |
508 | CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255); |
- | |
509 | CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z,0,255); |
- | |
510 | CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255); |
- | |
511 | CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255); |
509 | CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255); |
512 | CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255); |
- | |
513 | CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,0,255); |
- | |
514 | CHK_POTI(Parameter_Gyro_Gier_P,EE_Parameter.Gyro_Gier_P,10,255); |
- | |
515 | CHK_POTI(Parameter_Gyro_Gier_I,EE_Parameter.Gyro_Gier_I,0,255); |
- | |
516 | CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255); |
- | |
517 | CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255); |
- | |
518 | CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255); |
- | |
519 | CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255); |
- | |
520 | CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255); |
- | |
521 | CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255); |
- | |
522 | CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255); |
- | |
523 | CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255); |
- | |
524 | CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255); |
- | |
525 | CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
- | |
526 | CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl,0,255); |
- | |
527 | CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255); |
- | |
528 | CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255); |
- | |
529 | CHK_POTI(Parameter_AchsKopplung2, EE_Parameter.AchsKopplung2,0,255); |
- | |
530 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection,0,255); |
- | |
531 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
- | |
532 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255); |
- | |
533 | CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255); |
510 | CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255); |
534 | CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
511 | CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
- | 512 | ||
- | 513 | CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe); |
|
- | 514 | CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung); |
|
- | 515 | CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z); |
|
- | 516 | CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung); |
|
- | 517 | CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I); |
|
- | 518 | CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D); |
|
- | 519 | CHK_POTI(Parameter_Gyro_Gier_P,EE_Parameter.Gyro_Gier_P); |
|
- | 520 | CHK_POTI(Parameter_Gyro_Gier_I,EE_Parameter.Gyro_Gier_I); |
|
- | 521 | CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor); |
|
- | 522 | CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1); |
|
- | 523 | CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2); |
|
- | 524 | CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3); |
|
- | 525 | CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4); |
|
- | 526 | CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5); |
|
- | 527 | CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6); |
|
- | 528 | CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7); |
|
- | 529 | CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8); |
|
- | 530 | CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl); |
|
- | 531 | CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl); |
|
- | 532 | CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit); |
|
- | 533 | CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1); |
|
- | 534 | CHK_POTI(Parameter_AchsKopplung2, EE_Parameter.AchsKopplung2); |
|
- | 535 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection); |
|
- | 536 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
|
- | 537 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
|
535 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255); |
538 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
536 | Ki = 10300 / (Parameter_I_Faktor + 1); |
539 | Ki = 10300 / (Parameter_I_Faktor + 1); |
537 | MAX_GAS = EE_Parameter.Gas_Max; |
540 | MAX_GAS = EE_Parameter.Gas_Max; |
538 | MIN_GAS = EE_Parameter.Gas_Min; |
541 | MIN_GAS = EE_Parameter.Gas_Min; |
- | 542 | J5Low; |
|
- | 543 | sei(); |
|
539 | } |
544 | } |
Line 540... | Line -... | ||
540 | - | ||
541 | 545 | ||
542 | //############################################################################ |
546 | //############################################################################ |
543 | // |
547 | // |
544 | void MotorRegler(void) |
548 | void MotorRegler(void) |
545 | //############################################################################ |
549 | //############################################################################ |