Rev 2027 | Rev 2033 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2027 | Rev 2032 | ||
|---|---|---|---|
| Line 56... | Line 56... | ||
| 56 | // Switch is OFF. |
56 | // Switch is OFF. |
| 57 | setHeightLatch = 0; |
57 | setHeightLatch = 0; |
| 58 | } |
58 | } |
| 59 | } else { |
59 | } else { |
| 60 | // Switch is not activated; take the "max-height" as the target height. |
60 | // Switch is not activated; take the "max-height" as the target height. |
| 61 | targetHeight = (uint16_t) dynamicParams.heightSetting * 5L - 500L; // should be: 100 (or make a param out of it) |
61 | targetHeight = (uint16_t) dynamicParams.heightSetting * 50L - 3000L; // should be: 100 (or make a param out of it) |
| 62 | } |
62 | } |
| Line 63... | Line 63... | ||
| 63 | 63 | ||
| 64 | if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
64 | if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
| 65 | heightRampingTimer = 0; |
65 | heightRampingTimer = 0; |
| Line 95... | Line 95... | ||
| 95 | 95 | ||
| Line 96... | Line 96... | ||
| 96 | static int32_t lastHeight; |
96 | static int32_t lastHeight; |
| 97 | 97 | ||
| 98 | int16_t dHeight = height - lastHeight; |
98 | int16_t dHeight = height - lastHeight; |
| 99 | lastHeight = height; |
- | |
| 100 | - | ||
| 101 | // DebugOut.Analog[20] = dHeight; |
- | |
| 102 | // DebugOut.Analog[21] = dynamicParams.MaxHeight; |
99 | lastHeight = height; |
| 103 | 100 | ||
| Line 104... | Line 101... | ||
| 104 | // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
101 | // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
| 105 | // iHeight += heightError; |
102 | iHeight += heightError; |
| 106 | 103 | ||
| 107 | if (heightError > 0) { |
104 | if (heightError > 0) { |
| 108 | debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
105 | debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
| 109 | debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
106 | debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
| 110 | } else if (heightError < 0) { |
107 | } else if (heightError < 0) { |
| Line -... | Line 108... | ||
| - | 108 | debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF; |
|
| 111 | debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF; |
109 | debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
| 112 | debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
110 | } |
| 113 | } |
111 | |
| 114 | 112 | int16_t dThrottle = (iHeight * staticParams.heightI) / 10000L; |
|
| 115 | /* |
113 | |
| 116 | if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}} |
- | |
| 117 | else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }} |
114 | if (dThrottle > staticParams.heightControlMaxIntegralThrottleChange) |
| 118 | else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;} |
- | |
| 119 | else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;} |
115 | dThrottle = staticParams.heightControlMaxIntegralThrottleChange; |
| 120 | */ |
116 | else if (dThrottle < -staticParams.heightControlMaxIntegralThrottleChange) |
| 121 | 117 | dThrottle = -staticParams.heightControlMaxIntegralThrottleChange; |
|
| 122 | int16_t dThrottle = ((heightError * staticParams.heightP) >> 10) |
118 | |
| 123 | /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */-((dHeight * staticParams.heightD) >> 7); |
119 | dThrottle += ((heightError * staticParams.heightP) >> 10) + ((dHeight * staticParams.heightD) >> 7); |
| 124 | 120 | ||
| Line 125... | Line 121... | ||
| 125 | if (dThrottle > staticParams.heightControlMaxThrottleChange) |
121 | if (dThrottle > staticParams.heightControlMaxThrottleChange) |
| 126 | dThrottle = staticParams.heightControlMaxThrottleChange; |
122 | dThrottle = staticParams.heightControlMaxThrottleChange; |
| 127 | else if (dThrottle < -staticParams.heightControlMaxThrottleChange) |
123 | else if (dThrottle < -staticParams.heightControlMaxThrottleChange) |
| - | 124 | dThrottle = -staticParams.heightControlMaxThrottleChange; |
|
| Line 128... | Line 125... | ||
| 128 | dThrottle = -staticParams.heightControlMaxThrottleChange; |
125 | |
| 129 | 126 | debugOut.analog[19] = rampedTargetHeight; |
|
| 130 | debugOut.analog[19] = rampedTargetHeight; |
127 | debugOut.analog[21] = dThrottle; |
| 131 | debugOut.analog[21] = dThrottle; |
128 | debugOut.analog[26] = height; |