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