WebSVN - FlightCtrl - Diff - Rev 211 and 212 - /branches/V0.63_LED1

 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Copyright (c) 04.2007 Holger Buss
 // + Nur für den privaten Gebrauch
 // + www.MikroKopter.com
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
 // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
 // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
 // + bzgl. der Nutzungsbedingungen aufzunehmen.
 // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
 // + Verkauf von Luftbildaufnahmen, usw.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
 // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
 // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
 // + eindeutig als Ursprung verlinkt werden
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
 // + Benutzung auf eigene Gefahr
 // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
 // + mit unserer Zustimmung zulässig
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Redistributions of source code (with or without modifications) must retain the above copyright notice,
 // + this list of conditions and the following disclaimer.
 // +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
 // +     from this software without specific prior written permission.
 // +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
 // +     for non-commercial use (directly or indirectly)
 // +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
 // +     with our written permission
 // +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
 // +     clearly linked as origin
 // +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
 // +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 // +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 // +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 // +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 // +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 // +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 // +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 // +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // +  POSSIBILITY OF SUCH DAMAGE.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include "main.h"
 volatile int  KompassStartwert = 0;
 volatile int  KompassRichtung = 0;
 unsigned char MAX_GAS,MIN_GAS;
 unsigned char Notlandung = 0;
 unsigned char HoehenReglerAktiv = 0;
+//-------HF-------
+unsigned char LED_Switch = 0;
+unsigned char LED_flash  = 0;
+unsigned char LED_1  = 0;
+unsigned char LED_2  = 0;
+unsigned char LED_2on  = 0;
+//-------HF-------
 float GyroFaktor;
 float IntegralFaktor;
 volatile int  DiffNick,DiffRoll;
 int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
 unsigned char MotorWert[5];
 volatile unsigned char SenderOkay = 0;
 //  Nullwerte ermitteln
 void SetNeutral(void)
 //############################################################################
+{
     unsigned int timer;
         NeutralAccX = 0;
         NeutralAccY = 0;
         NeutralAccZ = 0;
     AdNeutralNick = 0;
         AdNeutralRoll = 0;
         AdNeutralGier = 0;
     CalibrierMittelwert();
     timer = SetDelay(5);
         while (!CheckDelay(timer));
         CalibrierMittelwert();
     if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
      {
       if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
+     }
     if(PlatinenVersion == 10)
     {
      AdNeutralNick= abs(MesswertNick);
          AdNeutralRoll= abs(MesswertRoll);
          AdNeutralGier= abs(MesswertGier);
     }
     else
     {
      AdNeutralNick= abs(MesswertNick) / 2;
          AdNeutralRoll= abs(MesswertRoll) / 2;
          AdNeutralGier= abs(MesswertGier) / 2;
+    }
     NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
         NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
         NeutralAccZ = Aktuell_az;
         Mess_IntegralNick = 0;
     Mess_IntegralNick2 = 0;
     Mess_IntegralRoll = 0;
     Mess_IntegralRoll2 = 0;
     Mess_Integral_Gier = 0;
     MesswertNick = 0;
     MesswertRoll = 0;
     MesswertGier = 0;
     StartLuftdruck = Luftdruck;
     HoeheD = 0;
     Mess_Integral_Hoch = 0;
     KompassStartwert = KompassValue;
     GPS_Neutral();
     beeptime = 50;
+}
 //############################################################################
 // Bildet den Mittelwert aus den Messwerten
 void Mittelwert(void)
 //############################################################################
+{
     // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
     ANALOG_OFF;
         if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
     accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
     Integral_Gier  = Mess_Integral_Gier;
 //    Integral_Gier2 = Mess_Integral_Gier2;
     IntegralNick = Mess_IntegralNick;
     IntegralRoll = Mess_IntegralRoll;
     IntegralNick2 = Mess_IntegralNick2;
     IntegralRoll2 = Mess_IntegralRoll2;
     // ADC einschalten
     ANALOG_ON;
 /*
 //------------------------------------------------------------------------------
     if(MesswertNick > 200)  MesswertNick += 4 * (MesswertNick - 200);
     else
     if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
     if(MesswertRoll > 200)  MesswertRoll += 4 * (MesswertRoll - 200);
     else
     if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
 //------------------------------------------------------------------------------
 */
     if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
     if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
 //############################################################################
 // Messwerte beim Ermitteln der Nullage
 void CalibrierMittelwert(void)
 //############################################################################
+{
     // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
         ANALOG_OFF;
     if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
         if(MessanzahlRoll)    (MesswertRoll = AccumulateRoll / MessanzahlRoll);
     AccumulateGier = 0;   MessanzahlGier = 0;
     accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
     accumulate_AccNick = 0;messanzahl_AccNick = 0;
     accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
     // ADC einschalten
     ANALOG_ON;
     if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
     if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
     if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
     if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
     if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
 //############################################################################
 // Senden der Motorwerte per I2C-Bus
 void SendMotorData(void)
 //############################################################################
+{
     if(MOTOR_OFF || !MotorenEin)
+        {
         Motor_Hinten = 0;
         Motor_Vorne = 0;
+        }
     DebugOut.Analog[12] = Motor_Vorne;
     DebugOut.Analog[13] = Motor_Hinten;
     DebugOut.Analog[14] = Motor_Links;
     DebugOut.Analog[15] = Motor_Rechts;
     //Start I2C Interrupt Mode
     twi_state = 0;
     motor = 0;
     i2c_start();
 }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Konstanten
 // + 0-250 -> normale Werte
 // + 251 -> Poti1
 // + 252 -> Poti2
 // + 253 -> Poti3
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 void DefaultKonstanten1(void)
 {
- EE_Parameter.Kanalbelegung[K_NICK]  = 1;
+ EE_Parameter.Kanalbelegung[K_NICK]  = 3;
  EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
- EE_Parameter.Kanalbelegung[K_GAS]   = 3;
+ EE_Parameter.Kanalbelegung[K_GAS]   = 1;
  EE_Parameter.Kanalbelegung[K_GIER]  = 4;
  EE_Parameter.Kanalbelegung[K_POTI1] = 5;
  EE_Parameter.Kanalbelegung[K_POTI2] = 6;
  EE_Parameter.Kanalbelegung[K_POTI3] = 7;
  EE_Parameter.Kanalbelegung[K_POTI4] = 8;
  EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01;
  EE_Parameter.Hoehe_MinGas = 30;
  EE_Parameter.MaxHoehe     = 251;      // Wert : 0-32   251 -> Poti1
  EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
  EE_Parameter.ServoNickRefresh = 5;
  EE_Parameter.LoopGasLimit = 50;
  EE_Parameter.LoopThreshold = 90;         // Wert: 0-250  Schwelle für Stickausschlag
  EE_Parameter.LoopConfig = 0;             // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt
  memcpy(EE_Parameter.Name, "Sport\0", 12);
+}
 void DefaultKonstanten2(void)
 {
- EE_Parameter.Kanalbelegung[K_NICK]  = 1;
+ EE_Parameter.Kanalbelegung[K_NICK]  = 3;
  EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
- EE_Parameter.Kanalbelegung[K_GAS]   = 3;
+ EE_Parameter.Kanalbelegung[K_GAS]   = 1;
  EE_Parameter.Kanalbelegung[K_GIER]  = 4;
  EE_Parameter.Kanalbelegung[K_POTI1] = 5;
  EE_Parameter.Kanalbelegung[K_POTI2] = 6;
  EE_Parameter.Kanalbelegung[K_POTI3] = 7;
  EE_Parameter.Kanalbelegung[K_POTI4] = 8;
  EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01;
  EE_Parameter.Hoehe_MinGas = 30;
  EE_Parameter.MaxHoehe     = 251;     // Wert : 0-32   251 -> Poti1
  EE_Parameter.Hoehe_P      = 10;      // Wert : 0-32
  EE_Parameter.Luftdruck_D  = 50;      // Wert : 0-250
  EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
  EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
  EE_Parameter.ServoNickRefresh = 5;
  EE_Parameter.LoopGasLimit = 50;
  EE_Parameter.LoopThreshold = 90;         // Wert: 0-250  Schwelle für Stickausschlag
  EE_Parameter.LoopConfig = 0;              // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts
  memcpy(EE_Parameter.Name, "Kamera\0", 12);
+}
 //############################################################################
 // Trägt ggf. das Poti als Parameter ein
 void ParameterZuordnung(void)
 //############################################################################
+{
  CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
  CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
  CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
  CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
  CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
  CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
  CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
  CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
  CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
  CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
  CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
  CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
  CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
  CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
          static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
          static unsigned int  modell_fliegt = 0;
      static int hoehenregler = 0;
      static char TimerWerteausgabe = 0;
      static char NeueKompassRichtungMerken = 0;
         Mittelwert();
     GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gaswert ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
     if(GasMischanteil < 0) GasMischanteil = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Emfang schlecht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(SenderOkay < 100)
+        {
         if(!PcZugriff)
          {
            if(BeepMuster == 0xffff)
+            {
              beeptime = 15000;
              BeepMuster = 0x0c00;
             }
          }
         if(RcLostTimer) RcLostTimer--;
         else
+         {
           MotorenEin = 0;
           Notlandung = 0;
+         }
         ROT_ON;
         if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
+            {
             PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
             PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
+            }
          else MotorenEin = 0;
+        }
         else
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Emfang gut
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(SenderOkay > 140)
+            {
             Notlandung = 0;
             RcLostTimer = EE_Parameter.NotGasZeit * 50;
             if(GasMischanteil > 40)
+                }
             if((modell_fliegt < 200) || (GasMischanteil < 40))
+                {
                 SummeNick = 0;
                 SummeRoll = 0;
                 Mess_Integral_Gier = 0;
                 Mess_Integral_Gier2 = 0;
+                }
             if((GasMischanteil > 200) && MotorenEin == 0)
+                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // auf Nullwerte kalibrieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
+                    {
                     unsigned char setting;
                     if(++delay_neutral > 200)  // nicht sofort
+                        {
                          eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
+                        }
                         if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+                          {
                              if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
                           }
                             ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
                         SetNeutral();
                         Piep(GetActiveParamSetNumber());
                         }
+                    }
                  else delay_neutral = 0;
+                }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gas ist unten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
             if(GasMischanteil < 35)
+                {
                 // Starten
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
+                    {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Einschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                     if(++delay_einschalten > 200)
+                        {
                         delay_einschalten = 200;
                         modell_fliegt = 1;
                         MotorenEin = 1;
                         sollGier = 0;
                         Mess_Integral_Gier = 0;
                         Mess_Integral_Gier2 = 0;
                         Mess_IntegralNick = 0;
                         Mess_IntegralRoll = 0;
                         Mess_IntegralNick2 = IntegralNick;
                         Mess_IntegralRoll2 = IntegralRoll;
                         SummeNick = 0;
                         SummeRoll = 0;
                         }
                     }
                     else delay_einschalten = 0;
                 //Auf Neutralwerte setzen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Auschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
+                    {
                     if(++delay_ausschalten > 200)  // nicht sofort
+                        {
                         MotorenEin = 0;
                         delay_ausschalten = 200;
                         modell_fliegt = 0;
                         }
+                    }
                 else delay_ausschalten = 0;
+                }
+            }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // neue Werte von der Funke
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(!NewPpmData-- || Notlandung)
+  {
     ParameterZuordnung();
     StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
     StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
     StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
     StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
     StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
     GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
     IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
     if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
+    if(GyroFaktor < 0) GyroFaktor = 0;
+    if(IntegralFaktor < 0) IntegralFaktor = 0;
+    //------HF------- LEDs Schalten - LED1 Normal, LED2 Blitzlicht
+        LED_Switch = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]];
+        if (((LED_Switch > 32) & (LED_Switch < 96)) | (LED_Switch > 160)) LED_2++; else if (LED_2 > 0) LED_2--;
+        if ((LED_Switch > 96)) LED_1++; else if (LED_1 > 0) LED_1--;
+        if (LED_1 >= 10) {LED_1 = 10; LED1_ON;}
+    if (LED_1 <=  1) LED1_OFF;
+        if (LED_2 >= 10) {LED_2 = 10; LED_2on = 1;}
+        if (LED_2 <=  1) LED_2on = 0;
+        if (LED_2on  ==  1) LED_flash++;
+        if (LED_flash > 77) LED2_ON; else LED2_OFF;
+        if (LED_flash > 80) LED_flash = 0;
-    if(GyroFaktor < 0) GyroFaktor = 0;
+        //------HF------- End LEDs Schalten
     if(IntegralFaktor < 0) IntegralFaktor = 0;
     // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
     if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
      {
       if(IntegralNick >  60000)
       {
        StickNick -=  8 * EE_Parameter.Stick_P;
        if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;
       }
       else
       if(IntegralNick < -60000)
       {
        StickNick += 8 * EE_Parameter.Stick_P;
        if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;
       }
       if(IntegralRoll >  60000)
       {
        StickRoll -=  8 * EE_Parameter.Stick_P;
        if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;
       }
       else
       if(IntegralRoll < -60000)
       {
        StickRoll += 8 * EE_Parameter.Stick_P;
        if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;
+      }
      }
   }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Looping?
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) ||
      ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
    {
     Looping_Roll = 1;
     if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
    }
    else Looping_Roll = 0;
   if(((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) ||
      ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN))
    {
     Looping_Nick = 1;
     Looping_Roll = 0;
     if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
    }
    else Looping_Nick = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Bei Empfangsausfall im Flug
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if(Notlandung)
+    {
      StickGier = 0;
      StickNick = 0;
      StickRoll = 0;
      GyroFaktor  = 0.1;
      IntegralFaktor = 0.005;
      Looping_Roll = 0;
      Looping_Nick = 0;
+    }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gyro-Drift kompensieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #define DRIFT_FAKTOR 3
     if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR)
+        {
         IntegralFehlerNick = IntegralNick2 - IntegralNick;
         IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
         ZaehlMessungen = 0;
         if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
         if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
         if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
         if(IntegralFehlerRoll < -500/DRIFT_FAKTOR)  AdNeutralRoll--;
 //        if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;   // macht nur mit Referenz (Kompass Sinn)
 //        if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;   // macht nur mit Referenz (Kompass Sinn)
     ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
         Mess_IntegralNick2 = IntegralNick;
         Mess_IntegralRoll2 = IntegralRoll;
         Mess_Integral_Gier2 = Integral_Gier;
     ANALOG_ON;  // ADC einschalten
+        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Integrale auf ACC-Signal abgleichen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(IntegralFaktor && !Looping_Nick && !Looping_Roll)
+  {
     tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
     if(labs(Mittelwert_AccNick) < 200) tmp_long /= 8;
     else tmp_long /= 16;
     tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
     if(labs(Mittelwert_AccRoll) < 200) tmp_long2 /= 8;
     else tmp_long2 /= 16;
  #define AUSGLEICH 500
     if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
     if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
     if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
     if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
+  }
   else
   {
    tmp_long = 0;
    tmp_long2 = 0;
   }
  ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
     Mess_IntegralNick -= tmp_long;
     Mess_IntegralRoll -= tmp_long2;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Gieren
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(abs(StickGier) > 35)
+     {
       if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
      }
     tmp_int  = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo  y = ax + bx²
     tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
     sollGier = tmp_int;
     Mess_Integral_Gier -= tmp_int;
     if(Mess_Integral_Gier > 25000) Mess_Integral_Gier = 25000;  // begrenzen
     if(Mess_Integral_Gier <-25000) Mess_Integral_Gier =-25000;
  ANALOG_ON;     // ADC einschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Kompass
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
+     {
        int w,v;
        static int SignalSchlecht = 0;
        w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
        v = abs(IntegralRoll /512);
+        {
           ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
           if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
           ANALOG_ON;  // ADC einschalten
           if(SignalSchlecht) SignalSchlecht--;
         }
         else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
      }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Debugwerte zuordnen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(!TimerWerteausgabe--)
+   {
     TimerWerteausgabe = 49;
     DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
 */
 //    DebugOut.Analog[9] = MesswertNick;
 //    DebugOut.Analog[9] = SollHoehe;
 //    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
 //    DebugOut.Analog[11] = KompassStartwert;
 //    DebugOut.Analog[10] = Parameter_Gyro_I;
 //    DebugOut.Analog[10] = EE_Parameter.Gyro_I;
 //    DebugOut.Analog[9] = KompassRichtung;
 //    DebugOut.Analog[10] = GasMischanteil;
 //    DebugOut.Analog[3] = HoeheD * 32;
 //    DebugOut.Analog[4] = hoehenregler;
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
     else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
     if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
     else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
     if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
     if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
     if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
     if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Höhenregelung
 // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //OCR0B = 180 - (Poti1 + 120) / 4;
 //DruckOffsetSetting = OCR0B;
  if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
+  {
     int tmp_int;
     if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
     {
      if(Parameter_MaxHoehe < 50)
+      {
        SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
        HoehenReglerAktiv = 0;
       }
       else
         HoehenReglerAktiv = 1;
     }
     else
+    {
      SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20;
      HoehenReglerAktiv = 1;
       h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
       tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
       if(tmp_int > 50) tmp_int = 50;
       else if(tmp_int < -50) tmp_int = -50;
       h -= tmp_int;
       hoehenregler = (hoehenregler*15 + h) / 16;
       if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
+       {
          if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
          if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
        }
       if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
       GasMischanteil = hoehenregler;
      }
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mischer und PI-Regler
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   DebugOut.Analog[7] = GasMischanteil;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gier-Anteil
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #define MUL_G  1.4
     GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
 //GierMischanteil = 0;
     if(GierMischanteil > (MUL_G * GasMischanteil))  GierMischanteil = MUL_G * GasMischanteil;
     if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil);
     if(GasMischanteil < 10) GierMischanteil = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Nick-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
     SummeNick += DiffNick;                                   // I-Anteil
     if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
     if(SummeNick >  16000) SummeNick =  16000;
     if(SummeNick < -16000) SummeNick = -16000;
     pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick
     // Motor Vorn
 #define MUL  2
     if((pd_ergebnis >  MUL * (GasMischanteil))) pd_ergebnis =  MUL * (GasMischanteil);
     if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil);
     motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
         Motor_Vorne = motorwert;
     // Motor Heck
         motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
         Motor_Hinten = motorwert;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Achse
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
         SummeRoll += DiffRoll;                                                              // I-Anteil
     if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
     if(SummeRoll >  16000) SummeRoll =  16000;
     if(SummeRoll < -16000) SummeRoll = -16000;
     pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll
     if((pd_ergebnis >  MUL * (GasMischanteil))) pd_ergebnis =  MUL * (GasMischanteil);
     if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil);
     // Motor Links
     motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
         if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
         Motor_Links = motorwert;
     // Motor Rechts
         motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
         if ((motorwert < 0)) motorwert = 0;
         else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;

Subversion Repositories FlightCtrl

(root)/branches/V0.63_LED1_LED2/fc.c @ 377 - Rev 211 → 212