Subversion Repositories FlightCtrl

Rev

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 
//############################################################################