WebSVN - FlightCtrl - Diff - Rev 791 and 792 - /branches/V0.68d CRK HexaLotte/fc.c

 volatile int32_t IntegralPitch = 0,IntegralPitch2 = 0;
 volatile int32_t IntegralRoll = 0,IntegralRoll2 = 0;
 volatile int32_t IntegralYaw = 0;
 volatile int32_t Reading_IntegralGyroPitch = 0, Reading_IntegralGyroPitch2 = 0;
 volatile int32_t Reading_IntegralGyroRoll = 0,  Reading_IntegralGyroRoll2 = 0;
-volatile int32_t Reading_IntegralGyroYaw = 0,   Reading_IntegralGyroYaw2 = 0;
+volatile int32_t Reading_IntegralGyroYaw = 0;
 volatile int32_t MeanIntegralPitch;
 volatile int32_t MeanIntegralRoll;
 // attitude acceleration integrals
     IntegralAccRoll  += ACC_AMPLIFY * AdValueAccRoll;
 // Yaw
         // calculate yaw gyro intergral (~ to rotation angle)
-    Reading_IntegralGyroYaw  += Reading_GyroYaw;
-    Reading_IntegralGyroYaw2 += Reading_GyroYaw;
+    Reading_IntegralGyroYaw  += Reading_GyroYaw;
         // Coupling fraction
         if(!Looping_Pitch && !Looping_Roll && (ParamSet.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE))
+        {
                 tmpl = Reading_IntegralGyroPitch / 4096L;
         static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0;
         static uint16_t Model_Is_Flying = 0;
         static uint8_t HeightControlActive = 0;
         static int16_t HeightControlThrust = 0;
         static int8_t TimerDebugOut = 0;
-        static int8_t StoreNewCompassCourse = 0;
+        static uint16_t UpdateCompassCourse = 0;
         static int32_t CorrectionPitch, CorrectionRoll;
         Mean();
                 if((Model_Is_Flying < 200) || (ThrustMixFraction < 40))
+                {
                         SumPitch = 0;
                         SumRoll = 0;
                         Reading_IntegralGyroYaw = 0;
-                        Reading_IntegralGyroYaw2 = 0;
+                }
                 if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
                 if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
+                        }
                         // and if the yaw stick is in the rightmost position
                         // save the ACC neutral setting to eeprom
                         else if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75)
+                        {
-                        if(++delay_neutral > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
+                                if(++delay_neutral > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
+                                {
-                                delay_neutral = 0;
+                                        delay_neutral = 0;
-                                GRN_OFF;
+                                        GRN_OFF;
-                                SetParamWord(PID_ACC_PITCH, 0xFFFF); // make value invalid
+                                        SetParamWord(PID_ACC_PITCH, 0xFFFF); // make value invalid
-                                Model_Is_Flying = 0;
+                                        Model_Is_Flying = 0;
-                                SetNeutral();
+                                        SetNeutral();
-                                // Save ACC neutral settings to eeprom
+                                        // Save ACC neutral settings to eeprom
-                                SetParamWord(PID_ACC_PITCH, (uint16_t)NeutralAccX);
+                                        SetParamWord(PID_ACC_PITCH, (uint16_t)NeutralAccX);
-                                SetParamWord(PID_ACC_ROLL,  (uint16_t)NeutralAccY);
+                                        SetParamWord(PID_ACC_ROLL,  (uint16_t)NeutralAccY);
-                                SetParamWord(PID_ACC_Z,     (uint16_t)NeutralAccZ);
+                                        SetParamWord(PID_ACC_Z,     (uint16_t)NeutralAccZ);
-                                Beep(GetActiveParamSet());
+                                        Beep(GetActiveParamSet());
+                                }
+                        }
                         else delay_neutral = 0;
+                }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                                         delay_startmotors = 200; // do not repeat if once executed
                                         Model_Is_Flying = 1;
                                         MotorsOn = 1;
                                         SetPointYaw = 0;
                                         Reading_IntegralGyroYaw = 0;
-                                        Reading_IntegralGyroYaw2 = 0;
                                         Reading_IntegralGyroPitch = 0;
                                         Reading_IntegralGyroRoll = 0;
                                         Reading_IntegralGyroPitch2 = IntegralPitch;
                                         Reading_IntegralGyroRoll2 = IntegralRoll;
                                         SumPitch = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Yawing
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 20) // yaw stick is activated
+        if(abs(StickYaw) > 20 ) // yaw stick is activated
-        {   // if not fixed compass course is set update compass course
+        {   // if not fixed compass course is set update compass course for 1s
-                if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) StoreNewCompassCourse = 1;
+                if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) UpdateCompassCourse = 500;
+        }
         // exponential stick sensitivity in yawring rate
         tmp_int  = (int32_t) ParamSet.Yaw_P * ((int32_t)StickYaw * abs(StickYaw)) / 512L; // expo  y = ax + bx²
-        tmp_int += (ParamSet.Yaw_P * StickYaw) / 4;
-        SetPointYaw = tmp_int;
+        tmp_int += (ParamSet.Yaw_P * StickYaw) / 4;
-        Reading_IntegralGyroYaw -= tmp_int;
+        SetPointYaw = tmp_int;
         // limit the effect
         if(Reading_IntegralGyroYaw > 50000) Reading_IntegralGyroYaw = 50000;
                         CompassHeading = MM3_Heading();
                         #endif
                         #ifdef USE_CMPS03
                         CompassHeading = CMPS03_Heading();
                         #endif
                         if (CompassHeading < 0) // no compass data available
+                        {
                                 CompassOffCourse = 0; // disables gyro compass correction
+                        }
                         else // calculate OffCourse (angular deviation from heading to course)
                         CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
+                }
                 // reduce compass effect with increasing declination
                 w = abs(IntegralPitch / 512);
                 v = abs(IntegralRoll  / 512);
                 if(v > w) w = v; // get maximum declination
                 // if declination is small enough then update compass course if neccessary
-                if((w < 35) && StoreNewCompassCourse && (CompassHeading>=0) ) // 35 corresponds to a declination of ~14 deg
+                if((w < 70) && UpdateCompassCourse && (CompassHeading>=0) ) // 70 corresponds to a declination of ~28 deg
+                {
                         CompassCourse = CompassHeading;
-                        StoreNewCompassCourse = 0;
+                        CompassOffCourse = 0; // should be set immediatly because the CompassOffCourse is otherwise only refreshed if new compass heading is calulated
+                }
+                if(UpdateCompassCourse) UpdateCompassCourse--;
                 w = (w * FCParam.CompassYawEffect) / 64;  // (w=0 for 64->~25 deg, 128->~50 deg) for higher declinaions the compass drift compensation is disabled
                 w = FCParam.CompassYawEffect - w; // reduce compass effect with increasing declination
                 if(w > 0) // if there is any compass effect (avoid negative compass feedback)
+                {
                         Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32;

Subversion Repositories FlightCtrl

(root)/branches/V0.68d CRK HexaLotte/fc.c @ 1465 - Rev 791 → 792