Subversion Repositories FlightCtrl

Rev

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

Rev 2161 Rev 2165
Line 203... Line 203...
203
signed char cosinus, sinus; // MartinR : extern für PAN-Funktion
203
signed char cosinus, sinus; // MartinR : extern für PAN-Funktion
Line 204... Line 204...
204
 
204
 
205
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
205
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
206
//int MaxStickNick = 0,MaxStickRoll = 0; MartinR: so war es
206
//int MaxStickNick = 0,MaxStickRoll = 0; MartinR: so war es
-
 
207
int MaxStickNick = 0,MaxStickRoll = 0,stick_nick_neutral = 0,stick_roll_neutral = 0;  // MartinR: stick_.._neutral hinzugefügt
Line 207... Line 208...
207
int MaxStickNick = 0,MaxStickRoll = 0,stick_nick_neutral = 0,stick_roll_neutral = 0;  // MartinR: stick_.._neutral hinzugefügt
208
unsigned char stick_p;
208
 
209
 
209
unsigned int  modell_fliegt = 0;
210
unsigned int  modell_fliegt = 0;
210
volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0;
211
volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0;
Line 242... Line 243...
242
       
243
       
243
        DebugOut.Analog[16] = cosinus;  // MartinR: zur Einstellung der Pan-Funktion
244
        DebugOut.Analog[16] = cosinus;  // MartinR: zur Einstellung der Pan-Funktion
244
        DebugOut.Analog[17] = sinus;  // MartinR: test zur Einstellung der Pan-Funktion
245
        DebugOut.Analog[17] = sinus;  // MartinR: test zur Einstellung der Pan-Funktion
245
        DebugOut.Analog[18] = ServoPanValue;  // MartinR:  zur Einstellung der Pan-Funktion
246
        DebugOut.Analog[18] = ServoPanValue;  // MartinR:  zur Einstellung der Pan-Funktion
-
 
247
        DebugOut.Analog[19] = ServoRollValue;  // MartinR:  Test
246
        DebugOut.Analog[19] = ServoRollValue;  // MartinR:  Test
248
       
247
    DebugOut.Analog[20] = ServoNickValue;
249
    DebugOut.Analog[20] = ServoNickValue;
248
    DebugOut.Analog[22] = Capacity.ActualCurrent;
250
    DebugOut.Analog[22] = Capacity.ActualCurrent;
Line 249... Line 251...
249
#ifdef WITH_REMAINCAPACITY      // only include functions if DEBUG is defined in main.h
251
#ifdef WITH_REMAINCAPACITY      // only include functions if DEBUG is defined in main.h
Line 939... Line 941...
939
                                                                        modell_fliegt = 1;
941
                                                                        modell_fliegt = 1;
940
                                                                        MotorenEin = 1;
942
                                                                        MotorenEin = 1;
941
                                                                        sollGier = 0;
943
                                                                        sollGier = 0;
942
                                                                        Mess_Integral_Gier = 0;
944
                                                                        Mess_Integral_Gier = 0;
943
                                                                        // Mess_Integral_Gier2 = 0;
945
                                                                        // Mess_Integral_Gier2 = 0;
-
 
946
                                                                       
944
                                                                        Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
947
                                                                        Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
945
                                                                        Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
948
                                                                        Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
946
                                                                        Mess_IntegralNick2 = IntegralNick;
949
                                                                        Mess_IntegralNick2 = IntegralNick;
947
                                                                        Mess_IntegralRoll2 = IntegralRoll;
950
                                                                        Mess_IntegralRoll2 = IntegralRoll;
948
                                                                        SummeNick = 0;
951
                                                                        SummeNick = 0;
949
                                                                        SummeRoll = 0;
952
                                                                        SummeRoll = 0;
950
                                                                        FC_StatusFlags |= FC_STATUS_START;
953
                                                                        FC_StatusFlags |= FC_STATUS_START;
951
//                                                                      ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
954
//                                                                      ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
952
                                                                        NeueKompassRichtungMerken = 100; // 2 sekunden
955
                                                                        NeueKompassRichtungMerken = 100; // 2 sekunden
-
 
956
                                                                       
-
 
957
                                                                        // MartinR: hinzugefügt Anfang
-
 
958
                                                                        stick_nick_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; //  aktueller Stickwert wird als Neutralposition im HH verwendet, MartinR
-
 
959
                                                                        stick_roll_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; //  aktueller Stickwert wird als Neutralposition im HH verwendet, MartinR
-
 
960
                                                                        SummeNickHH = 0 ; // Zurücksetzen der Integratoren
-
 
961
                                                                        SummeRollHH = 0 ; // Zurücksetzen der Integratoren
-
 
962
                                                                        // MartinR: hinzugefügt Ende
953
                                                                }
963
                                                                }
954
                                                                else
964
                                                                else
955
                                                                {
965
                                                                {
956
                                                                        beeptime = 1500; // indicate missing calibration
966
                                                                        beeptime = 1500; // indicate missing calibration
957
                                                                }
967
                                                                }
Line 993... Line 1003...
993
    stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
1003
    stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
994
    stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4;
1004
    stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4;
995
    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
1005
    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
996
                        */
1006
                        */
997
// MartinR: geändert Anfang
1007
// MartinR: geändert Anfang
998
        if(Parameter_UserParam1 > 50)   // MartinR: zweiter Stick_P Wert nur, wenn HH über Schalter aktiv ist
-
 
999
                {
-
 
1000
                stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam3 - stick_nick_neutral) / 4;
-
 
1001
                stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam3 - stick_roll_neutral) / 4 ;
-
 
1002
                //stick_nick = (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam3 - stick_nick_neutral);
-
 
1003
                //stick_roll = (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam3 - stick_roll_neutral);
-
 
1004
                }
-
 
1005
               
-
 
1006
         else
-
 
1007
                {
-
 
1008
                stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * stick_p) / 4;
-
 
1009
                stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4;
-
 
1010
                stick_nick_neutral = stick_nick; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
-
 
1011
                stick_roll_neutral = stick_roll; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
-
 
1012
                }
-
 
Line 1013... Line 1008...
1013
       
1008
       
1014
         if(IntegralFaktor)  
1009
         if(IntegralFaktor)  // ACC-Mode
-
 
1010
                {
-
 
1011
                stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * stick_p) / 4;
1015
                {
1012
                stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4;
1016
                //stick_nick_neutral = stick_nick; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
1013
                stick_nick_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
1017
                //stick_roll_neutral = stick_roll; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
1014
                stick_roll_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
1018
                stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
1015
                stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
Line 1019... Line -...
1019
                stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
-
 
1020
               
-
 
1021
                //StickNick = stick_nick - (GPS_Nick + GPS_Nick2); // MartinR: GPS nur im ACC-Mode wirksam  
1016
                stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
1022
                //StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); // MartinR: GPS nur im ACC-Mode wirksam
1017
               
1023
                }
1018
                }
-
 
1019
        else            // HH-Mode
-
 
1020
                {
1024
        /*else          // wenn HH , MartinR
1021
                        if(Parameter_UserParam1 > 49)   // MartinR: zweiter Stick_P Wert nur, wenn HH über Schalter aktiv ist
1025
                {
1022
                        {
-
 
1023
                        stick_nick = ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] - stick_nick_neutral) * Parameter_UserParam3);
-
 
1024
                        stick_roll = ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] - stick_roll_neutral) * Parameter_UserParam3);
-
 
1025
                        }
-
 
1026
               
1026
                //stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben
1027
                        else
1027
                //stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben
1028
                        {
-
 
1029
                        stick_nick = ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] - stick_nick_neutral) * stick_p);
1028
                //StickNick = stick_nick; // MartinR: GPS nur im ACC-Mode wirksam
1030
                        stick_roll = ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] - stick_roll_neutral) * stick_p);
1029
                //StickRoll = stick_roll; // MartinR: GPS nur im ACC-Mode wirksam
-
 
1030
                }
1031
                        }
Line 1031... Line 1032...
1031
        */
1032
                }
1032
// MartinR: geändert Ende
1033
// MartinR: geändert Ende
1033
 
1034
 
Line 1066... Line 1067...
1066
    StickNick -= GPS_Nick;
1067
    StickNick -= GPS_Nick;
1067
    StickRoll -= GPS_Roll;
1068
    StickRoll -= GPS_Roll;
1068
        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 127;
1069
        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 127;
Line 1069... Line 1070...
1069
 
1070
 
1070
    GyroFaktor     = (Parameter_Gyro_P + 10.0);
1071
    GyroFaktor     = (Parameter_Gyro_P + 10.0);
1071
    IntegralFaktor = Parameter_Gyro_I;
1072
        // IntegralFaktor = Parameter_Gyro_I; // MartinR: verschoben um Code zu sparen
1072
    GyroFaktorGier     = (Parameter_Gyro_Gier_P + 10.0);
1073
    GyroFaktorGier     = (Parameter_Gyro_Gier_P + 10.0);
Line 1073... Line 1074...
1073
    IntegralFaktorGier = Parameter_Gyro_Gier_I;
1074
    IntegralFaktorGier = Parameter_Gyro_Gier_I;
1074
 
1075
 
Line 1191... Line 1192...
1191
 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
1192
 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
1192
 MittelIntegralRoll  += IntegralRoll;
1193
 MittelIntegralRoll  += IntegralRoll;
1193
 MittelIntegralNick2 += IntegralNick2;
1194
 MittelIntegralNick2 += IntegralNick2;
1194
 MittelIntegralRoll2 += IntegralRoll2;
1195
 MittelIntegralRoll2 += IntegralRoll2;
Line 1195... Line -...
1195
 
-
 
1196
// if(Looping_Nick || Looping_Roll)  // MartinR: so war es
-
 
1197
   if(Looping_Nick || Looping_Roll || (!IntegralFaktor & (Parameter_UserParam1 < 50) & !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: erweitert
-
 
1198
  {
-
 
1199
    IntegralAccNick = 0;
-
 
1200
    IntegralAccRoll = 0;
-
 
1201
    MittelIntegralNick = 0;
-
 
1202
    MittelIntegralRoll = 0;
-
 
1203
    MittelIntegralNick2 = 0;
-
 
1204
    MittelIntegralRoll2 = 0;
-
 
1205
    Mess_IntegralNick2 = Mess_IntegralNick;
-
 
1206
    Mess_IntegralRoll2 = Mess_IntegralRoll;
-
 
1207
    ZaehlMessungen = 0;
-
 
1208
    LageKorrekturNick = 0;
-
 
1209
    LageKorrekturRoll = 0;
-
 
1210
  }
-
 
1211
 
1196
 
1212
  if((!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))    ) // MartinR:
1197
  if((!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR:
1213
          // nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH 
1198
          // nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH 
1214
          // um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten!
1199
          // um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten!
1215
          // bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet 
1200
          // bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet 
1216
        {
-
 
1217
       
1201
        {
1218
        IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
1202
        IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
1219
    IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
1203
    IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
1220
    Mess_IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode       
1204
    Mess_IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode       
1221
    Mess_IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
1205
    Mess_IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
-
 
1206
    Mess_Integral_Gier = 0;     // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode  
1222
    Mess_Integral_Gier = 0;     // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode  
1207
        sollGier = 0;
1223
        Integral_Gier = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
1208
        Integral_Gier = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
1224
    //Mess_Integral_Gier2 = 0; // MartinR:  Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
1209
    //Mess_Integral_Gier2 = 0; // MartinR:  Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
1225
        KompassSollWert = KompassValue; // MartinR: aktuelle Ausrichtung beibehalten
1210
        KompassSollWert = KompassValue; // MartinR: aktuelle Ausrichtung beibehalten
-
 
1211
        ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; // MartinR: aktuelle Ausrichtung beibehalten
1226
        ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; // MartinR: aktuelle Ausrichtung beibehalten
1212
        NeueKompassRichtungMerken = 1; // MartinR: aktuelle Ausrichtung beibehalten
-
 
1213
        }
-
 
1214
 
-
 
1215
// if(Looping_Nick || Looping_Roll)  // MartinR: so war es
-
 
1216
   if(Looping_Nick || Looping_Roll || (!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: erweitert
-
 
1217
   // MartinR: beim ACC-Loop oder beim zurückschalten von HH auf ACC
-
 
1218
  {
-
 
1219
    IntegralAccNick = 0;
-
 
1220
    IntegralAccRoll = 0;
-
 
1221
    MittelIntegralNick = 0;
-
 
1222
    MittelIntegralRoll = 0;
-
 
1223
    MittelIntegralNick2 = 0;
-
 
1224
    MittelIntegralRoll2 = 0;
-
 
1225
    Mess_IntegralNick2 = Mess_IntegralNick;
-
 
1226
    Mess_IntegralRoll2 = Mess_IntegralRoll;
-
 
1227
    ZaehlMessungen = 0;
-
 
1228
    LageKorrekturNick = 0;
-
 
1229
    LageKorrekturRoll = 0;
Line 1227... Line 1230...
1227
        }
1230
  }
1228
 
1231
 
Line 1229... Line 1232...
1229
        if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 49)) IntegralFaktor = 0; // MartinR geändert und verschoben
1232
        if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 49)) IntegralFaktor = 0; // MartinR geändert und verschoben