WebSVN - FlightCtrl - Diff - Rev 1600 and 1622 - /branches/metro/v0.82a metro

 // +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // +  POSSIBILITY OF SUCH DAMAGE.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-#include "main.h"
-#include "eeprom.c"
+#include "main.h"
 #include "mymath.h"
 #include "isqrt.h"
 unsigned char h,m,s;
 unsigned int BaroExpandActive = 0;
 volatile unsigned int I2CTimeout = 100;
-int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
-int TrimNick, TrimRoll;
+int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
-int AdNeutralGierBias;
+int TrimNick, TrimRoll;
 int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
 int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
 int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
 //int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
 unsigned char Poti[9] = {0,0,0,0,0,0,0,0};
 volatile unsigned char SenderOkay = 0;
 int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
 char MotorenEin = 0;
+unsigned char RequiredMotors = 0;
 long HoehenWert = 0;
 long SollHoehe = 0;
 int LageKorrekturRoll = 0,LageKorrekturNick = 0;
 //float Ki =  FAKTOR_I;
 int Ki = 10300 / 33;
 unsigned char Parameter_NaviOperatingRadius;
 unsigned char Parameter_NaviWindCorrection;
 unsigned char Parameter_NaviSpeedCompensation;
 unsigned char Parameter_ExternalControl;
 unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;
-struct mk_param_struct EE_Parameter;
 signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
 int MaxStickNick = 0,MaxStickRoll = 0;
 unsigned int  modell_fliegt = 0;
 volatile unsigned char FCFlags = 0;
 long GIER_GRAD_FAKTOR = 1291;
 #define LIMIT_MIN(value, min) {if(value <= min) value = min;}
 #define LIMIT_MAX(value, max) {if(value >= max) value = max;}
+#define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;}
 #define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;}
 int MotorSmoothing(int neu, int alt)
+{
  int motor;
   Delay_ms(dauer * 2);
+ }
+}
+//############################################################################
+// Messwerte beim Ermitteln der Nullage
+void CalibrierMittelwert(void)
+//############################################################################
+{
+    unsigned char i;
+    if(PlatinenVersion == 13) SucheGyroOffset();
+    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
+        ANALOG_OFF;
+        MesswertNick = AdWertNick;
+        MesswertRoll = AdWertRoll;
+        MesswertGier = AdWertGier;
+        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
+        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
+        Mittelwert_AccHoch = (long)AdWertAccHoch;
+   // ADC einschalten
+    ANALOG_ON;
+   for(i=0;i<8;i++)
+    {
+     int tmp;
+         tmp = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 110;
+         LIMIT_MIN_MAX(tmp, 0, 255);
+     if(Poti[i] > tmp) Poti[i]--;  else  if(Poti[i] < tmp) Poti[i]++;
+        }
+        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
+        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
+}
 //############################################################################
 //  Nullwerte ermitteln
-void SetNeutral(void)
+void SetNeutral(unsigned char AccAdjustment)
 //############################################################################
 {
- unsigned char i;
+        unsigned char i;
+        unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;
  unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;
     HEF4017R_ON;
         NeutralAccX = 0;
         NeutralAccY = 0;
+        NeutralAccZ = 0;
         NeutralAccZ = 0;
     AdNeutralNick = 0;
         AdNeutralRoll = 0;
-        AdNeutralGier = 0;
+        AdNeutralGier = 0;
     AdNeutralGierBias = 0;
     Parameter_AchsKopplung1 = 0;
+    Parameter_AchsKopplung2 = 0;
     Parameter_AchsKopplung2 = 0;
+    ExpandBaro = 0;
     ExpandBaro = 0;
+    CalibrierMittelwert();
     CalibrierMittelwert();
+    Delay_ms_Mess(100);
     Delay_ms_Mess(100);
+        CalibrierMittelwert();
         CalibrierMittelwert();
     if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+     {
       if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
+     }
           roll_neutral += AdWertRoll;
+         }
      AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
          AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
          AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER);
-     AdNeutralGierBias = AdNeutralGier;
      StartNeutralRoll = AdNeutralRoll;
      StartNeutralNick = AdNeutralNick;
-    if(eeprom_read_byte((unsigned char*)(EEPROM_ADR_ACC_NICK)) > 4)
+     if(AccAdjustment)
-    {
+     {
-      NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
+            NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);
-          NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);
+            NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
-          NeutralAccZ = Aktuell_az;
+            NeutralAccZ = Aktuell_az;
+                // Save ACC neutral settings to eeprom
+                SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccY);
+                SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccX);
+                SetParamWord(PID_ACC_TOP,  (uint16_t)NeutralAccZ);
+    }
     else
+    {
+                // restore from eeprom
-      NeutralAccX = (int)eeprom_read_byte((unsigned char*)(EEPROM_ADR_ACC_NICK)) * 256 + (int)eeprom_read_byte((unsigned char*)(EEPROM_ADR_ACC_NICK+1));
+                NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK);
-          NeutralAccY = (int)eeprom_read_byte((unsigned char*)(EEPROM_ADR_ACC_ROLL)) * 256 + (int)eeprom_read_byte((unsigned char*)(EEPROM_ADR_ACC_ROLL+1));
+                NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL);
+                NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP);
+                // strange settings?
-          NeutralAccZ = (int)eeprom_read_byte((unsigned char*)(EEPROM_ADR_ACC_Z)) * 256 + (int)eeprom_read_byte((unsigned char*)(EEPROM_ADR_ACC_Z+1));
+                if((NeutralAccX > 2048) || (NeutralAccY > 2048) || (NeutralAccZ > 1024))
+                {
+                        printf("\n\rACC not calibrated!\r\n");
+                        NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);
+                        NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
+                NeutralAccZ = Aktuell_az;
+                }
+    }
     MesswertNick = 0;
     MesswertRoll = 0;
     static signed long tmpl,tmpl2,tmpl3,tmpl4;
         static signed int oldNick, oldRoll, d2Roll, d2Nick;
         signed long winkel_nick, winkel_roll;
     unsigned char i;
         MesswertGier = (signed int) AdNeutralGier - AdWertGier;
-//    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
     MesswertNick = (signed int) AdWertNickFilter / 8;
     MesswertRoll = (signed int) AdWertRollFilter / 8;
     RohMesswertNick = MesswertNick;
     RohMesswertRoll = MesswertRoll;
+          }
+        }
+}
-//############################################################################
-// Messwerte beim Ermitteln der Nullage
-void CalibrierMittelwert(void)
-//############################################################################
-{
-    unsigned char i;
-    if(PlatinenVersion == 13) SucheGyroOffset();
-    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
-        ANALOG_OFF;
-        MesswertNick = AdWertNick;
-        MesswertRoll = AdWertRoll;
-        MesswertGier = AdWertGier;
-        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
-        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
-        Mittelwert_AccHoch = (long)AdWertAccHoch;
-   // ADC einschalten
-    ANALOG_ON;
-   for(i=0;i<8;i++)
-    {
-     int tmp;
-         tmp = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 110;
-         LIMIT_MIN_MAX(tmp, 0, 255);
-     if(Poti[i] > tmp) Poti[i]--;  else  if(Poti[i] < tmp) Poti[i]++;
-        }
-        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
-        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
-}
 //############################################################################
 // Senden der Motorwerte per I2C-Bus
 void SendMotorData(void)
 //############################################################################
+{
      static long IntegralFehlerNick = 0;
      static long IntegralFehlerRoll = 0;
          static unsigned int RcLostTimer;
          static unsigned char delay_neutral = 0;
          static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
+         static unsigned char calibration_done = 0;
      static char TimerWerteausgabe = 0;
      static char NeueKompassRichtungMerken = 0;
      static long ausgleichNick, ausgleichRoll;
      int IntegralNickMalFaktor,IntegralRollMalFaktor;
          unsigned char i;
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
                          if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
-                         SetActiveParamSetNumber(setting);  // aktiven Datensatz merken
+                         SetActiveParamSet(setting);  // aktiven Datensatz merken
+                        }
 //                        else
                          if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)
+                          {
                            WinkelOut.CalcState = 1;
                            beeptime = 1000;
+                          }
                           else
+                          {
-                               ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
+                               ParamSet_ReadFromEEProm(GetActiveParamSet());
                                LipoDetection(0);
                                                    LIBFC_ReceiverInit();
                            if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
+                            {
                              if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
+                            }
                                                    ServoActive = 0;
-                           SetNeutral();
+                           SetNeutral(0);
+                           calibration_done = 1;
                                                    ServoActive = 1;
                                                    DDRD  |=0x80; // enable J7 -> Servo signal
-                           Piep(GetActiveParamSetNumber(),120);
+                           Piep(GetActiveParamSet(),120);
+                         }
+                        }
+                    }
                  else
                 if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern
+                    {
                     if(++delay_neutral > 200)  // nicht sofort
+                        {
                         GRN_OFF;
-                        eeprom_write_byte((unsigned char*)(EEPROM_ADR_ACC_NICK),0xff); // Werte löschen
                         MotorenEin = 0;
                         delay_neutral = 0;
                         modell_fliegt = 0;
-                        SetNeutral();
+                        SetNeutral(1);
-                        eeprom_write_byte((unsigned char*)(EEPROM_ADR_ACC_NICK),NeutralAccX / 256); // ACC-NeutralWerte speichern
-                        eeprom_write_byte((unsigned char*)(EEPROM_ADR_ACC_NICK+1),NeutralAccX % 256); // ACC-NeutralWerte speichern
-                        eeprom_write_byte((unsigned char*)(EEPROM_ADR_ACC_ROLL),NeutralAccY / 256);
+                        calibration_done = 1;
-                        eeprom_write_byte((unsigned char*)(EEPROM_ADR_ACC_ROLL+1),NeutralAccY % 256);
-                        eeprom_write_byte((unsigned char*)(EEPROM_ADR_ACC_Z),(int)NeutralAccZ / 256);
-                        eeprom_write_byte((unsigned char*)(EEPROM_ADR_ACC_Z+1),(int)NeutralAccZ % 256);
-                        Piep(GetActiveParamSetNumber(),120);
+                        Piep(GetActiveParamSet(),120);
+                        }
+                    }
                  else delay_neutral = 0;
+                }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Einschalten
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                                                         if(++delay_einschalten > 200)
+                                                        {
                                                                 delay_einschalten = 0;
+                                                                if(calibration_done)
+                                                                {
-                                                                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;
-                                                                FCFlags |= FCFLAG_START;
+                                                                        FCFlags |= FCFLAG_START;
+                                                                }
+                                                                else
+                                                                {
+                                                                        beeptime = 1500; // indicate missing calibration
+                                                                }
+                                                        }
+                                                }
                                                 else delay_einschalten = 0;
+                                        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     Mess_IntegralNick2 -= IntegralFehlerNick;
     Mess_IntegralRoll2 -= IntegralFehlerRoll;
   if(EE_Parameter.Driftkomp)
    {
-    if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }
+    if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; }
-    if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
+    if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; }
+   }
     GierGyroFehler = 0;
     tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L;
     tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L;
     tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;
     tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]);  // Filter
         tmp_int = tmp_motorwert[i] / STICK_GAIN;
-        CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS);
+        LIMIT_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS);
     Motor[i].SetPoint = tmp_int;
+   }
    else Motor[i].SetPoint = 0;
+ }
+}

Subversion Repositories FlightCtrl

(root)/branches/metro/v0.82a metro_Akku+/fc.c @ 1833 - Rev 1600 → 1622