Subversion Repositories FlightCtrl

Rev

Rev 1986 | Rev 2027 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1986 Rev 2026
Line 63... Line 63...
63 
      // Switch is OFF.
 63 
      // Switch is OFF.
64 
      setHeightLatch = 0;
 64 
      setHeightLatch = 0;
65 
    }
 65 
    }
66 
  } else {
 66 
  } else {
67 
    // Switch is not activated; take the "max-height" as the target height.
 67 
    // Switch is not activated; take the "max-height" as the target height.
68 
    targetHeight = (uint16_t) dynamicParams.heightSetting * 100;
 68 
    targetHeight = (uint16_t) dynamicParams.heightSetting * 5L - 500L; // should be: 100 (or make a param out of it)
69 
  }
 69 
  }
Line 70... Line 70...
70 
 
 70 
 
71 
  if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
 71 
  if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
72 
    heightRampingTimer = 0;
 72 
    heightRampingTimer = 0;
Line 75... Line 75...
75 
      if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) {
 75 
      if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) {
76 
        rampedTargetHeight += staticParams.heightSlewRate;
 76 
        rampedTargetHeight += staticParams.heightSlewRate;
77 
      } else {
 77 
      } else {
78 
        rampedTargetHeight = targetHeight;
 78 
        rampedTargetHeight = targetHeight;
79 
      }
 79 
      }
80 
    } else if (rampedTargetHeight != targetHeight) {
 80 
    } else {
81 
      // descending
 81 
      // descending
82 
      if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) {
 82 
      if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) {
83 
        rampedTargetHeight -= staticParams.heightSlewRate;
 83 
        rampedTargetHeight -= staticParams.heightSlewRate;
84 
      } else {
 84 
      } else {
85 
        rampedTargetHeight = targetHeight;
 85 
        rampedTargetHeight = targetHeight;
Line 97... Line 97...
97 
// takes 180-200 usec (with integral term). That is too heavy!!!
 97 
// takes 180-200 usec (with integral term). That is too heavy!!!
98 
// takes 100 usec without integral term.
 98 
// takes 100 usec without integral term.
99 
uint16_t HC_getThrottle(uint16_t throttle) {
 99 
uint16_t HC_getThrottle(uint16_t throttle) {
100 
  int32_t height = getHeight();
 100 
  int32_t height = getHeight();
101 
  int32_t heightError = rampedTargetHeight - height;
 101 
  int32_t heightError = rampedTargetHeight - height;
102 
 
 102 
 
103 
  static int32_t lastHeight;
 103 
  static int32_t lastHeight;
Line 104... Line 104...
104 
 
 104 
 
105 
  int16_t dHeight = height - lastHeight;
 105 
  int16_t dHeight = height - lastHeight;
Line 109... Line 109...
109 
  // DebugOut.Analog[21] = dynamicParams.MaxHeight;
 109 
  // DebugOut.Analog[21] = dynamicParams.MaxHeight;
Line 110... Line 110...
110 
 
 110 
 
111 
  // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
 111 
  // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
Line 112... Line 112...
112 
  // iHeight += heightError;
 112 
  // iHeight += heightError;
113 
 
 113 
 
114 
  if (dHeight > 0) {
 114 
  if (heightError > 0) {
115 
    debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
 115 
    debugOut.digital[0] |= DEBUG_HEIGHT_DIFF;
116 
    debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
 116 
    debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF;
117 
  } else if (dHeight < 0) {
 117 
  } else if (heightError < 0) {
118 
    debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
 118 
    debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF;
Line 119... Line 119...
119 
    debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
 119 
    debugOut.digital[1] |= DEBUG_HEIGHT_DIFF;
Line 124... Line 124...
124 
    else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}
 124 
    else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }}
125 
    else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}
 125 
    else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;}
126 
    else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
 126 
    else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;}
127 
  */
 127 
  */
Line 128... Line 128...
128 
 
 128 
 
129 
  int16_t dThrottle = ((heightError * staticParams.heightP) << 10)
 129 
  int16_t dThrottle = ((heightError * staticParams.heightP) >> 9)
Line 130... Line -...
130 
    /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight * staticParams.heightD;
 - 
 
131 
 
 130 
    /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */-((dHeight * staticParams.heightD) >> 7);
132 
  // the "minGas" is now a limit for how much up / down the throttle can be varied
 131 
 
133 
  if (dThrottle > staticParams.heightControlMaxThrottleChange)
 132 
  if (dThrottle > staticParams.heightControlMaxThrottleChange)
134 
    dThrottle = staticParams.heightControlMaxThrottleChange;
 133 
    dThrottle = staticParams.heightControlMaxThrottleChange;
135 
  else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
 134 
  else if (dThrottle < -staticParams.heightControlMaxThrottleChange)
- 
 
 135 
    dThrottle = -staticParams.heightControlMaxThrottleChange;
- 
 
 136 
 
- 
 
 137 
  debugOut.analog[19] = rampedTargetHeight;
- 
 
 138 
  debugOut.analog[21] = dThrottle;
136 
    dThrottle = -staticParams.heightControlMaxThrottleChange;
 139 
  debugOut.analog[26] = height;
137 
 
 140 
 
138 
  if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
 141 
  if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) {
139 
    if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)
 142 
    if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH)
140 
        || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) {
 143 
        || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) {