Subversion Repositories FlightCtrl

Rev

Rev 595 | Rev 614 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 595 Rev 604
Line 196... Line 196...
196 
    static signed long tmpl,tmpl2;     
 196 
    static signed long tmpl,tmpl2;     
197 
    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
 197 
    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
198 
    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
 198 
    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
199 
    MesswertNick = (signed int) AdWertNick - AdNeutralNick;
 199 
    MesswertNick = (signed int) AdWertNick - AdNeutralNick;
Line -... Line 200...
- 
 
 200 
 
- 
 
 201 
//DebugOut.Analog[26] = MesswertNick;
- 
 
 202 
DebugOut.Analog[28] = MesswertRoll;
200 
 
 203 
 
201 
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
 204 
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
202 
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
 205 
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
203 
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
 206 
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
204 
        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
 207 
        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
Line 576... Line 579...
576 
// neue Werte von der Funke
 579 
// neue Werte von der Funke
577 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
580 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
578 
 if(!NewPpmData-- || Notlandung)  
 581 
 if(!NewPpmData-- || Notlandung)  
579 
  {
 582 
  {
580 
    int tmp_int;
 583 
    int tmp_int;
- 
 
 584 
        static int stick_nick,stick_roll;
581 
    ParameterZuordnung();
 585 
    ParameterZuordnung();
582 
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
 586 
    StickNick = (StickNick * 7 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 8;
583 
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
 587 
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
584 
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
 588 
    StickRoll = (StickRoll * 7 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 8;
585 
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
 589 
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
- 
 
 590 
 
586 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
 591 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
587 
        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
 592 
        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
Line 588... Line 593...
588 
 
 593 
 
589 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
 594 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
Line 622... Line 627...
622 
    }
 627 
    }
Line 623... Line 628...
623 
 
 628 
 
624 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
 629 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
625 
    if(GyroFaktor < 0) GyroFaktor = 0;
 630 
    if(GyroFaktor < 0) GyroFaktor = 0;
- 
 
 631 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
626 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
 632 
/*
627 
    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
 633 
    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
628 
    if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
 634 
    if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
629 
     {
 635 
     {
630 
      if(IntegralNick >  60000)  
 636 
      if(IntegralNick >  60000)  
Line 648... Line 654...
648 
      {
 654 
      {
649 
       StickRoll += 8 * EE_Parameter.Stick_P;
 655 
       StickRoll += 8 * EE_Parameter.Stick_P;
650 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
 656 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
651 
      }
 657 
      }
652 
     }
 658 
     }
- 
 
 659 
*/
653 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
660 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
654 
// Looping?
 661 
// Looping?
655 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
662 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
656 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
 663 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
657 
  else
 664 
  else
Line 994... Line 1001...
994 
  }
 1001 
  }
Line 995... Line 1002...
995 
 
 1002 
 
996 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1003 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
997 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 1004 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
- 
 
 1005 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
- 
 
 1006 
//DebugOut.Analog[26] = MesswertNick;
- 
 
 1007 
//DebugOut.Analog[28] = MesswertRoll;
998 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1008 
 
999 
    if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
 1009 
    if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
1000 
    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
 1010 
    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
1001 
    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
 1011 
    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
1002 
    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
 - 
 
1003 
//    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
 1012 
    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
Line 1004... Line -...
1004 
    MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
 - 
 
1005 
 
 1013 
    MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
1006 
DebugOut.Analog[28] = MesswertRoll;
 1014 
 
- 
 
 1015 
DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
Line 1007... Line 1016...
1007 
DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
 1016 
DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
1008 
DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
 1017 
DebugOut.Analog[28] = MesswertRoll;
1009 
 
 1018 
 
1010 
    // Maximalwerte abfangen
 1019 
    // Maximalwerte abfangen
Line 1069... Line 1078...
1069 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1078 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1070 
// Gier-Anteil
 1079 
// Gier-Anteil
1071 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1080 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1072 
#define MUL_G  1.0
 1081 
#define MUL_G  1.0
1073 
    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
 1082 
    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
1074 
//GierMischanteil = 0;
 1083 
// GierMischanteil = 0;
Line 1075... Line 1084...
1075 
 
 1084 
 
1076 
    if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
 1085 
    if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
1077 
    if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
 1086 
    if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
1078 
    if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS - GasMischanteil));
 1087 
    if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS - GasMischanteil));
Line 1118... Line 1127...
1118 
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
 1127 
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1119 
    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
 1128 
    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
1120 
    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
 1129 
    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
1121 
    // Motor Links
 1130 
    // Motor Links
1122 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
 1131 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
- 
 
 1132 
#define GRENZE Poti1
- 
 
 1133 
 
1123 
        if ((motorwert < 0)) motorwert = 0;
 1134 
        if ((motorwert < 0)) motorwert = 0;
1124 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 1135 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
1125 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 1136 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
1126 
        Motor_Links = motorwert;               
 1137 
Motor_Links = motorwert;               
- 
 
 1138 
/*
- 
 
 1139 
if(motorwert > (int)Motor_Links + GRENZE)      Motor_Links += GRENZE;
- 
 
 1140 
else if(motorwert < (int)Motor_Links - GRENZE) Motor_Links -= GRENZE;
- 
 
 1141 
else Motor_Links = motorwert;          
- 
 
 1142 
*/
1127 
    // Motor Rechts
 1143 
    // Motor Rechts
1128 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
 1144 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
- 
 
 1145 
 
1129 
        if ((motorwert < 0)) motorwert = 0;
 1146 
        if ((motorwert < 0)) motorwert = 0;
1130 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 1147 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
1131 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
 1148 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
1132 
        Motor_Rechts = motorwert;
 1149 
Motor_Rechts = motorwert;              
- 
 
 1150 
/*
- 
 
 1151 
if(motorwert > Motor_Rechts + GRENZE)      Motor_Rechts += GRENZE;
- 
 
 1152 
else if(motorwert < Motor_Rechts - GRENZE) Motor_Rechts -= GRENZE;
- 
 
 1153 
else Motor_Rechts = motorwert;         
- 
 
 1154 
*/
1133 
   // +++++++++++++++++++++++++++++++++++++++++++++++
 1155 
   // +++++++++++++++++++++++++++++++++++++++++++++++
1134 
}
 1156 
}