WebSVN - FlightCtrl - Diff - Rev 2497 and 2499

 unsigned char h,m,s;
 unsigned int BaroExpandActive = 0;
 int MesswertNick,MesswertRoll,MesswertGier,RohMesswertNick,RohMesswertRoll;
 int TrimNick, TrimRoll;
 int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
+int BoatNeutralNick = 0,BoatNeutralRoll = 0,BoatNeutralGier = 0;
 int Mittelwert_AccNick, Mittelwert_AccRoll;
 unsigned int NeutralAccX=0, NeutralAccY=0;
 int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
 int NeutralAccZ = 0;
 signed char NeutralAccZfine = 0;
                         printf("\n\rACC not calibrated!\r\n");
                         NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);
                         NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
                         sucess = 0;
+                }
+        // restore from eeprom
+        BoatNeutralNick = (int16_t)GetParamWord(PID_GYRO_NICK);
+        BoatNeutralRoll = (int16_t)GetParamWord(PID_GYRO_ROLL);
+        BoatNeutralGier = (int16_t)GetParamWord(PID_GYRO_YAW);
      if(FC_StatusFlags3 & FC_STATUS3_BOAT) // Read Gyro Data from eeprom
+     {
-            unsigned int nick, roll, gier;
-                // restore from eeprom
-                nick = (int16_t)GetParamWord(PID_GYRO_NICK);
-                roll = (int16_t)GetParamWord(PID_GYRO_ROLL);
-                gier = (int16_t)GetParamWord(PID_GYRO_YAW);
                 // strange settings?
-                if(((unsigned int) nick > (600 * 16)) || ((unsigned int) nick < (400 * 16))
+                if(((unsigned int) BoatNeutralNick > (600 * 16)) || ((unsigned int) BoatNeutralNick < (400 * 16))
-                   || ((unsigned int) roll > (600 * 16)) || ((unsigned int) roll < (400 * 16))
+                   || ((unsigned int) BoatNeutralRoll > (600 * 16)) || ((unsigned int) BoatNeutralRoll < (400 * 16))
-                   || ((unsigned int) gier > (600 * 2)) || ((unsigned int) gier < (400 * 2)))
+                   || ((unsigned int) BoatNeutralGier > (600 * 2)) || ((unsigned int) BoatNeutralGier < (400 * 2)))
+                {
                         printf("\n\rGyro calibration data not valid\r\n");
                         sucess = 0;
                         FC_StatusFlags3 &= ~FC_STATUS3_BOAT;
+                }
                 else
+                {
-                        AdNeutralNick = nick;
+                        AdNeutralNick = BoatNeutralNick;
-                        AdNeutralRoll = roll;
+                        AdNeutralRoll = BoatNeutralRoll;
-                        AdNeutralGier = gier;
+                        AdNeutralGier = BoatNeutralGier;
+                }
+     }
+    }
         EEAR = EE_DUMMY;  // Set the EEPROM Address pointer to an unused space
         MesswertNick = 0;
+        }
   else StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint;
   LIMIT_MIN_MAX(StickGasHover, 70, 175); // reserve some range for trim up and down
+}
+void ChannelAssingment(void)
+{
+cli();
+        ChannelNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
+        ChannelRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
+        ChannelYaw = PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
+        ChannelGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]];
+//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+//+ Analoge Steuerung per Seriell
+//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+if((ExternalControl.Config & EC_VALID) && (Parameter_ExternalControl > 128))
+    {
+                ChannelNick += ExternalControl.Nick;
+                ChannelRoll += ExternalControl.Roll;
+                ChannelYaw += ExternalControl.Gier;
+        if(ExternalControl.Config & EC_GAS_ADD) ChannelGas += ExternalControl.Gas;
+                else
+                 {
+                  if(ExternalControl.Gas < ChannelGas) ChannelGas = ExternalControl.Gas; // the RC-Stick is the MAX value here
+             }
+        }
+sei();
+        if(ChannelNick > 127) ChannelNick = 127;        else if(ChannelNick < -127) ChannelNick = -127;
+        if(ChannelRoll > 127) ChannelRoll = 127;        else if(ChannelRoll < -127) ChannelRoll = -127;
+        if(ChannelYaw  > 127) ChannelYaw = 127;         else if(ChannelYaw < -127) ChannelYaw = -127;
+        if(ChannelGas  > 127) ChannelGas = 127;         else if(ChannelGas < -127) ChannelGas = -127;
+}
 //############################################################################
 //
     } else HooverGasEmergencyPercent = EE_Parameter.NotGas;
+   }
    if(GasIsZeroCnt == 30000)  // in that case we have RC-Lost, but the MK is probably landed
+    {
          StickGas = 0; // Hold Gas down in that case
-         ExternalControl.Gas = 0;
+//       ExternalControl.Gas = 0;
          HooverGasEmergencyPercent = MIN_GAS;
+        }
         GasMischanteil = StickGas;
     if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if(!NewPpmData-- || (FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING))
+  {
         static int stick_nick,stick_roll;
         unsigned char stick_p;
     ParameterZuordnung();
+        ChannelAssingment();
-        stick_p = EE_Parameter.Stick_P;
-cli();
-        ChannelNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
-        ChannelRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
-        ChannelYaw = PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
-        ChannelGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]];
-//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-//+ Analoge Steuerung per Seriell
-//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-if((ExternalControl.Config & EC_VALID) && (Parameter_ExternalControl > 128))
-    {
-                ChannelNick += ExternalControl.Nick;
-                ChannelRoll += ExternalControl.Roll;
-                ChannelYaw += ExternalControl.Gier;
-        if(ExternalControl.Config & EC_GAS_ADD) ChannelGas += ExternalControl.Gas;
-                else
-                 {
-                  if(ExternalControl.Gas < ChannelGas) ChannelGas = ExternalControl.Gas; // the RC-Stick is the MAX value here
-             }
-        }
-sei();
-        if(ChannelNick > 127) ChannelNick = 127;        else if(ChannelNick < -127) ChannelNick = -127;
-        if(ChannelRoll > 127) ChannelRoll = 127;        else if(ChannelRoll < -127) ChannelRoll = -127;
-        if(ChannelYaw  > 127) ChannelYaw = 127;         else if(ChannelYaw < -127) ChannelYaw = -127;
-        if(ChannelGas  > 127) ChannelGas = 127;         else if(ChannelGas < -127) ChannelGas = -127;
+        stick_p = EE_Parameter.Stick_P;
     stick_nick = (stick_nick * 3 + ChannelNick * stick_p) / 4;
     stick_roll = (stick_roll * 3 + ChannelRoll * stick_p) / 4;
 cli();
     stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
         Mess_IntegralRoll -= tmp_long2;
         DriftNick += tmp_long;
         DriftRoll += tmp_long2;
+  }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
- if(ZaehlMessungen >= ABGLEICH_ANZAHL)
+ if(ZaehlMessungen >= ABGLEICH_ANZAHL)  //alle 512ms
+ {
 //  static int cnt = 0;
 //  static char last_n_p,last_n_n,last_r_p,last_r_n;
   static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Gyro-Drift ermitteln
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(EE_Parameter.Driftkomp && abs(Mittelwert_AccNick) < 200*4 && abs(Mittelwert_AccRoll) < 200*4 && !TrichterFlug && abs(MesswertGier) < 32/* && (FC_StatusFlags & FC_STATUS_FLY)*/)
+  {
-DebugOut.Analog[16] = EE_Parameter.Driftkomp;
    DriftNick -= DriftNick / (64 * (unsigned int) EE_Parameter.Driftkomp);
    DriftRoll -= DriftRoll / (64 * (unsigned int) EE_Parameter.Driftkomp);
    GierGyroFehler -= GierGyroFehler / (64 * (unsigned int) EE_Parameter.Driftkomp);
    if((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(ChannelYaw) > 25))
+    {

Subversion Repositories FlightCtrl

(root)/tags/V2.14a/fc.c @ 2661 - Rev 2497 → 2499