WebSVN - FlightCtrl - Diff - Rev 935 and 936 - /branches/V0.70d Code Redesign killagreg/fc.c

 #include "mk3mag.h"
 #include "gps.h"
 #endif
 #include "led.h"
 volatile uint16_t I2CTimeout = 100;
 // gyro readings
-volatile int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
+int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
 // gyro neutral readings
-volatile int16_t AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;
+int16_t AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralYaw = 0;
-volatile int16_t StartNeutralRoll = 0, StartNeutralNick = 0;
+int16_t StartNeutralRoll = 0, StartNeutralNick = 0;
 // mean accelerations
-volatile int16_t Mean_AccNick, Mean_AccRoll, Mean_AccTop;
+int16_t Mean_AccNick, Mean_AccRoll, Mean_AccTop;
 // neutral acceleration readings
 volatile int16_t NeutralAccX=0, NeutralAccY=0;
 volatile float NeutralAccZ = 0;
 // attitude gyro integrals
-volatile int32_t IntegralNick = 0,IntegralNick2 = 0;
+int32_t IntegralNick = 0,IntegralNick2 = 0;
-volatile int32_t IntegralRoll = 0,IntegralRoll2 = 0;
+int32_t IntegralRoll = 0,IntegralRoll2 = 0;
-volatile int32_t IntegralYaw = 0;
+int32_t IntegralYaw = 0;
-volatile int32_t Reading_IntegralGyroNick = 0, Reading_IntegralGyroNick2 = 0;
+int32_t Reading_IntegralGyroNick = 0, Reading_IntegralGyroNick2 = 0;
-volatile int32_t Reading_IntegralGyroRoll = 0,  Reading_IntegralGyroRoll2 = 0;
+int32_t Reading_IntegralGyroRoll = 0, Reading_IntegralGyroRoll2 = 0;
-volatile int32_t Reading_IntegralGyroYaw = 0;
+int32_t Reading_IntegralGyroYaw = 0;
-volatile int32_t MeanIntegralNick;
+int32_t MeanIntegralNick;
-volatile int32_t MeanIntegralRoll;
+int32_t MeanIntegralRoll;
 // attitude acceleration integrals
-volatile int32_t IntegralAccNick = 0, IntegralAccRoll = 0;
+int32_t IntegralAccNick = 0, IntegralAccRoll = 0;
-int16_t NaviAccNick = 0, NaviAccRoll = 0, NaviCntAcc = 0;
 int16_t NaviAccNick = 0, NaviAccRoll = 0, NaviCntAcc = 0;
 // flags
-uint8_t MotorsOn = 0;
+// MK flags
-uint8_t EmergencyLanding = 0;
+uint16_t Model_Is_Flying = 0;
-uint16_t Model_Is_Flying = 0;
+volatile uint8_t MKFlags = 0;
 int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L;
 /************************************************************************/
 void Beep(uint8_t numbeeps)
+{
         while(numbeeps--)
+        {
-                if(MotorsOn) return; //auf keinen Fall im Flug!
+                if(MKFlags & MKFLAG_MOTOR_RUN) return; //auf keinen Fall bei laufenden Motoren!
                 BeepTime = 100; // 0.1 second
                 Delay_ms(250); // blocks 250 ms as pause to next beep,
                 // this will block the flight control loop,
                 // therefore do not use this funktion if motors are running
+        }
     Reading_IntegralGyroRoll2 = 0;
     Reading_IntegralGyroYaw = 0;
     Reading_GyroNick = 0;
     Reading_GyroRoll = 0;
     Reading_GyroYaw = 0;
+    Delay_ms_Mess(100);
     StartAirPressure = AirPressure;
     HeightD = 0;
     Reading_Integral_Top = 0;
     CompassCourse = CompassHeading;
     BeepTime = 50;
     ExternHeightValue = 0;
     GPS_Nick = 0;
     GPS_Roll = 0;
     YawGyroHeading = CompassHeading * YAW_GYRO_DEG_FACTOR;
     YawGyroDrift = 0;
+    MKFlags |= MKFLAG_CALIBRATE;
+}
 /************************************************************************/
 /*  Averaging Measurement Readings                                      */
 /************************************************************************/
 /*  Averaging Measurement Readings  for Calibration                     */
 /************************************************************************/
 void CalibMean(void)
+{
+        if(BoardRelease >= 13) SearchGyroOffset();
     // stop ADC to avoid changing values during calculation
         ADC_Disable();
         Reading_GyroNick = AdValueGyrNick;
 /************************************************************************/
 /*  Transmit Motor Data via I2C                                         */
 /************************************************************************/
 void SendMotorData(void)
+{
-    if(MOTOR_OFF || !MotorsOn)
+    if(!(MKFlags & MKFLAG_MOTOR_RUN))
+    {
         Motor_Rear = 0;
         Motor_Front = 0;
         Motor_Right = 0;
         Motor_Left = 0;
         if(MotorTest[0]) Motor_Front = MotorTest[0];
         if(MotorTest[1]) Motor_Rear  = MotorTest[1];
         if(MotorTest[2]) Motor_Left  = MotorTest[2];
         if(MotorTest[3]) Motor_Right = MotorTest[3];
+        MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off
-     }
+    }
     DebugOut.Analog[12] = Motor_Front;
     DebugOut.Analog[13] = Motor_Rear;
     DebugOut.Analog[14] = Motor_Left;
     DebugOut.Analog[15] = Motor_Right;
     //Start I2C Interrupt Mode
-    twi_state = 0;
-    motor = 0;
+    twi_state = TWI_STATE_MOTOR_TX;
     I2C_Start();
+{
         if(RC_Quality > 160) // do the mapping of RC-Potis only if the rc-signal is ok
         // else the last updated values are used
+        {
                  //update poti values by rc-signals
-                #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
+                #define CHK_POTI_MM(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
+                #define CHK_POTI(b,a) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a;}
-                CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight,0,255);
+                CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight);
-                CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100);
+                CHK_POTI_MM(FCParam.Height_D,ParamSet.Height_D,0,100);
-                CHK_POTI(FCParam.Height_P,ParamSet.Height_P,0,100);
+                CHK_POTI_MM(FCParam.Height_P,ParamSet.Height_P,0,100);
-                CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect,0,255);
+                CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect);
-                CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect,0,255);
+                CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect);
-                CHK_POTI(FCParam.Gyro_P,ParamSet.Gyro_P,10,255);
+                CHK_POTI_MM(FCParam.Gyro_P,ParamSet.Gyro_P,10,255);
-                CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I,0,255);
+                CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I);
-                CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor,0,255);
+                CHK_POTI(FCParam.I_Factor,ParamSet.I_Factor);
-                CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1,0,255);
+                CHK_POTI(FCParam.UserParam1,ParamSet.UserParam1);
-                CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2,0,255);
+                CHK_POTI(FCParam.UserParam2,ParamSet.UserParam2);
-                CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3,0,255);
+                CHK_POTI(FCParam.UserParam3,ParamSet.UserParam3);
-                CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4,0,255);
+                CHK_POTI(FCParam.UserParam4,ParamSet.UserParam4);
-                CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5,0,255);
+                CHK_POTI(FCParam.UserParam5,ParamSet.UserParam5);
-                CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6,0,255);
+                CHK_POTI(FCParam.UserParam6,ParamSet.UserParam6);
-                CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7,0,255);
+                CHK_POTI(FCParam.UserParam7,ParamSet.UserParam7);
-                CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255);
+                CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8);
-                CHK_POTI(FCParam.ServoNickControl,ParamSet.ServoNickControl,0,255);
+                CHK_POTI(FCParam.ServoNickControl,ParamSet.ServoNickControl);
-                CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit,0,255);
+                CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit);
-                CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255);
+                CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback);
-                CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255);
+                CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback);
-                CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255);
+                CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability);
+                CHK_POTI_MM(FCParam.J16Timing,ParamSet.J16Timing,1,255);
+                CHK_POTI_MM(FCParam.J17Timing,ParamSet.J17Timing,1,255);
+                CHK_POTI(FCParam.NaviGpsModeControl,ParamSet.NaviGpsModeControl);
+                CHK_POTI(FCParam.NaviGpsGain,ParamSet.NaviGpsGain);
+                CHK_POTI(FCParam.NaviGpsP,ParamSet.NaviGpsP);
+                CHK_POTI(FCParam.NaviGpsI,ParamSet.NaviGpsI);
+                CHK_POTI(FCParam.NaviGpsD,ParamSet.NaviGpsD);
+                CHK_POTI(FCParam.NaviGpsACC,ParamSet.NaviGpsACC);
+                CHK_POTI(FCParam.ExternalControl,ParamSet.ExternalControl);
                 Ki = (float) FCParam.I_Factor * FACTOR_I;
+        }
+}
+                        }
+                }
                 if(RcLostTimer) RcLostTimer--; // decremtent timer after rc sigal lost
                 else // rc lost countdown finished
+                {
-                  MotorsOn = 0; // stop all motors
-                  EmergencyLanding = 0; // emergency landing is over
+                  MKFlags &= ~(MKFLAG_MOTOR_RUN|MKFLAG_EMERGENCY_LANDING); // clear motor run flag that stop the motors in SendMotorData()
+                }
-                ROT_ON; // set red led
+                RED_ON; // set red led
                 if(Model_Is_Flying > 1000)  // wahrscheinlich in der Luft --> langsam absenken
+                {
                         GasMixFraction = ParamSet.EmergencyGas; // set emergency gas
-                        EmergencyLanding = 1; // enable emergency landing
+                        MKFlags |= (MKFLAG_EMERGENCY_LANDING); // ser flag fpr emergency landing
                         // set neutral rc inputs
                         PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] = 0;
                         PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
                         PPM_diff[ParamSet.ChannelAssignment[CH_YAW]] = 0;
                         PPM_in[ParamSet.ChannelAssignment[CH_NICK]] = 0;
                         PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
                         PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0;
+                }
-                else MotorsOn = 0; // switch of all motors
+                else MKFlags &= ~(MKFLAG_MOTOR_RUN); // clear motor run flag that stop the motors in SendMotorData()
         } // eof RC_Quality < 120
         else
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // RC-signal is good
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(RC_Quality > 140)
+        {
-                EmergencyLanding = 0; // switch off emergency landing if RC-signal is okay
+                MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing
                 // reset emergency timer
                 RcLostTimer = ParamSet.EmergencyGasDuration * 50;
-                if(GasMixFraction > 40)
+                if(GasMixFraction > 40 && (MKFlags & MKFLAG_MOTOR_RUN) )
+                {
                         if(Model_Is_Flying < 0xFFFF) Model_Is_Flying++;
+                }
                 if(Model_Is_Flying < 256)
+                {
                         SumNick = 0;
                         SumRoll = 0;
                         StickYaw = 0;
-                        if(Model_Is_Flying == 250) UpdateCompassCourse = 1;
+                        if(Model_Is_Flying == 250)
+                        {
+                                UpdateCompassCourse = 1;
+                                Reading_IntegralGyroYaw = 0;
+                                SetPointYaw = 0;
+                        }
+                }
+                else MKFlags |= (MKFLAG_FLY); // set fly flag
                 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--;
                 if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
                 if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255;
                 if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255;
                 if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255;
                 // if motors are off and the gas stick is in the upper position
-                if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorsOn == 0)
+                if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && !(MKFlags & MKFLAG_MOTOR_RUN) )
+                {
                         // and if the yaw stick is in the leftmost position
                         if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75)
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+                        {
                                 if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
+                                {
                                         delay_startmotors = 200; // do not repeat if once executed
                                         Model_Is_Flying = 1;
-                                        MotorsOn = 1;
+                                        MKFlags |= (MKFLAG_MOTOR_RUN|MKFLAG_START); // set flag RUN and START
                                         SetPointYaw = 0;
                                         Reading_IntegralGyroYaw = 0;
                                         Reading_IntegralGyroNick = 0;
                                         Reading_IntegralGyroRoll = 0;
                                         Reading_IntegralGyroNick2 = IntegralNick;
                                         Reading_IntegralGyroRoll2 = IntegralRoll;
                                         SumNick = 0;
                                         SumRoll = 0;
-                                        #if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
-                                        if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
-                                        {
-                                                GPS_SetHomePosition();
-                                        }
-                                        #endif
+                                }
+                        }
                         else delay_startmotors = 0; // reset delay timer if sticks are not in this position
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // and yaw stick is leftmost --> stop motors
+                                {
                                 if(++delay_stopmotors > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
+                                {
                                         delay_stopmotors = 200; // do not repeat if once executed
                                         Model_Is_Flying = 0;
-                                        MotorsOn = 0;
-                                        #if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
-                                        if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
-                                        {
-                                                GPS_ClearHomePosition();
+                                        MKFlags &= ~(MKFLAG_MOTOR_RUN);
-                                        }
-                                        #endif
+                                }
+                        }
                         else delay_stopmotors = 0; // reset delay timer if sticks are not in this position
+                }
                         // remapping of paameters only if the signal rc-sigbnal conditions are good
         } // eof RC_Quality > 150
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // new values from RC
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        if(!NewPpmData-- || EmergencyLanding) // NewData = 0 means new data from RC
+        if(!NewPpmData-- || (MKFlags & MKFLAG_EMERGENCY_LANDING) ) // NewData = 0 means new data from RC
+        {
                 int tmp_int;
                 ParameterMapping(); // remapping params (online poti replacement)
                 // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
                 StickNick = (StickNick * 3 + PPM_in[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_P) / 4;
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Digital Control via DubWise
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-                #define KEY_VALUE (FCParam.UserParam8 * 4) // step width
+                #define KEY_VALUE (FCParam.ExternalControl * 4) // step width
                 if(DubWiseKeys[1]) BeepTime = 10;
                 if(DubWiseKeys[1] & DUB_KEY_UP)  tmp_int = KEY_VALUE;
                 else if(DubWiseKeys[1] & DUB_KEY_DOWN)  tmp_int = -KEY_VALUE;
                 if(DubWiseKeys[0] & 8)  ExternStickYaw = 50;else
                 if(DubWiseKeys[0] & 4)  ExternStickYaw =-50;else ExternStickYaw = 0;
                 if(DubWiseKeys[0] & 2)  ExternHeightValue++;
                 if(DubWiseKeys[0] & 16) ExternHeightValue--;
-                StickNick += (STICK_GAIN * ExternStickNick) / 8;
+                StickNick  += (STICK_GAIN * ExternStickNick) / 8;
                 StickRoll  += (STICK_GAIN * ExternStickRoll) / 8;
                 StickYaw   += (STICK_GAIN * ExternStickYaw);
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //+ Analog control via serial communication
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-                if(ExternControl.Config & 0x01 && FCParam.UserParam8 > 128)
+                if(ExternControl.Config & 0x01 && FCParam.ExternalControl > 128)
+                {
                          StickNick += (int16_t) ExternControl.Nick * (int16_t) ParamSet.Stick_P;
                 if(Gyro_I_Factor < 0) Gyro_I_Factor = 0;
+                // update max stick positions for nick and roll
+                if(abs(StickNick / STICK_GAIN) > MaxStickNick)
+                {
-                // update max stick positions for nick and roll
+                        MaxStickNick = abs(StickNick)/STICK_GAIN;
+                        if(MaxStickNick > 100) MaxStickNick = 100;
+                }
+                else MaxStickNick--;
+                if(abs(StickRoll / STICK_GAIN) > MaxStickRoll)
+                {
-                if(abs(StickNick / STICK_GAIN) > MaxStickNick) MaxStickNick = abs(StickNick)/STICK_GAIN;
+                        MaxStickRoll = abs(StickRoll)/STICK_GAIN;
-                else MaxStickNick--;
+                        if(MaxStickRoll > 100) MaxStickRoll = 100;
                 if(abs(StickRoll / STICK_GAIN) > MaxStickRoll) MaxStickRoll = abs(StickRoll)/STICK_GAIN;
                 else MaxStickRoll--;
         if(Looping_Roll || Looping_Nick)
+        {
                 if(GasMixFraction > ParamSet.LoopGasLimit) GasMixFraction = ParamSet.LoopGasLimit;
+        }
-//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-//+ LED Control on J16/J17
-//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        LED1_Time = FCParam.UserParam7;
-        LED2_Time = FCParam.UserParam8;
-        LED_Update();
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // in case of emergency landing
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         // set all inputs to save values
-        if(EmergencyLanding)
+        if(MKFlags & MKFLAG_EMERGENCY_LANDING)
+        {
                 StickYaw = 0;
                 StickNick = 0;
                 StickRoll = 0;
                 tmp_long   =  (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);
                 tmp_long  /= 16;
                 tmp_long2  = (int32_t)(IntegralRoll   / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);
                 tmp_long2 /= 16;
-                if((MaxStickNick > 32) || (MaxStickRoll > 32)) // reduce effect during stick commands
+                if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
+                {
                         tmp_long  /= 3;
                         tmp_long2 /= 3;
+                }
                         // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
                         IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);
                         CorrectionRoll  = IntegralErrorRoll / ParamSet.GyroAccTrim;
                         AttitudeCorrectionRoll  = CorrectionRoll  / BALANCE_NUMBER;
-                        if((MaxStickNick > 32) || (MaxStickRoll > 32) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25))
+                        if((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25))
+                        {
                                 AttitudeCorrectionNick /= 2;
                                 AttitudeCorrectionRoll /= 2;
                                 else  last_n_n = 0;
+                        }
                         else
+                        {
                                 cnt = 0;
-                                BadCompassHeading = 500;
+                                BadCompassHeading = 1000;
+                        }
                         if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
                         // correct Gyro Offsets
                         if(IntegralErrorNick >  ERROR_LIMIT)   AdNeutralNick += cnt;
                         if(IntegralErrorNick < -ERROR_LIMIT)   AdNeutralNick -= cnt;
                                 else  last_r_n = 0;
+                        }
                         else
+                        {
                                 cnt = 0;
-                                BadCompassHeading = 500;
+                                BadCompassHeading = 1000;
+                        }
                         // correct Gyro Offsets
                         if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
                         if(IntegralErrorRoll >  ERROR_LIMIT)   AdNeutralRoll += cnt;
                         if(IntegralErrorRoll < -ERROR_LIMIT)   AdNeutralRoll -= cnt;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Yawing
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(abs(StickYaw) > 15 ) // yaw stick is activated
+        {
+                BadCompassHeading = 1000;
                 if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX))
+                {
                         UpdateCompassCourse = 1;
-                        CompassCourse = YawGyroHeading;
-                        BadCompassHeading = 250;
+                }
+        }
         // 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;
     // compass code is used if Compass option is selected
         if((ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE))
+        {
                 int16_t w, v, r,correction, error;
-                if(CompassCalState && MotorsOn == 0 )
+                if(CompassCalState && !(MKFlags & MKFLAG_MOTOR_RUN) )
+                {
                         SetCompassCalState();
                         #ifdef USE_KILLAGREG
                         MM3_Calibrate();
                                 MM3_Heading();
+                        }
                         #endif
                         // get maximum attitude angle
-                        w = abs(IntegralNick/512);
+                        w = abs(IntegralNick / 512);
-                        v = abs(IntegralRoll /512);
+                        v = abs(IntegralRoll / 512);
-                        if(v > w) w = v;
-                        // update compass course
-                        if (w < 25 && UpdateCompassCourse && !BadCompassHeading)
-                        {
+                        if(v > w) w = v;
-                                BeepTime = 200;
-                                CompassCourse = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
-                                UpdateCompassCourse = 0;
                         correction = w / 8 + 1;
                         // calculate the deviation of the yaw gyro heading and the compass heading
                         if (CompassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined
+                        else error = ((540 + CompassHeading - (YawGyroHeading / YAW_GYRO_DEG_FACTOR)) % 360) - 180;
+                        if(!BadCompassHeading && w < 25)
+                        {
+                                YawGyroDrift += error;
+                                if(UpdateCompassCourse)
-                        else error = ((540 + CompassHeading - (YawGyroHeading / YAW_GYRO_DEG_FACTOR)) % 360) - 180;
+                                {
+                                        BeepTime = 200;
+                                        CompassCourse = (YawGyroHeading / YAW_GYRO_DEG_FACTOR);
+                                        UpdateCompassCourse = 0;
+                                }
                         correction = w / 8 + 1;
                         YawGyroHeading += (error * 8) / correction;
-                        w = (w * FCParam.CompassYawEffect) / 64;
+                        w = (w * FCParam.CompassYawEffect) / 32;
                         w = FCParam.CompassYawEffect - w;
-                        if(w > 0)
+                        if(w >= 0)
                         {
-                                if(BadCompassHeading)
-                                {       // wait a while
-                                        BadCompassHeading--;
+                                if(!BadCompassHeading)
-                                }
-                                else
-                                {   //
                                         YawGyroDrift += error;
                                         v = 64 + (MaxStickNick + MaxStickRoll) / 8;
                                         // calc course deviation
                                         r = ((540 + (YawGyroHeading / YAW_GYRO_DEG_FACTOR) - CompassCourse) % 360) - 180;
                                         v = (r * w) / v; // align to compass course
                                         // limit yaw rate
                                         w = 3 * FCParam.CompassYawEffect;
                                         if (v > w) v = w;
                                         else if (v < -w) v = -w;
                                         Reading_IntegralGyroYaw += v;
+                                }
+                                else
+                                { // wait a while
+                                        BadCompassHeading--;
+                                }
+                        }
                         else
                         {  // ignore compass at extreme attitudes for a while
-                                BadCompassHeading = 250;
+                                BadCompassHeading = 500;
+                        }
+                }
+        }
         #if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  GPS
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
-        {
-                GPS_I_Factor = FCParam.UserParam2;
+        if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
-                GPS_P_Factor = FCParam.UserParam5;
+        {
-                GPS_D_Factor = FCParam.UserParam6;
-                if(EmergencyLanding) GPS_Main(230); // enables Comming Home
+                GPS_Main();
-                else GPS_Main(Poti3);               // behavior controlled by Poti3
+                MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
+        }
         else
+        {
                 DebugOut.Analog[6]  = (Reading_Integral_Top / 512);
                 DebugOut.Analog[8]  = CompassHeading;
                 DebugOut.Analog[9]  = UBat;
                 DebugOut.Analog[10] = RC_Quality;
                 DebugOut.Analog[11] = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
-                DebugOut.Analog[16] = Mean_AccTop;
+                //DebugOut.Analog[16] = Mean_AccTop;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         GasMixFraction *= STICK_GAIN;
         // If height control is activated and no emergency landing is active
-        if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) )
+        if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && !(MKFlags & MKFLAG_EMERGENCY_LANDING) )
+        {
                 int tmp_int;
                 // if height control is activated by an rc channel
         // Motor Front
     MotorValue = GasMixFraction + pd_result + YawMixFraction;     // Mixer
     MotorValue /= STICK_GAIN;
         if ((MotorValue < 0)) MotorValue = 0;
-        else if(MotorValue > ParamSet.Gas_Max)              MotorValue = ParamSet.Gas_Max;
+        else if(MotorValue > ParamSet.Gas_Max)            MotorValue = ParamSet.Gas_Max;
         if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
         Motor_Front = MotorValue;
  // Motor Rear
         MotorValue = GasMixFraction - pd_result + YawMixFraction;     // Mixer
         MotorValue /= STICK_GAIN;
         if ((MotorValue < 0)) MotorValue = 0;
         else if(MotorValue > ParamSet.Gas_Max)      MotorValue = ParamSet.Gas_Max;
-        if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
+        if (MotorValue < ParamSet.Gas_Min)          MotorValue = ParamSet.Gas_Min;
         Motor_Rear = MotorValue;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Axis
     // Motor Left
     MotorValue = GasMixFraction + pd_result - YawMixFraction;  // Mixer
     MotorValue /= STICK_GAIN;
         if ((MotorValue < 0)) MotorValue = 0;
         else if(MotorValue > ParamSet.Gas_Max)          MotorValue = ParamSet.Gas_Max;
-        if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
+        if (MotorValue < ParamSet.Gas_Min)          MotorValue = ParamSet.Gas_Min;
     Motor_Left = MotorValue;
  // Motor Right
         MotorValue = GasMixFraction - pd_result - YawMixFraction;  // Mixer
         MotorValue /= STICK_GAIN;
         if ((MotorValue < 0)) MotorValue = 0;
         else if(MotorValue > ParamSet.Gas_Max)          MotorValue = ParamSet.Gas_Max;
-        if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
+        if (MotorValue < ParamSet.Gas_Min)          MotorValue = ParamSet.Gas_Min;
     Motor_Right = MotorValue;

Subversion Repositories FlightCtrl

(root)/branches/V0.70d Code Redesign killagreg/fc.c - Rev 935 → 936