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 | /* |