Subversion Repositories FlightCtrl

Rev

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

Rev 1943 Rev 1944
Line 1037... Line 1037...
1037 
//DebugOut.Analog[18] = (Mittelwert_AccRoll-FromNaviCtrl.AccErrorR) / 4;
 1037 
//DebugOut.Analog[18] = (Mittelwert_AccRoll-FromNaviCtrl.AccErrorR) / 4;
1038 
//DebugOut.Analog[19] = -FromNaviCtrl.AccErrorR / 4;
 1038 
//DebugOut.Analog[19] = -FromNaviCtrl.AccErrorR / 4;
Line 1039... Line 1039...
1039 
 
 1039 
 
1040 
   if(FromNaviCtrl_Value.Kalman_K > 0 /*&& !TrichterFlug*/)
 1040 
   if(FromNaviCtrl_Value.Kalman_K > 0 /*&& !TrichterFlug*/)
1041 
     {
 1041 
     {
1042 
      tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccNick - FromNaviCtrl.AccErrorN));
 1042 
                tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccNick - FromNaviCtrl.AccErrorN));
1043 
      tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccRoll - FromNaviCtrl.AccErrorR));
 1043 
                tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccRoll - FromNaviCtrl.AccErrorR));
1044 
      tmp_long  = (tmp_long  * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
 1044 
                tmp_long  = (tmp_long  * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
1045 
      tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
 - 
 
1046 
          KompassFusion = FromNaviCtrl_Value.Kalman_K;
 1045 
                tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
1047 
     if((MaxStickNick > 64) || (MaxStickRoll > 64))
 1046 
                if((MaxStickNick > 64) || (MaxStickRoll > 64))
1048 
      {
 1047 
                {
1049 
      tmp_long  /= 2;
 1048 
                tmp_long  /= 2;
1050 
      tmp_long2 /= 2;
 1049 
                tmp_long2 /= 2;
1051 
      }
 1050 
                }
1052 
     if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
 1051 
                if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
1053 
      {
 1052 
                {
1054 
      tmp_long  /= 3;
 1053 
                tmp_long  /= 3;
1055 
      tmp_long2 /= 3;
 1054 
                tmp_long2 /= 3;
1056 
      }
 1055 
                }
1057 
      if(tmp_long >  (long) FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
 1056 
                if(tmp_long >  (long) FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
1058 
      if(tmp_long <  (long)-FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
 1057 
                if(tmp_long <  (long)-FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
1059 
      if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
 1058 
                if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
1060 
      if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
 1059 
                if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
1061 
     }
 1060 
     }
1062 
     else
 1061 
     else
1063 
     {
 1062 
     {
1064 
      tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
 1063 
                tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
1065 
      tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
 1064 
                tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
1066 
      tmp_long /= 16;
 1065 
                tmp_long /= 16;
1067 
      tmp_long2 /= 16;
 1066 
                tmp_long2 /= 16;
1068 
     if((MaxStickNick > 64) || (MaxStickRoll > 64))
 1067 
                if((MaxStickNick > 64) || (MaxStickRoll > 64))
1069 
      {
 1068 
                {
1070 
      tmp_long  /= 3;
 1069 
                tmp_long  /= 3;
1071 
      tmp_long2 /= 3;
 1070 
                tmp_long2 /= 3;
1072 
      }
 1071 
                }
1073 
     if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
 1072 
                if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
1074 
      {
 1073 
                {
1075 
      tmp_long  /= 3;
 1074 
                tmp_long  /= 3;
1076 
      tmp_long2 /= 3;
 1075 
                tmp_long2 /= 3;
1077 
      }
 1076 
                }
1078 
      KompassFusion = 25;
 1077 
                KompassFusion = 25;
1079 
#define AUSGLEICH  32
 1078 
#define AUSGLEICH  32
1080 
      if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
 1079 
                if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
1081 
      if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
 1080 
                if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
1082 
      if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
 1081 
                if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
1083 
      if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
 1082 
                if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
Line 1084... Line 1083...
1084 
     }
 1083 
     }
1085 
 
 1084 
 
1086 
   Mess_IntegralNick -= tmp_long;
 1085 
   Mess_IntegralNick -= tmp_long;
Line 1246... Line 1245...
1246 
      if(!(Parameter_GlobalConfig & CFG_KOMPASS_FIX))
 1245 
      if(!(Parameter_GlobalConfig & CFG_KOMPASS_FIX))
1247 
       {
 1246 
       {
1248 
         NeueKompassRichtungMerken = 50; // eine Sekunde zum Einloggen
 1247 
         NeueKompassRichtungMerken = 50; // eine Sekunde zum Einloggen
1249 
        };
 1248 
        };
1250 
     }
 1249 
     }
1251 
    tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
 1250 
    tmp_int  = (long) EE_Parameter.StickGier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
1252 
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
 1251 
    tmp_int += (EE_Parameter.StickGier_P * StickGier) / 4;
1253 
        tmp_int += CompassGierSetpoint;
 1252 
        tmp_int += CompassGierSetpoint;
1254 
    sollGier = tmp_int;
 1253 
    sollGier = tmp_int;
1255 
    Mess_Integral_Gier -= tmp_int;
 1254 
    Mess_Integral_Gier -= tmp_int;
1256 
    if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
 1255 
    if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
1257 
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
 1256 
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
Line 1283... Line 1282...
1283 
        GierGyroFehler += fehler;
 1282 
        GierGyroFehler += fehler;
1284 
        if(NeueKompassRichtungMerken)
 1283 
        if(NeueKompassRichtungMerken)
1285 
         {
 1284 
         {
1286 
          if(--NeueKompassRichtungMerken == 0)
 1285 
          if(--NeueKompassRichtungMerken == 0)
1287 
                   {
 1286 
                   {
1288 
//-->       ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
 - 
 
1289 
            KompassSollWert = ErsatzKompassInGrad;
 1287 
            KompassSollWert = ErsatzKompassInGrad;
1290 
                   }   
 1288 
                   }   
1291 
         }
 1289 
         }
1292 
        }
 1290 
        }
1293 
       // Kompass fusionieren
 1291 
       // Kompass fusionieren
1294 
       if(!KompassSignalSchlecht) ErsatzKompass += (fehler * KompassFusion) / korrektur;
 1292 
       if(!KompassSignalSchlecht) ErsatzKompass += (fehler * KompassFusion) / korrektur;
1295 
 
 - 
 
- 
 
 1293 
DebugOut.Analog[16] = KompassFusion;
1296 
       // MK Gieren
 1294 
       // MK Gieren
1297 
           if(!NeueKompassRichtungMerken)
 1295 
           if(!NeueKompassRichtungMerken)
1298 
       {
 1296 
       {
1299 
           r = ((540 + (KompassSollWert - ErsatzKompassInGrad)) % 360) - 180;
 1297 
           r = ((540 + (KompassSollWert - ErsatzKompassInGrad)) % 360) - 180;
1300 
           v = r * (Parameter_KompassWirkung/2);  // nach Kompass ausrichten
 1298 
           v = r * (Parameter_KompassWirkung/2);  // nach Kompass ausrichten
Line 1726... Line 1724...
1726 
  if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
 1724 
  if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
Line 1727... Line 1725...
1727 
 
 1725 
 
1728 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1726 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1729 
// all BL-Ctrl connected?
 1727 
// all BL-Ctrl connected?
1730 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1728 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1731 
  if(MissingMotor || Capacity.MinOfMaxPWM != 255)                      // wait until all BL-Ctrls started
 1729 
  if(MissingMotor || Capacity.MinOfMaxPWM != 255 || NC_ErrorCode)      // wait until all BL-Ctrls started and no Errors
1732 
  if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)    // only during start-phase
 1730 
  if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)    // only during start-phase
1733 
   {
 1731 
   {
1734 
    modell_fliegt = 1;
 1732 
    modell_fliegt = 1;
1735 
        GasMischanteil = (MIN_GAS + 10) * STICK_GAIN;
 1733 
        GasMischanteil = (MIN_GAS + 10) * STICK_GAIN;