Rev 1955 | Rev 1961 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1955 | Rev 1960 | ||
---|---|---|---|
Line 44... | Line 44... | ||
44 | uint8_t Height_Gain; // Value : 0-50 |
44 | uint8_t Height_Gain; // Value : 0-50 |
45 | uint8_t Height_ACC_Effect; // Value : 0-250 |
45 | uint8_t Height_ACC_Effect; // Value : 0-250 |
46 | */ |
46 | */ |
Line 47... | Line 47... | ||
47 | 47 | ||
48 | int32_t getHeight(void) { |
48 | int32_t getHeight(void) { |
49 | return groundPressure - filteredAirPressure; |
49 | return groundPressure - filteredAirPressure; |
Line 50... | Line 50... | ||
50 | } |
50 | } |
51 | 51 | ||
52 | void HC_setGround(void) { |
52 | void HC_setGround(void) { |
53 | groundPressure = filteredAirPressure; |
53 | groundPressure = filteredAirPressure; |
54 | // This should also happen when height control is enabled in-flight. |
54 | // This should also happen when height control is enabled in-flight. |
55 | rampedTargetHeight = getHeight(); |
55 | rampedTargetHeight = getHeight(); |
56 | maxHeight = 0; |
56 | maxHeight = 0; |
Line 57... | Line 57... | ||
57 | iHeight = 0; |
57 | iHeight = 0; |
58 | } |
58 | } |
59 | 59 | ||
60 | void HC_update(void) { |
60 | void HC_update(void) { |
61 | int32_t height = getHeight(); |
61 | int32_t height = getHeight(); |
62 | static uint8_t setHeightLatch = 0; |
62 | static uint8_t setHeightLatch = 0; |
63 | 63 | ||
64 | if (height > maxHeight) |
64 | if (height > maxHeight) |
65 | maxHeight = height; |
65 | maxHeight = height; |
66 | 66 | ||
67 | if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) { |
67 | if (staticParams.bitConfig & 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.heightSetting < 40 || dynamicParams.heightSetting > 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. |
- | |
75 | targetHeight = height; |
- | |
76 | // DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH; |
- | |
77 | } |
- | |
78 | // Time not yet reached. |
- | |
79 | setHeightLatch++; |
- | |
80 | } |
- | |
81 | } else { |
- | |
82 | // Switch is OFF. |
- | |
83 | setHeightLatch = 0; |
- | |
84 | // DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH; |
- | |
85 | } |
- | |
86 | } else { |
- | |
87 | // Switch is not activated; take the "max-height" as the target height. |
- | |
88 | // DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH; |
- | |
89 | targetHeight = (uint16_t) dynamicParams.MaxHeight * 100; //getHeight() + 10 * 100; |
- | |
90 | } |
- | |
91 | - | ||
92 | if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
- | |
93 | heightRampingTimer = 0; |
- | |
94 | if (rampedTargetHeight + staticParams.Height_Gain <= targetHeight) { |
- | |
95 | rampedTargetHeight += staticParams.Height_Gain; |
- | |
96 | } else if (rampedTargetHeight - staticParams.Height_Gain >= targetHeight) { |
74 | // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
- | 75 | targetHeight = height; |
|
- | 76 | // DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH; |
|
- | 77 | } |
|
- | 78 | // Time not yet reached. |
|
- | 79 | setHeightLatch++; |
|
- | 80 | } |
|
- | 81 | } else { |
|
- | 82 | // Switch is OFF. |
|
- | 83 | setHeightLatch = 0; |
|
- | 84 | // DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH; |
|
- | 85 | } |
|
- | 86 | } else { |
|
- | 87 | // Switch is not activated; take the "max-height" as the target height. |
|
97 | rampedTargetHeight -= staticParams.Height_Gain; |
88 | // DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH; |
- | 89 | targetHeight = (uint16_t) dynamicParams.heightSetting * 100; //getHeight() + 10 * 100; |
|
- | 90 | } |
|
- | 91 | ||
- | 92 | if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
|
- | 93 | heightRampingTimer = 0; |
|
- | 94 | if (rampedTargetHeight + staticParams.heightSlewRate <= targetHeight) { |
|
- | 95 | rampedTargetHeight += staticParams.heightSlewRate; |
|
- | 96 | } else if (rampedTargetHeight - staticParams.heightSlewRate >= targetHeight) { |
|
- | 97 | rampedTargetHeight -= staticParams.heightSlewRate; |
|
98 | } |
98 | } |
99 | } |
99 | } |
100 | 100 | ||
Line 101... | Line 101... | ||
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 / 10; |
102 | debugOut.analog[30] = height / 10; |
103 | } |
103 | } |
Line 104... | Line 104... | ||
104 | 104 | ||
105 | // ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
105 | // ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
106 | // ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH |
106 | // ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH |
107 | // ParamSet.BitConfig & CFG_HEIGHT_3SWITCH |
107 | // ParamSet.BitConfig & CFG_HEIGHT_3SWITCH |
108 | 108 | ||
109 | // takes 180-200 usec (with integral term). That is too heavy!!! |
109 | // takes 180-200 usec (with integral term). That is too heavy!!! |
110 | // takes 100 usec without integral term. |
110 | // takes 100 usec without integral term. |
111 | uint16_t HC_getThrottle(uint16_t throttle) { |
111 | uint16_t HC_getThrottle(uint16_t throttle) { |
112 | int32_t height = getHeight(); |
112 | int32_t height = getHeight(); |
113 | int32_t heightError = rampedTargetHeight - height; |
113 | int32_t heightError = rampedTargetHeight - height; |
114 | 114 | ||
115 | static int32_t lastHeight; |
115 | static int32_t lastHeight; |
116 | 116 | ||
117 | int16_t dHeight = height - lastHeight; |
117 | int16_t dHeight = height - lastHeight; |
118 | lastHeight = height; |
118 | lastHeight = height; |
119 | 119 | ||
120 | // DebugOut.Analog[20] = dHeight; |
120 | // DebugOut.Analog[20] = dHeight; |
121 | // DebugOut.Analog[21] = dynamicParams.MaxHeight; |
121 | // DebugOut.Analog[21] = dynamicParams.MaxHeight; |
122 | 122 | ||
123 | // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
123 | // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
124 | // iHeight += heightError; |
124 | // iHeight += heightError; |
125 | 125 | ||
126 | if (dHeight > 0) { |
126 | if (dHeight > 0) { |
127 | debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
127 | debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
128 | debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
128 | debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
129 | } else if (dHeight < 0) { |
129 | } else if (dHeight < 0) { |
130 | debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
130 | debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
131 | debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF; |
131 | debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF; |
132 | } |
132 | } |
133 | 133 | ||
134 | /* |
134 | /* |
135 | if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}} |
135 | if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}} |
136 | else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }} |
136 | else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }} |
137 | else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;} |
137 | else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;} |
138 | else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;} |
138 | else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;} |
139 | */ |
139 | */ |
140 | 140 | ||
141 | int16_t dThrottle = heightError * staticParams.HeightP / 1000 |
141 | int16_t dThrottle = heightError * staticParams.heightP / 1000 |
142 | /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight |
142 | /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight |
143 | * staticParams.HeightD; |
143 | * staticParams.heightD; |
144 | 144 | ||
145 | // the "minGas" is now a limit for how much up / down the throttle can be varied |
145 | // the "minGas" is now a limit for how much up / down the throttle can be varied |
146 | if (dThrottle > staticParams.HeightMinGas) |
146 | if (dThrottle > staticParams.heightMaxThrottleChange) |
147 | dThrottle = staticParams.HeightMinGas; |
147 | dThrottle = staticParams.heightMaxThrottleChange; |
148 | else if (dThrottle < -staticParams.HeightMinGas) |
148 | else if (dThrottle < -staticParams.heightMaxThrottleChange) |
149 | dThrottle = -staticParams.HeightMinGas; |
149 | dThrottle = -staticParams.heightMaxThrottleChange; |
150 | 150 | ||
151 | // TODO: Eliminate repetition. |
151 | // TODO: Eliminate repetition. |
152 | if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) { |
152 | if (staticParams.bitConfig & CFG_HEIGHT_CONTROL) { |
153 | if (!(staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) |
153 | if (!(staticParams.bitConfig & CFG_HEIGHT_SWITCH) |
154 | || (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40)) { |
154 | || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) { |
155 | // If switch is not in use --> Just apply height control. |
155 | // If switch is not in use --> Just apply height control. |
156 | // If switch is in use --> only apply height control when switch is also ON. |
156 | // If switch is in use --> only apply height control when switch is also ON. |
157 | throttle += dThrottle; |
157 | throttle += dThrottle; |
158 | } |
158 | } |
159 | } |
159 | } |
160 | 160 | ||
161 | /* Experiment: Find hover-throttle */ |
161 | /* Experiment: Find hover-throttle */ |
162 | stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff |
162 | stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff |
163 | * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER; |
163 | * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER; |
164 | stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER |
164 | stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER |
165 | - 1) + throttle) / HOVERTHROTTLEFILTER; |
165 | - 1) + throttle) / HOVERTHROTTLEFILTER; |
166 | 166 | ||
167 | if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 |
167 | if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 |
168 | && stronglyFilteredHeightDiff > -3) { |
168 | && stronglyFilteredHeightDiff > -3) { |
169 | hoverThrottle = stronglyFilteredThrottle; |
169 | hoverThrottle = stronglyFilteredThrottle; |
170 | debugOut.digital[0] |= DEBUG_HOVERTHROTTLE; |
170 | debugOut.digital[0] |= DEBUG_HOVERTHROTTLE; |
171 | // DebugOut.Analog[18] = hoverThrottle; |
171 | // DebugOut.Analog[18] = hoverThrottle; |
172 | } else |
172 | } else |
173 | debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE; |
173 | debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE; |
174 | 174 | ||
Line 175... | Line 175... | ||
175 | debugOut.analog[20] = dThrottle; |
175 | debugOut.analog[20] = dThrottle; |
176 | debugOut.analog[21] = hoverThrottle; |
176 | debugOut.analog[21] = hoverThrottle; |
177 | 177 |