Rev 2095 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2095 | Rev 2160 | ||
---|---|---|---|
1 | #include <inttypes.h> |
1 | #include <inttypes.h> |
2 | #include "analog.h" |
2 | #include "analog.h" |
3 | #include "attitude.h" |
3 | #include "attitude.h" |
4 | #include "configuration.h" |
4 | #include "configuration.h" |
5 | #include "controlMixer.h" |
5 | #include "controlMixer.h" |
6 | // for digital / LED debug. |
6 | // for digital / LED debug. |
7 | #include "output.h" |
7 | #include "output.h" |
8 | 8 | ||
9 | // For scope debugging only! |
9 | // For scope debugging only! |
10 | #include "rc.h" |
10 | #include "rc.h" |
11 | 11 | ||
12 | #define INTEGRAL_LIMIT 100000 |
12 | #define INTEGRAL_LIMIT 100000 |
13 | 13 | ||
14 | #define LATCH_TIME 3 |
14 | #define LATCH_TIME 3 |
15 | 15 | ||
16 | // This will translate a height value to a target height in centimeters. |
16 | // This will translate a height value to a target height in centimeters. |
17 | // Currently: One step is 50 cm and zero is 5 meters below the takeoff altitude. With max. 255 steps the max. |
17 | // Currently: One step is 50 cm and zero is 5 meters below the takeoff altitude. With max. 255 steps the max. |
18 | // target height is then about 120m. |
18 | // target height is then about 120m. |
19 | #define HEIGHT_GAIN 50L |
19 | #define HEIGHT_GAIN 50L |
20 | #define HEIGHT_FORMULA(x) ((x) * HEIGHT_GAIN - 500L) |
20 | #define HEIGHT_FORMULA(x) ((x) * HEIGHT_GAIN - 500L) |
21 | 21 | ||
22 | int32_t setHeight; |
22 | int32_t setHeight; |
23 | int32_t targetHeight; |
23 | int32_t targetHeight; |
24 | 24 | ||
25 | uint16_t hc_testOscPrescaler = 0; |
25 | uint16_t hc_testOscPrescaler = 0; |
26 | uint8_t hc_testOscTimer = 0; |
26 | uint8_t hc_testOscTimer = 0; |
27 | 27 | ||
28 | int32_t maxHeightThisFlight; |
28 | int32_t maxHeightThisFlight; |
29 | int32_t iHeight; |
29 | int32_t iHeight; |
30 | 30 | ||
31 | void HC_setGround(void) { |
31 | void HC_setGround(void) { |
32 | analog_setGround(); |
32 | analog_setGround(); |
33 | // This should also happen when height control is enabled in-flight. |
33 | // This should also happen when height control is enabled in-flight. |
34 | setHeight = targetHeight = analog_getHeight(); |
34 | setHeight = targetHeight = analog_getHeight(); |
35 | hc_testOscTimer = 0; |
35 | hc_testOscTimer = 0; |
36 | maxHeightThisFlight = 0; |
36 | maxHeightThisFlight = 0; |
37 | iHeight = 0; |
37 | iHeight = 0; |
38 | } |
38 | } |
39 | 39 | ||
40 | uint8_t HC_isSwitchOn(void) { |
40 | uint8_t HC_isSwitchOn(void) { |
41 | return (dynamicParams.heightSetting >= 255/3); |
41 | return (dynamicParams.heightSetting >= 255/3); |
42 | } |
42 | } |
43 | 43 | ||
44 | void HC_periodicTask(void) { |
44 | void HC_periodicTask(void) { |
45 | int32_t height = analog_getHeight(); |
45 | int32_t height = analog_getHeight(); |
46 | static uint8_t setHeightLatch = 0; |
46 | static uint8_t setHeightLatch = 0; |
47 | 47 | ||
48 | if (height > maxHeightThisFlight) |
48 | if (height > maxHeightThisFlight) |
49 | maxHeightThisFlight = height; |
49 | maxHeightThisFlight = height; |
50 | 50 | ||
51 | // debugOut.analog[25] = dynamicParams.heightSetting; |
51 | // debugOut.analog[25] = dynamicParams.heightSetting; |
52 | 52 | ||
53 | if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) { |
53 | if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) { |
54 | if (HC_isSwitchOn()) { |
54 | if (HC_isSwitchOn()) { |
55 | // Switch is ON |
55 | // Switch is ON |
56 | if (setHeightLatch <= LATCH_TIME) { |
56 | if (setHeightLatch <= LATCH_TIME) { |
57 | if (setHeightLatch == LATCH_TIME) { |
57 | if (setHeightLatch == LATCH_TIME) { |
58 | // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
58 | // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
59 | setHeight = height; |
59 | setHeight = height; |
60 | hc_testOscTimer = 0; |
60 | hc_testOscTimer = 0; |
61 | iHeight = 0; |
61 | iHeight = 0; |
62 | } |
62 | } |
63 | // Time not yet reached. |
63 | // Time not yet reached. |
64 | setHeightLatch++; |
64 | setHeightLatch++; |
65 | } |
65 | } |
66 | } else { |
66 | } else { |
67 | // Switch is OFF. |
67 | // Switch is OFF. |
68 | setHeightLatch = 0; |
68 | setHeightLatch = 0; |
69 | } |
69 | } |
70 | } else { |
70 | } else { |
71 | // Switch is not activated; take the "max-height" as the target height. |
71 | // Switch is not activated; take the "max-height" as the target height. |
72 | setHeight = (uint16_t) (HEIGHT_FORMULA(dynamicParams.heightSetting)); // should be: 100 (or make a param out of it) |
72 | setHeight = (uint16_t) (HEIGHT_FORMULA(dynamicParams.heightSetting)); // should be: 100 (or make a param out of it) |
73 | } |
73 | } |
74 | 74 | ||
75 | /* |
75 | /* |
76 | if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
76 | if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
77 | heightRampingTimer = 0; |
77 | heightRampingTimer = 0; |
78 | if (rampedTargetHeight < targetHeight) { |
78 | if (rampedTargetHeight < targetHeight) { |
79 | // climbing |
79 | // climbing |
80 | if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) { |
80 | if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) { |
81 | rampedTargetHeight += staticParams.heightSlewRate; |
81 | rampedTargetHeight += staticParams.heightSlewRate; |
82 | } else { |
82 | } else { |
83 | rampedTargetHeight = targetHeight; |
83 | rampedTargetHeight = targetHeight; |
84 | } |
84 | } |
85 | } else { |
85 | } else { |
86 | // descending |
86 | // descending |
87 | if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) { |
87 | if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) { |
88 | rampedTargetHeight -= staticParams.heightSlewRate; |
88 | rampedTargetHeight -= staticParams.heightSlewRate; |
89 | } else { |
89 | } else { |
90 | rampedTargetHeight = targetHeight; |
90 | rampedTargetHeight = targetHeight; |
91 | } |
91 | } |
92 | } |
92 | } |
93 | } |
93 | } |
94 | */ |
94 | */ |
95 | // uint8_t heightControlTestOscPeriod; |
95 | // uint8_t heightControlTestOscPeriod; |
96 | // uint8_t heightControlTestOscAmplitude; |
96 | // uint8_t heightControlTestOscAmplitude; |
97 | 97 | ||
98 | hc_testOscPrescaler++; |
98 | hc_testOscPrescaler++; |
99 | if (hc_testOscPrescaler == 488) { |
99 | if (hc_testOscPrescaler == 488) { |
100 | hc_testOscPrescaler = 0; |
100 | hc_testOscPrescaler = 0; |
101 | hc_testOscTimer++; |
101 | hc_testOscTimer++; |
102 | if (hc_testOscTimer == staticParams.heightControlTestOscPeriod) { |
102 | if (hc_testOscTimer == staticParams.heightControlTestOscPeriod) { |
103 | hc_testOscTimer = 0; |
103 | hc_testOscTimer = 0; |
104 | if (staticParams.heightControlTestOscAmplitude) |
104 | if (staticParams.heightControlTestOscAmplitude) |
105 | iHeight = 0; |
105 | iHeight = 0; |
106 | } else if (hc_testOscTimer == staticParams.heightControlTestOscPeriod/2) { |
106 | } else if (hc_testOscTimer == staticParams.heightControlTestOscPeriod/2) { |
107 | if (staticParams.heightControlTestOscAmplitude) |
107 | if (staticParams.heightControlTestOscAmplitude) |
108 | iHeight = 0; |
108 | iHeight = 0; |
109 | } |
109 | } |
110 | } |
110 | } |
111 | 111 | ||
112 | if (hc_testOscTimer < staticParams.heightControlTestOscPeriod/2) |
112 | if (hc_testOscTimer < staticParams.heightControlTestOscPeriod/2) |
113 | targetHeight = setHeight; |
113 | targetHeight = setHeight; |
114 | else |
114 | else |
115 | targetHeight = setHeight + (uint16_t)staticParams.heightControlTestOscAmplitude * HEIGHT_GAIN; |
115 | targetHeight = setHeight + (uint16_t)staticParams.heightControlTestOscAmplitude * HEIGHT_GAIN; |
116 | 116 | ||
117 | //if (staticParams.) |
117 | //if (staticParams.) |
118 | // height, in meters (so the division factor is: 100) |
118 | // height, in meters (so the division factor is: 100) |
119 | // debugOut.analog[24] = (117100 - filteredAirPressure) / 100; |
119 | // debugOut.analog[24] = (117100 - filteredAirPressure) / 100; |
120 | // Calculated 0 alt number: 108205 |
120 | // Calculated 0 alt number: 108205 |
121 | // Experimental 0 alt number: 117100 |
121 | // Experimental 0 alt number: 117100 |
122 | } |
122 | } |
123 | 123 | ||
124 | #define LOG_PHEIGHT_SCALE 10 |
124 | #define LOG_PHEIGHT_SCALE 10 |
125 | #define LOG_IHEIGHT_SCALE 24 |
125 | #define LOG_IHEIGHT_SCALE 24 |
126 | #define LOG_DHEIGHT_SCALE 6 |
126 | #define LOG_DHEIGHT_SCALE 6 |
127 | 127 | ||
128 | // takes 180-200 usec (with integral term). That is too heavy!!! |
128 | // takes 180-200 usec (with integral term). That is too heavy!!! |
129 | // takes 100 usec without integral term. |
129 | // takes 100 usec without integral term. |
130 | void HC_periodicTaskAndPRTY(int16_t* PRTY) { |
130 | void HC_periodicTaskAndPRTY(int16_t* PRTY) { |
131 | HC_periodicTask(); |
131 | HC_periodicTask(); |
132 | int16_t throttle = PRTY[CONTROL_THROTTLE]; |
132 | int16_t throttle = PRTY[CONTROL_THROTTLE]; |
133 | int32_t height = analog_getHeight(); |
133 | int32_t height = analog_getHeight(); |
134 | int32_t heightError = targetHeight - height; |
134 | int32_t heightError = targetHeight - height; |
135 | int16_t dHeight = analog_getDHeight(); |
135 | int16_t dHeight = analog_getDHeight(); |
136 | 136 | ||
137 | debugOut.analog[22] = height/10L; |
137 | debugOut.analog[22] = height/10L; |
138 | debugOut.analog[23] = dHeight; |
138 | debugOut.analog[23] = dHeight; |
139 | 139 | ||
140 | if (heightError > 0) { |
140 | if (heightError > 0) { |
141 | debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
141 | debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
142 | } else { |
142 | } else { |
143 | debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF; |
143 | debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF; |
144 | } |
144 | } |
145 | 145 | ||
146 | if (dHeight > 0) { |
146 | if (dHeight > 0) { |
147 | debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
147 | debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
148 | } else { |
148 | } else { |
149 | debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
149 | debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
150 | } |
150 | } |
151 | 151 | ||
152 | int32_t heightErrorForIntegral = heightError; |
152 | int32_t heightErrorForIntegral = heightError; |
153 | int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << LOG_PHEIGHT_SCALE; |
153 | int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << LOG_PHEIGHT_SCALE; |
154 | 154 | ||
155 | if (heightErrorForIntegral > heightErrorForIntegralLimit) { |
155 | if (heightErrorForIntegral > heightErrorForIntegralLimit) { |
156 | heightErrorForIntegral = heightErrorForIntegralLimit; |
156 | heightErrorForIntegral = heightErrorForIntegralLimit; |
157 | } else if (heightErrorForIntegral < -heightErrorForIntegralLimit) { |
157 | } else if (heightErrorForIntegral < -heightErrorForIntegralLimit) { |
158 | heightErrorForIntegral =- heightErrorForIntegralLimit; |
158 | heightErrorForIntegral =- heightErrorForIntegralLimit; |
159 | } |
159 | } |
160 | 160 | ||
161 | // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
161 | // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
162 | iHeight += heightErrorForIntegral; |
162 | iHeight += heightErrorForIntegral; |
163 | 163 | ||
164 | #define IHEIGHT_SCALE 24 |
164 | #define IHEIGHT_SCALE 24 |
165 | // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256 |
165 | // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256 |
166 | int16_t dThrottleI = (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE); |
166 | int16_t dThrottleI = (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE); |
167 | 167 | ||
168 | if (dThrottleI > staticParams.heightControlMaxIntegralOut) { |
168 | if (dThrottleI > staticParams.heightControlMaxIntegralOut) { |
169 | dThrottleI = staticParams.heightControlMaxIntegralOut; |
169 | dThrottleI = staticParams.heightControlMaxIntegralOut; |
170 | iHeight = ((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI; |
170 | iHeight = ((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI; |
171 | } else if (dThrottleI < -staticParams.heightControlMaxIntegralOut) { |
171 | } else if (dThrottleI < -staticParams.heightControlMaxIntegralOut) { |
172 | dThrottleI = -staticParams.heightControlMaxIntegralOut; |
172 | dThrottleI = -staticParams.heightControlMaxIntegralOut; |
173 | iHeight = -((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI; |
173 | iHeight = -((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI; |
174 | } |
174 | } |
175 | 175 | ||
176 | int16_t dThrottleP = (heightError * dynamicParams.heightP) >> LOG_PHEIGHT_SCALE; |
176 | int16_t dThrottleP = (heightError * dynamicParams.heightP) >> LOG_PHEIGHT_SCALE; |
177 | int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> LOG_DHEIGHT_SCALE; |
177 | int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> LOG_DHEIGHT_SCALE; |
178 | 178 | ||
179 | debugOut.analog[10] = dThrottleP; |
179 | //debugOut.analog[10] = dThrottleP; |
180 | debugOut.analog[11] = dThrottleI; |
180 | //debugOut.analog[11] = dThrottleI; |
181 | debugOut.analog[12] = dThrottleD; |
181 | //debugOut.analog[12] = dThrottleD; |
182 | //debugOut.analog[13] = heightError/10; |
182 | //debugOut.analog[13] = heightError/10; |
183 | 183 | ||
184 | int16_t dThrottle = dThrottleI + dThrottleP - dThrottleD; |
184 | int16_t dThrottle = dThrottleI + dThrottleP - dThrottleD; |
185 | 185 | ||
186 | if (dThrottle > staticParams.heightControlMaxThrottleChange) |
186 | if (dThrottle > staticParams.heightControlMaxThrottleChange) |
187 | dThrottle = staticParams.heightControlMaxThrottleChange; |
187 | dThrottle = staticParams.heightControlMaxThrottleChange; |
188 | else if (dThrottle < -staticParams.heightControlMaxThrottleChange) |
188 | else if (dThrottle < -staticParams.heightControlMaxThrottleChange) |
189 | dThrottle = -staticParams.heightControlMaxThrottleChange; |
189 | dThrottle = -staticParams.heightControlMaxThrottleChange; |
190 | 190 | ||
191 | /* |
191 | /* |
192 | debugOut.analog[19] = throttle; |
192 | debugOut.analog[19] = throttle; |
193 | debugOut.analog[20] = dThrottle; |
193 | debugOut.analog[20] = dThrottle; |
194 | debugOut.analog[21] = height; |
194 | debugOut.analog[21] = height; |
195 | debugOut.analog[22] = rampedTargetHeight; |
195 | debugOut.analog[22] = rampedTargetHeight; |
196 | debugOut.analog[23] = heightError; |
196 | debugOut.analog[23] = heightError; |
197 | */ |
197 | */ |
198 | 198 | ||
199 | if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) { |
199 | if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) { |
200 | if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) || HC_isSwitchOn()) { |
200 | if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) || HC_isSwitchOn()) { |
201 | // If switch is not in use --> Just apply height control. |
201 | // If switch is not in use --> Just apply height control. |
202 | // If switch is in use --> only apply height control when switch is also ON. |
202 | // If switch is in use --> only apply height control when switch is also ON. |
203 | throttle += dThrottle; |
203 | throttle += dThrottle; |
204 | } |
204 | } |
205 | } |
205 | } |
206 | 206 | ||
207 | /* Experiment: Find hover-throttle */ |
207 | /* Experiment: Find hover-throttle */ |
208 | 208 | ||
209 | #define DEFAULT_HOVERTHROTTLE 50 |
209 | #define DEFAULT_HOVERTHROTTLE 50 |
210 | int32_t stronglyFilteredHeightDiff = 0; |
210 | int32_t stronglyFilteredHeightDiff = 0; |
211 | // uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE; |
211 | // uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE; |
212 | uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE; |
212 | uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE; |
213 | #define HOVERTHROTTLEFILTER 25 |
213 | #define HOVERTHROTTLEFILTER 25 |
214 | 214 | ||
215 | stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff |
215 | stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff |
216 | * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER; |
216 | * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER; |
217 | stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER |
217 | stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER |
218 | - 1) + throttle) / HOVERTHROTTLEFILTER; |
218 | - 1) + throttle) / HOVERTHROTTLEFILTER; |
219 | 219 | ||
220 | /* |
220 | /* |
221 | if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 |
221 | if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 |
222 | && stronglyFilteredHeightDiff > -3) { |
222 | && stronglyFilteredHeightDiff > -3) { |
223 | hoverThrottle = stronglyFilteredThrottle; |
223 | hoverThrottle = stronglyFilteredThrottle; |
224 | debugOut.digital[0] |= DEBUG_HOVERTHROTTLE; |
224 | debugOut.digital[0] |= DEBUG_HOVERTHROTTLE; |
225 | } else |
225 | } else |
226 | debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE; |
226 | debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE; |
227 | 227 | ||
228 | */ |
228 | */ |
229 | 229 | ||
230 | PRTY[CONTROL_THROTTLE] = throttle; |
230 | PRTY[CONTROL_THROTTLE] = throttle; |
231 | } |
231 | } |
232 | 232 | ||
233 | /* |
233 | /* |
234 | For a variometer thingy: |
234 | For a variometer thingy: |
235 | When switch is thrown on, freeze throttle (capture it into variable) |
235 | When switch is thrown on, freeze throttle (capture it into variable) |
236 | For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping. |
236 | For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping. |
237 | Output = frozen throttle + whatever is computed +/-. Integral? |
237 | Output = frozen throttle + whatever is computed +/-. Integral? |
238 | */ |
238 | */ |
239 | 239 |