WebSVN - FlightCtrl - Diff - Rev 1077 and 1078 - /branches/V0.71h Code Redesign killagreg/fc.c

 #ifdef USE_MK3MAG
 #include "mk3mag.h"
 #include "gps.h"
 #endif
 #include "led.h"
+#include "spi.h"
 // gyro readings
 int16_t Reading_GyroNick, Reading_GyroRoll, Reading_GyroYaw;
 // gyro neutral readings
 // stick values derived by uart inputs
 int16_t ExternStickNick = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20;
     AdNeutralNick = 0;
         AdNeutralRoll = 0;
         AdNeutralYaw = 0;
     FCParam.Yaw_PosFeedback = 0;
     FCParam.Yaw_NegFeedback = 0;
+    ExpandBaro = 0;
     CalibMean();
     Delay_ms_Mess(100);
         CalibMean();
     if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL))  // Height Control activated?
+    {
     GPS_Nick = 0;
     GPS_Roll = 0;
     YawGyroHeading = CompassHeading * YAW_GYRO_DEG_FACTOR;
     YawGyroDrift = 0;
     MKFlags |= MKFLAG_CALIBRATE;
+    FromNaviCtrl_Value.Kalman_K = -1;
+        FromNaviCtrl_Value.Kalman_MaxDrift = ParamSet.DriftComp * 16;
+        FromNaviCtrl_Value.Kalman_MaxFusion = 32;
+}
 /************************************************************************/
 /*  Averaging Measurement Readings                                      */
 /************************************************************************/
 /*  Averaging Measurement Readings  for Calibration                     */
 /************************************************************************/
 void CalibMean(void)
+{
-        if(BoardRelease >= 13) SearchGyroOffset();
+        if(BoardRelease == 13) SearchGyroOffset();
     // stop ADC to avoid changing values during calculation
         ADC_Disable();
         Reading_GyroNick = AdValueGyrNick;
                 CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback);
                 CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback);
                 CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability);
                 CHK_POTI_MM(FCParam.J16Timing,ParamSet.J16Timing,1,255);
                 CHK_POTI_MM(FCParam.J17Timing,ParamSet.J17Timing,1,255);
+                #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG))
                 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_MM(FCParam.NaviOperatingRadius,ParamSet.NaviOperatingRadius,10, 255);
+                CHK_POTI(FCParam.NaviWindCorrection,ParamSet.NaviWindCorrection);
+                CHK_POTI(FCParam.NaviSpeedCompensation,NaviSpeedCompensation.NaviGpsACC);
+                #endif
                 CHK_POTI(FCParam.ExternalControl,ParamSet.ExternalControl);
                 Ki = (float) FCParam.I_Factor * FACTOR_I;
+        }
+}
         // if nick is down trigger to next cal state
         if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70) && !stick)
+        {
                 stick = 1;
                 CompassCalState++;
                 if(CompassCalState < 5) Beep(CompassCalState);
                 else BeepTime = 1000;
+        }
+}
                                         else
+                                        {
                                                 if(ParamSet.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE))
+                                                {
                                                         // if roll stick is centered and nick stick is down
-                                                        if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 20 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70)
+                                                        if (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < 30 && PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -70)
+                                                        {
                                                                 // nick/roll joystick
                                                                 //  _________
                                                                 // |         |
                                                                 // |         |
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // new values 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;
                 StickNick += PPM_diff[ParamSet.ChannelAssignment[CH_NICK]] * ParamSet.Stick_D;
                 StickNick -= (GPS_Nick);
                 // update gyro control loop factors
                 Gyro_P_Factor = ((float) FCParam.Gyro_P + 10.0) / (256.0 / STICK_GAIN);
-                Gyro_I_Factor = ((float) FCParam.Gyro_I) / (44000 / STICK_GAIN);
-//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Digital Control via DubWise
-//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-                #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;
-                else tmp_int = 0;
-                ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;
-                if(DubWiseKeys[1] & DUB_KEY_LEFT)  tmp_int = KEY_VALUE;
-                else if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE;
-                else tmp_int = 0;
-                ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
-                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;
-                StickRoll  += (STICK_GAIN * ExternStickRoll) / 8;
+                Gyro_I_Factor = ((float) FCParam.Gyro_I) / (44000 / STICK_GAIN);
                 StickYaw   += (STICK_GAIN * ExternStickYaw);
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Looping?
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LEFT)  Looping_Left = 1;
+                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_LEFT)  Looping_Left = 1;
                 else
+                {
                         if(Looping_Left) // Hysteresis
+                        {
                                 if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Left = 0;
                         }
+                }
-                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RIGHT) Looping_Right = 1;
+                if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_RIGHT) Looping_Right = 1;
                 else
+                {
                         if(Looping_Right) // Hysteresis
+                        {
                                 if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Right = 0;
                         }
+                }
-                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Top = 1;
+                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] > ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_UP) Looping_Top = 1;
                 else
+                {
                         if(Looping_Top)  // Hysteresis
+                        {
                                 if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0;
+                        }
+                }
-                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Down = 1;
+                if((PPM_in[ParamSet.ChannelAssignment[CH_NICK]] < -ParamSet.LoopThreshold) && ParamSet.BitConfig & CFG_LOOP_DOWN) Looping_Down = 1;
                 else
+                {
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(!Looping_Nick && !Looping_Roll) // if not lopping in any direction
+        {
-                int32_t tmp_long, tmp_long2;
-                // determine the deviation of gyro integral from averaged acceleration sensor
-                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);
+                int32_t tmp_long, tmp_long2;
-                tmp_long2 /= 16;
-                if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
+                if(FromNaviCtrl_Value.Kalman_K != -1)
+                {
+                        // determine the deviation of gyro integral from averaged acceleration sensor
+                        tmp_long   = (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);
+                        tmp_long   = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
+                        tmp_long2  = (int32_t)(IntegralRoll   / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);
+                        tmp_long2  = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
+                        if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
+                        {
+                                tmp_long  /= 2;
+                                tmp_long2 /= 2;
+                        }
+                        if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active
-                {
+                        {
-                        tmp_long  /= 3;
+                                tmp_long  /= 3;
+                                tmp_long2 /= 3;
+                        }
+                        // limit correction effect
+                        if(tmp_long >  (int32_t)FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (int32_t)FromNaviCtrl_Value.Kalman_MaxFusion;
+                        if(tmp_long < -(int32_t)FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  =-(int32_t)FromNaviCtrl_Value.Kalman_MaxFusion;
+                        if(tmp_long2 > (int32_t)FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 = (int32_t)FromNaviCtrl_Value.Kalman_MaxFusion;
-                        tmp_long2 /= 3;
+                        if(tmp_long2 <-(int32_t)FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 =-(int32_t)FromNaviCtrl_Value.Kalman_MaxFusion;
-                }
+                }
-                if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active
+                else
+                {
+                        // determine the deviation of gyro integral from averaged acceleration sensor
-                {
+                        tmp_long   =  (int32_t)(IntegralNick / ParamSet.GyroAccFactor - (int32_t)Mean_AccNick);
+                        tmp_long  /= 16;
-                        tmp_long  /= 3;
+                        tmp_long2  = (int32_t)(IntegralRoll   / ParamSet.GyroAccFactor - (int32_t)Mean_AccRoll);
-                        tmp_long2 /= 3;
+                        tmp_long2 /= 16;
+                        if((MaxStickNick > 64) || (MaxStickRoll > 64)) // reduce effect during stick commands
+                        {
+                                tmp_long  /= 3;
+                                tmp_long2 /= 3;
+                        }
+                        if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further if yaw stick is active
+                        {
+                                tmp_long  /= 3;
+                                tmp_long2 /= 3;
-                }
+                        }
-                #define BALANCE 32
+                        #define BALANCE 32
-                // limit correction effect
+                        // limit correction effect
-                if(tmp_long >  BALANCE)  tmp_long  = BALANCE;
+                        if(tmp_long >  BALANCE)  tmp_long  = BALANCE;
-                if(tmp_long < -BALANCE)  tmp_long  =-BALANCE;
+                        if(tmp_long < -BALANCE)  tmp_long  =-BALANCE;
+                        if(tmp_long2 > BALANCE)  tmp_long2 = BALANCE;
-                if(tmp_long2 > BALANCE)  tmp_long2 = BALANCE;
+                        if(tmp_long2 <-BALANCE)  tmp_long2 =-BALANCE;
                 if(tmp_long2 <-BALANCE)  tmp_long2 =-BALANCE;
                 // correct current readings
                 Reading_IntegralGyroNick -= tmp_long;
                 Reading_IntegralGyroRoll -= tmp_long2;
                         // 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 > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25))
+                        if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1))
+                        {
                                 AttitudeCorrectionNick /= 2;
                                 AttitudeCorrectionRoll /= 2;
                         Reading_IntegralGyroRoll2 -= IntegralErrorRoll;
                         if(YawGyroDrift >  BALANCE_NUMBER/2) AdNeutralYaw++;
                         if(YawGyroDrift < -BALANCE_NUMBER/2) AdNeutralYaw--;
-                        YawGyroDrift = 0;
-/*
-                        DebugOut.Analog[17] = IntegralAccNick / 26;
-                        DebugOut.Analog[18] = IntegralAccRoll / 26;
-                        DebugOut.Analog[19] = IntegralErrorNick;// / 26;
-                        DebugOut.Analog[20] = IntegralErrorRoll;// / 26;
-                        DebugOut.Analog[21] = MeanIntegralNick / 26;
-                        DebugOut.Analog[22] = MeanIntegralRoll / 26;
-                        //DebugOut.Analog[28] = CorrectionNick;
-                        DebugOut.Analog[29] = CorrectionRoll;
-                        DebugOut.Analog[30] = AttitudeCorrectionRoll * 10;
-*/
+                        YawGyroDrift = 0;
                         #define ERROR_LIMIT  (BALANCE_NUMBER * 4)
                         #define ERROR_LIMIT2 (BALANCE_NUMBER * 16)
                         #define MOVEMENT_LIMIT 20000
         // Nick +++++++++++++++++++++++++++++++++++++++++++++++++
                         cnt = 1;// + labs(IntegralErrorNick) / 4096;
                         CorrectionNick = 0;
-                        if(labs(MeanIntegralNick_old - MeanIntegralNick) < MOVEMENT_LIMIT)
+                        if((labs(MeanIntegralNick_old - MeanIntegralNick) < MOVEMENT_LIMIT) || (FromNaviCtrl_Value.Kalman_MaxDrift > 3* 16))
+                        {
                                 if(IntegralErrorNick >  ERROR_LIMIT2)
+                                {
+                        {
                                 cnt = 0;
                                 BadCompassHeading = 1000;
+                        }
                         if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
+                        if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift / 16;
                         // correct Gyro Offsets
                         if(IntegralErrorNick >  ERROR_LIMIT)   AdNeutralNick += cnt;
                         if(IntegralErrorNick < -ERROR_LIMIT)   AdNeutralNick -= cnt;
         // Roll +++++++++++++++++++++++++++++++++++++++++++++++++
                         cnt = 1;// + labs(IntegralErrorNick) / 4096;
                         CorrectionRoll = 0;
-                        if(labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT)
+                        if((labs(MeanIntegralRoll_old - MeanIntegralRoll) < MOVEMENT_LIMIT) || (FromNaviCtrl_Value.Kalman_MaxDrift > 3* 16))
+                        {
                                 if(IntegralErrorRoll >  ERROR_LIMIT2)
+                                {
                                         if(last_r_p)
                                 cnt = 0;
                                 BadCompassHeading = 1000;
+                        }
                         // correct Gyro Offsets
                         if(cnt > ParamSet.DriftComp) cnt = ParamSet.DriftComp;
+                        if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift / 16;
                         if(IntegralErrorRoll >  ERROR_LIMIT)   AdNeutralRoll += cnt;
                         if(IntegralErrorRoll < -ERROR_LIMIT)   AdNeutralRoll -= cnt;
-/*
-                        DebugOut.Analog[27] = CorrectionRoll;
-                        DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
-                        DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
-*/
+                }
                 else // looping is active
+                {
                         AttitudeCorrectionRoll  = 0;
                         AttitudeCorrectionNick = 0;
                         if(v > w) w = v;
                         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(UpdateCompassCourse)
+                        {
+                                error = 0;
+                                YawGyroHeading = CompassHeading * YAW_GYRO_DEG_FACTOR;
+                        }
                         if(!BadCompassHeading && w < 25)
+                        {
                                 YawGyroDrift += error;
                                 if(UpdateCompassCourse)
+                                {
                 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[17] = FromNaviCtrl_Value.Distance;
+                DebugOut.Analog[18] = FromNaviCtrl_Value.OsdBar;
                 DebugOut.Analog[20] = ServoValue;
+                DebugOut.Analog[27] = (int16_t)FromNaviCtrl_Value.Kalman_MaxDrift;
+                DebugOut.Analog[29] = (int16_t)FromNaviCtrl_Value.Kalman_K;
                 DebugOut.Analog[30] = GPS_Nick;
                 DebugOut.Analog[31] = GPS_Roll;
-                /*    DebugOut.Analog[16] = motor_rx[0];
-                DebugOut.Analog[17] = motor_rx[1];
-                DebugOut.Analog[18] = motor_rx[2];
-                DebugOut.Analog[19] = motor_rx[3];
-                DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
-                DebugOut.Analog[20] /= 14;
-                DebugOut.Analog[21] = motor_rx[4];
-                DebugOut.Analog[22] = motor_rx[5];
-                DebugOut.Analog[23] = motor_rx[6];
-                DebugOut.Analog[24] = motor_rx[7];
-                DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
-                DebugOut.Analog[9]  = Reading_GyroNick;
-                DebugOut.Analog[9]  = SetPointHeight;
-                DebugOut.Analog[10] = Reading_IntegralGyroYaw / 128;
-                DebugOut.Analog[10] = FCParam.Gyro_I;
-                DebugOut.Analog[10] = ParamSet.Gyro_I;
-                DebugOut.Analog[9]  = CompassOffCourse;
-                DebugOut.Analog[10] = GasMixFraction;
-                DebugOut.Analog[3]  = HeightD * 32;
-                DebugOut.Analog[4]  = HeightControlGas;
                 */
+        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // The height control algorithm reduces the gas but does not increase the gas.
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         GasMixFraction *= STICK_GAIN;
-        // If height control is activated and no emergency landing is active
+        // if height control is activated and no emergency landing is active
         if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && !(MKFlags & MKFLAG_EMERGENCY_LANDING) )
+        {
                 int tmp_int;
-                int tmp_int;
+                static uint8_t delay = 100;
                 // if height control is activated by an rc channel
+                if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH)
+                {       // check if parameter is less than activation threshold
+                        if(
+                                ( (ParamSet.BitConfig & CFG_HEIGHT_3SWITCH) && ( (FCParam.MaxHeight > 80) && (FCParam.MaxHeight < 140) ) )|| // for 3-state switch height control is only disabled in center position
+                                (!(ParamSet.BitConfig & CFG_HEIGHT_3SWITCH) && (FCParam.MaxHeight < 50) ) // for 2-State switch height control is disabled in lower position
+                        )
+                        {   //hight control not active
+                                if(!delay--)
-                if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH)
+                                {
+                                        // measurement of air pressure close to upper limit
+                                        if(ReadingAirPressure > 1000)
+                                        {   // lower offset
+                                                ExpandBaro -= 10;
+                                                OCR0A = PressureSensorOffset - ExpandBaro;
+                                                BeepTime = 300;
+                                        delay = 250;
+                                        }
+                                        // measurement of air pressure close to lower limit
+                                        else if(ReadingAirPressure < 100)
+                                        {   // increase offset
+                                                ExpandBaro += 10;
+                                                OCR0A = PressureSensorOffset - ExpandBaro;
+                                                BeepTime = 300;
+                                        delay = 250;
-                {       // check if parameter is less than activation threshold
+                                        }
-                        if(FCParam.MaxHeight < 50)
+                                        else
-                        {
+                                        {
+                                                SetPointHeight = ReadingHeight - 20;  // update SetPoint with current reading
+                                                HeightControlActive = 0; // disable height control
+                                                delay = 1;
+                                        }
+                                }
+                        }
+                        else
+                        {       //hight control not active
-                                SetPointHeight = ReadingHeight - 20;  // update SetPoint with current reading
+                                HeightControlActive = 1; // enable height control
-                                HeightControlActive = 0; // disable height control
                                 delay = 200;
                         else HeightControlActive = 1; // enable height control
+                }
                 else // no switchable height control
+                {

Subversion Repositories FlightCtrl

(root)/branches/V0.71h Code Redesign killagreg/fc.c - Rev 1077 → 1078