Rev 1867 | Rev 1955 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1867 | Rev 1869 | ||
|---|---|---|---|
| Line 64... | Line 64... | ||
| 64 | if (height > maxHeight) |
64 | if (height > maxHeight) |
| 65 | maxHeight = height; |
65 | maxHeight = height; |
| Line 66... | Line 66... | ||
| 66 | 66 | ||
| 67 | if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) { |
67 | if (staticParams.GlobalConfig & 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.MaxHeight < 40 || dynamicParams.MaxHeight > 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) { |
| 74 | // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
74 | // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
| 75 | targetHeight = height; |
75 | targetHeight = height; |
| 76 | DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH; |
76 | // DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH; |
| 77 | } |
77 | } |
| 78 | // Time not yet reached. |
78 | // Time not yet reached. |
| 79 | setHeightLatch++; |
79 | setHeightLatch++; |
| 80 | } |
80 | } |
| 81 | } else { |
81 | } else { |
| 82 | // Switch is OFF. |
82 | // Switch is OFF. |
| 83 | setHeightLatch = 0; |
83 | setHeightLatch = 0; |
| 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.MaxHeight * 100; //getHeight() + 10 * 100; |
| Line 90... | Line 90... | ||
| 90 | } |
90 | } |
| 91 | 91 | ||
| Line 97... | Line 97... | ||
| 97 | rampedTargetHeight -= staticParams.Height_Gain; |
97 | rampedTargetHeight -= staticParams.Height_Gain; |
| 98 | } |
98 | } |
| 99 | } |
99 | } |
| Line 100... | Line 100... | ||
| 100 | 100 | ||
| 101 | // height, in meters (so the division factor is: 100) |
101 | // height, in meters (so the division factor is: 100) |
| 102 | DebugOut.Analog[30] = height / 100; |
102 | DebugOut.Analog[30] = height / 10; |
| Line 103... | Line 103... | ||
| 103 | } |
103 | } |
| 104 | 104 | ||
| 105 | // ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
105 | // ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
| Line 146... | Line 146... | ||
| 146 | if (dThrottle > staticParams.HeightMinGas) |
146 | if (dThrottle > staticParams.HeightMinGas) |
| 147 | dThrottle = staticParams.HeightMinGas; |
147 | dThrottle = staticParams.HeightMinGas; |
| 148 | else if (dThrottle < -staticParams.HeightMinGas) |
148 | else if (dThrottle < -staticParams.HeightMinGas) |
| 149 | dThrottle = -staticParams.HeightMinGas; |
149 | dThrottle = -staticParams.HeightMinGas; |
| Line 150... | Line -... | ||
| 150 | - | ||
| 151 | //DebugOut.Analog[18] = dThrottle; |
- | |
| 152 | //DebugOut.Analog[19] = iHeight / 10000L; |
- | |
| 153 | 150 | ||
| 154 | // TODO: Eliminate repitition. |
151 | // TODO: Eliminate repetition. |
| 155 | if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) { |
152 | if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) { |
| 156 | if (!(staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) |
153 | if (!(staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) |
| 157 | || (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40)) { |
154 | || (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40)) { |
| 158 | // If switch is not in use --> Just apply height control. |
155 | // If switch is not in use --> Just apply height control. |
| Line 172... | Line 169... | ||
| 172 | hoverThrottle = stronglyFilteredThrottle; |
169 | hoverThrottle = stronglyFilteredThrottle; |
| 173 | DebugOut.Digital[0] |= DEBUG_HOVERTHROTTLE; |
170 | DebugOut.Digital[0] |= DEBUG_HOVERTHROTTLE; |
| 174 | // DebugOut.Analog[18] = hoverThrottle; |
171 | // DebugOut.Analog[18] = hoverThrottle; |
| 175 | } else |
172 | } else |
| 176 | DebugOut.Digital[0] &= ~DEBUG_HOVERTHROTTLE; |
173 | DebugOut.Digital[0] &= ~DEBUG_HOVERTHROTTLE; |
| - | 174 | ||
| - | 175 | DebugOut.Analog[20] = dThrottle; |
|
| - | 176 | DebugOut.Analog[21] = hoverThrottle; |
|
| - | 177 | ||
| 177 | return throttle; |
178 | return throttle; |
| 178 | } |
179 | } |
| Line 179... | Line 180... | ||
| 179 | 180 | ||
| 180 | /* |
181 | /* |