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