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