Subversion Repositories FlightCtrl

Rev

Rev 1955 | Rev 1961 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1955 Rev 1960
Line 62... Line 62...
62 
        static uint8_t setHeightLatch = 0;
 62 
  static uint8_t setHeightLatch = 0;
Line 63... Line 63...
63 
 
 63 
 
64 
        if (height > maxHeight)
 64 
  if (height > maxHeight)
Line 65... Line 65...
65 
                maxHeight = height;
 65 
    maxHeight = height;
66 
 
 66 
 
67 
        if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) {
 67 
  if (staticParams.bitConfig & CFG_HEIGHT_SWITCH) {
68 
                // If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle.
 68 
    // If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle.
69 
                // DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH;
 69 
    // DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH;
70 
                if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40) {
 70 
    if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) {
71 
                        // Switch is ON
 71 
      // Switch is ON
72 
                        if (setHeightLatch <= LATCH_TIME) {
 72 
      if (setHeightLatch <= LATCH_TIME) {
73 
                                if (setHeightLatch == LATCH_TIME) {
 73 
        if (setHeightLatch == LATCH_TIME) {
Line 84... Line 84...
84 
                        // DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH;
 84 
      // DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH;
85 
                }
 85 
    }
86 
        } else {
 86 
  } else {
87 
                // Switch is not activated; take the "max-height" as the target height.
 87 
    // Switch is not activated; take the "max-height" as the target height.
88 
                // DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH;
 88 
    // DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH;
89 
                targetHeight = (uint16_t) dynamicParams.MaxHeight * 100; //getHeight() + 10 * 100;
 89 
    targetHeight = (uint16_t) dynamicParams.heightSetting * 100; //getHeight() + 10 * 100;
90 
        }
 90 
  }
Line 91... Line 91...
91 
 
 91 
 
92 
        if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
 92 
  if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
93 
                heightRampingTimer = 0;
 93 
    heightRampingTimer = 0;
94 
                if (rampedTargetHeight + staticParams.Height_Gain <= targetHeight) {
 94 
    if (rampedTargetHeight + staticParams.heightSlewRate <= targetHeight) {
95 
                        rampedTargetHeight += staticParams.Height_Gain;
 95 
      rampedTargetHeight += staticParams.heightSlewRate;
96 
                } else if (rampedTargetHeight - staticParams.Height_Gain >= targetHeight) {
 96 
    } else if (rampedTargetHeight - staticParams.heightSlewRate >= targetHeight) {
97 
                        rampedTargetHeight -= staticParams.Height_Gain;
 97 
      rampedTargetHeight -= staticParams.heightSlewRate;
98 
                }
 98 
    }
Line 99... Line 99...
99 
        }
 99 
  }
100 
 
 100 
 
Line 136... Line 136...
136 
         else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}
 136 
    else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}
137 
         else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}
 137 
    else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}
138 
         else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
 138 
    else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
139 
         */
 139 
  */
Line 140... Line 140...
140 
 
 140 
 
141 
        int16_t dThrottle = heightError * staticParams.HeightP / 1000
 141 
  int16_t dThrottle = heightError * staticParams.heightP / 1000
142 
                        /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight
 142 
    /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight
Line 143... Line 143...
143 
                                        * staticParams.HeightD;
 143 
    * staticParams.heightD;
144 
 
 144 
 
145 
        // the "minGas" is now a limit for how much up / down the throttle can be varied
 145 
  // the "minGas" is now a limit for how much up / down the throttle can be varied
146 
        if (dThrottle > staticParams.HeightMinGas)
 146 
  if (dThrottle > staticParams.heightMaxThrottleChange)
147 
                dThrottle = staticParams.HeightMinGas;
 147 
    dThrottle = staticParams.heightMaxThrottleChange;
Line 148... Line 148...
148 
        else if (dThrottle < -staticParams.HeightMinGas)
 148 
  else if (dThrottle < -staticParams.heightMaxThrottleChange)
149 
                dThrottle = -staticParams.HeightMinGas;
 149 
    dThrottle = -staticParams.heightMaxThrottleChange;
150 
 
 150 
 
151 
        // TODO: Eliminate repetition.
 151 
        // TODO: Eliminate repetition.
152 
        if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) {
 152 
  if (staticParams.bitConfig & CFG_HEIGHT_CONTROL) {
153 
                if (!(staticParams.GlobalConfig & CFG_HEIGHT_SWITCH)
 153 
    if (!(staticParams.bitConfig & CFG_HEIGHT_SWITCH)
154 
                                || (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40)) {
 154 
        || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) {
155 
                        // If switch is not in use --> Just apply height control.
 155 
      // If switch is not in use --> Just apply height control.
156 
                        // If switch is in use     --> only apply height control when switch is also ON.
 156 
      // If switch is in use     --> only apply height control when switch is also ON.