Rev 2071 | Rev 2074 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2071 | Rev 2073 | ||
|---|---|---|---|
| Line 94... | Line 94... | ||
| 94 | // takes 100 usec without integral term. |
94 | // takes 100 usec without integral term. |
| 95 | void HC_periodicTaskAndPRTY(int16_t* PRTY) { |
95 | void HC_periodicTaskAndPRTY(int16_t* PRTY) { |
| 96 | HC_periodicTask(); |
96 | HC_periodicTask(); |
| 97 | int16_t throttle = PRTY[CONTROL_THROTTLE]; |
97 | int16_t throttle = PRTY[CONTROL_THROTTLE]; |
| 98 | int32_t height = analog_getHeight(); |
98 | int32_t height = analog_getHeight(); |
| 99 | int32_t heightError = /*ramped*/ targetHeight - height; |
99 | int32_t heightError = targetHeight - height; |
| 100 | int16_t dHeight = analog_getDHeight(); |
100 | int16_t dHeight = analog_getDHeight(); |
| Line 101... | Line 101... | ||
| 101 | 101 | ||
| 102 | debugOut.analog[22] = height/10L; |
102 | debugOut.analog[22] = height/10L; |
| Line 112... | Line 112... | ||
| 112 | debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
112 | debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
| 113 | } else { |
113 | } else { |
| 114 | debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
114 | debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
| 115 | } |
115 | } |
| Line -... | Line 116... | ||
| - | 116 | ||
| - | 117 | int32_t heightErrorForIntegral = heightError; |
|
| - | 118 | int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << 10; |
|
| - | 119 | ||
| - | 120 | if (heightErrorForIntegral > heightErrorForIntegralLimit) { |
|
| - | 121 | heightErrorForIntegral = heightErrorForIntegralLimit; |
|
| - | 122 | } else if (heightErrorForIntegral < -heightErrorForIntegralLimit) { |
|
| - | 123 | heightErrorForIntegral =- heightErrorForIntegralLimit; |
|
| - | 124 | } |
|
| 116 | 125 | ||
| 117 | // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
126 | // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
| Line 118... | Line 127... | ||
| 118 | iHeight += heightError; |
127 | iHeight += heightErrorForIntegral; |
| 119 | 128 | ||
| 120 | #define IHEIGHT_SCALE 24 |
129 | #define IHEIGHT_SCALE 24 |
| Line 121... | Line 130... | ||
| 121 | // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256 |
130 | // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256 |
| 122 | int16_t dThrottleI = (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE); |
131 | int16_t dThrottleI = (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE); |
| 123 | 132 | ||
| 124 | if (dThrottleI > staticParams.heightControlMaxIntegral) { |
133 | if (dThrottleI > staticParams.heightControlMaxIntegralOut) { |
| 125 | dThrottleI = staticParams.heightControlMaxIntegral; |
134 | dThrottleI = staticParams.heightControlMaxIntegralOut; |
| 126 | iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI; |
135 | iHeight = ((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI; |
| 127 | } else if (dThrottleI < -staticParams.heightControlMaxIntegral) { |
136 | } else if (dThrottleI < -staticParams.heightControlMaxIntegralOut) { |
| Line 128... | Line 137... | ||
| 128 | dThrottleI = -staticParams.heightControlMaxIntegral; |
137 | dThrottleI = -staticParams.heightControlMaxIntegralOut; |
| 129 | iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI; |
138 | iHeight = -((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI; |
| Line 130... | Line 139... | ||
| 130 | } |
139 | } |
| 131 | 140 | ||
| 132 | int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10; |
141 | int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10; |
| - | 142 | int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7; |
|
| Line 133... | Line 143... | ||
| 133 | int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7; |
143 | |
| 134 | 144 | debugOut.analog[10] = dThrottleP; |
|
| 135 | //debugOut.analog[24] = dThrottleP; |
145 | debugOut.analog[11] = dThrottleI; |