Subversion Repositories FlightCtrl

signed char cosinus, sinus; // MartinR : extern für PAN-Funktion

signed char cosinus, sinus; // MartinR : extern für PAN-Funktion

signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;

signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;

//int MaxStickNick = 0,MaxStickRoll = 0; MartinR: so war es

//int MaxStickNick = 0,MaxStickRoll = 0; MartinR: so war es

int MaxStickNick = 0,MaxStickRoll = 0,stick_nick_neutral = 0,stick_roll_neutral = 0;  // MartinR: stick_.._neutral hinzugefügt

unsigned char stick_p;

unsigned int  modell_fliegt = 0;

unsigned int  modell_fliegt = 0;

volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0;

volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0;

        DebugOut.Analog[16] = cosinus;  // MartinR: zur Einstellung der Pan-Funktion

        DebugOut.Analog[16] = cosinus;  // MartinR: zur Einstellung der Pan-Funktion

        DebugOut.Analog[17] = sinus;  // MartinR: test zur Einstellung der Pan-Funktion

        DebugOut.Analog[17] = sinus;  // MartinR: test zur Einstellung der Pan-Funktion

        DebugOut.Analog[18] = ServoPanValue;  // MartinR:  zur Einstellung der Pan-Funktion

        DebugOut.Analog[18] = ServoPanValue;  // MartinR:  zur Einstellung der Pan-Funktion

        DebugOut.Analog[19] = ServoRollValue;  // MartinR:  Test

    DebugOut.Analog[20] = ServoNickValue;

    DebugOut.Analog[20] = ServoNickValue;

    DebugOut.Analog[22] = Capacity.ActualCurrent;

    DebugOut.Analog[22] = Capacity.ActualCurrent;

#ifdef WITH_REMAINCAPACITY      // only include functions if DEBUG is defined in main.h

#ifdef WITH_REMAINCAPACITY      // only include functions if DEBUG is defined in main.h

                                                                        modell_fliegt = 1;

                                                                        modell_fliegt = 1;

                                                                        MotorenEin = 1;

                                                                        MotorenEin = 1;

                                                                        sollGier = 0;

                                                                        sollGier = 0;

                                                                        Mess_Integral_Gier = 0;

                                                                        Mess_Integral_Gier = 0;

                                                                        // Mess_Integral_Gier2 = 0;

                                                                        // Mess_Integral_Gier2 = 0;

                                                                        Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;

                                                                        Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;

                                                                        Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;

                                                                        Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;

                                                                        Mess_IntegralNick2 = IntegralNick;

                                                                        Mess_IntegralNick2 = IntegralNick;

                                                                        Mess_IntegralRoll2 = IntegralRoll;

                                                                        Mess_IntegralRoll2 = IntegralRoll;

                                                                        SummeNick = 0;

                                                                        SummeNick = 0;

                                                                        SummeRoll = 0;

                                                                        SummeRoll = 0;

                                                                        FC_StatusFlags |= FC_STATUS_START;

                                                                        FC_StatusFlags |= FC_STATUS_START;

//                                                                      ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;

//                                                                      ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;

                                                                        NeueKompassRichtungMerken = 100; // 2 sekunden

                                                                        NeueKompassRichtungMerken = 100; // 2 sekunden

                                                                }

                                                                }

                                                                else

                                                                else

                                                                {

                                                                {

                                                                        beeptime = 1500; // indicate missing calibration

                                                                        beeptime = 1500; // indicate missing calibration

                                                                }

                                                                }

    stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;

    stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;

    stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4;

    stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4;

    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;

    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;

                        */

                        */

// MartinR: geändert Anfang

// MartinR: geändert Anfang

         if(IntegralFaktor)  

         if(IntegralFaktor)  // ACC-Mode

                //stick_roll_neutral = stick_roll; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR

                stick_roll_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR

                stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;

                stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;

                //StickNick = stick_nick - (GPS_Nick + GPS_Nick2); // MartinR: GPS nur im ACC-Mode wirksam  

                stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;

                }

                }

                //StickNick = stick_nick; // MartinR: GPS nur im ACC-Mode wirksam

                        stick_roll = ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] - stick_roll_neutral) * stick_p);

                }

                        }

        */

                }

// MartinR: geändert Ende

// MartinR: geändert Ende

    StickNick -= GPS_Nick;

    StickNick -= GPS_Nick;

    StickRoll -= GPS_Roll;

    StickRoll -= GPS_Roll;

        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 127;

        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 127;

    IntegralFaktor = Parameter_Gyro_I;

        // IntegralFaktor = Parameter_Gyro_I; // MartinR: verschoben um Code zu sparen

    GyroFaktorGier     = (Parameter_Gyro_Gier_P + 10.0);

    GyroFaktorGier     = (Parameter_Gyro_Gier_P + 10.0);

    IntegralFaktorGier = Parameter_Gyro_Gier_I;

    IntegralFaktorGier = Parameter_Gyro_Gier_I;

 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren

 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren

 MittelIntegralRoll  += IntegralRoll;

 MittelIntegralRoll  += IntegralRoll;

 MittelIntegralNick2 += IntegralNick2;

 MittelIntegralNick2 += IntegralNick2;

 MittelIntegralRoll2 += IntegralRoll2;

 MittelIntegralRoll2 += IntegralRoll2;

  if((!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))    ) // MartinR:

  if((!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR:

          // nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH 

          // nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH 

          // um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten!

          // um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten!

          // bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet 

          // bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet 

        {

        IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode

        IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode

    IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode

    IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode

    Mess_IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode       

    Mess_IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode       

    Mess_IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode

    Mess_IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode

    Mess_Integral_Gier = 0;     // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode  

        sollGier = 0;

        Integral_Gier = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode

        Integral_Gier = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode

    //Mess_Integral_Gier2 = 0; // MartinR:  Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode

    //Mess_Integral_Gier2 = 0; // MartinR:  Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode

        KompassSollWert = KompassValue; // MartinR: aktuelle Ausrichtung beibehalten

        KompassSollWert = KompassValue; // MartinR: aktuelle Ausrichtung beibehalten

        ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; // MartinR: aktuelle Ausrichtung beibehalten

        NeueKompassRichtungMerken = 1; // MartinR: aktuelle Ausrichtung beibehalten

        }

  }

        if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 49)) IntegralFaktor = 0; // MartinR geändert und verschoben

        if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 49)) IntegralFaktor = 0; // MartinR geändert und verschoben