Rev 1980 | Rev 2026 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1980 | Rev 1986 | ||
---|---|---|---|
1 | #include <inttypes.h> |
1 | #include <inttypes.h> |
2 | #include "attitude.h" |
2 | #include "attitude.h" |
3 | #include "uart0.h" |
3 | #include "uart0.h" |
4 | #include "configuration.h" |
4 | #include "configuration.h" |
5 | 5 | ||
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 | /* |
14 | /* |
15 | #define DEBUGINTEGRAL 0 |
15 | #define DEBUGINTEGRAL 0 |
16 | #define DEBUGDIFFERENTIAL 0 |
16 | #define DEBUGDIFFERENTIAL 0 |
17 | #define DEBUGHOVERTHROTTLE 0 |
17 | #define DEBUGHOVERTHROTTLE 0 |
18 | #define DEBUGHEIGHTSWITCH 0 |
18 | #define DEBUGHEIGHTSWITCH 0 |
19 | */ |
19 | */ |
20 | 20 | ||
21 | #define LATCH_TIME 40 |
21 | #define LATCH_TIME 40 |
22 | 22 | ||
23 | int32_t groundPressure; |
23 | int32_t groundPressure; |
24 | int32_t targetHeight; |
24 | int32_t targetHeight; |
25 | int32_t rampedTargetHeight; |
25 | int32_t rampedTargetHeight; |
26 | 26 | ||
27 | uint8_t heightRampingTimer = 0; |
27 | uint8_t heightRampingTimer = 0; |
28 | int32_t maxHeightThisFlight; |
28 | int32_t maxHeightThisFlight; |
29 | int32_t iHeight; |
29 | int32_t iHeight; |
30 | 30 | ||
31 | int32_t getHeight(void) { |
31 | int32_t getHeight(void) { |
32 | return groundPressure - filteredAirPressure; |
32 | return groundPressure - filteredAirPressure; |
33 | } |
33 | } |
34 | 34 | ||
35 | void HC_setGround(void) { |
35 | void HC_setGround(void) { |
36 | groundPressure = filteredAirPressure; |
36 | groundPressure = filteredAirPressure; |
37 | // This should also happen when height control is enabled in-flight. |
37 | // This should also happen when height control is enabled in-flight. |
38 | rampedTargetHeight = getHeight(); |
38 | rampedTargetHeight = getHeight(); |
39 | maxHeightThisFlight = 0; |
39 | maxHeightThisFlight = 0; |
40 | iHeight = 0; |
40 | iHeight = 0; |
41 | } |
41 | } |
42 | 42 | ||
43 | void HC_update(void) { |
43 | void HC_update(void) { |
44 | int32_t height = getHeight(); |
44 | int32_t height = getHeight(); |
45 | static uint8_t setHeightLatch = 0; |
45 | static uint8_t setHeightLatch = 0; |
46 | 46 | ||
47 | if (height > maxHeightThisFlight) |
47 | if (height > maxHeightThisFlight) |
48 | maxHeightThisFlight = height; |
48 | maxHeightThisFlight = height; |
49 | 49 | ||
50 | if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) { |
50 | if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) { |
51 | // If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle. |
51 | // If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle. |
52 | if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) { |
52 | if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) { |
53 | // Switch is ON |
53 | // Switch is ON |
54 | if (setHeightLatch <= LATCH_TIME) { |
54 | if (setHeightLatch <= LATCH_TIME) { |
55 | if (setHeightLatch == LATCH_TIME) { |
55 | if (setHeightLatch == LATCH_TIME) { |
56 | // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
56 | // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
57 | targetHeight = height; |
57 | targetHeight = height; |
58 | } |
58 | } |
59 | // Time not yet reached. |
59 | // Time not yet reached. |
60 | setHeightLatch++; |
60 | setHeightLatch++; |
61 | } |
61 | } |
62 | } else { |
62 | } else { |
63 | // Switch is OFF. |
63 | // Switch is OFF. |
64 | setHeightLatch = 0; |
64 | setHeightLatch = 0; |
65 | } |
65 | } |
66 | } else { |
66 | } else { |
67 | // Switch is not activated; take the "max-height" as the target height. |
67 | // Switch is not activated; take the "max-height" as the target height. |
68 | targetHeight = (uint16_t) dynamicParams.heightSetting * 100; |
68 | targetHeight = (uint16_t) dynamicParams.heightSetting * 100; |
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 if (rampedTargetHeight != targetHeight) { |
80 | } else if (rampedTargetHeight != targetHeight) { |
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 | // height, in meters (so the division factor is: 100) |
90 | // height, in meters (so the division factor is: 100) |
91 | // debugOut.analog[30] = filteredAirPressure / 10; |
91 | // debugOut.analog[30] = filteredAirPressure / 10; |
92 | debugOut.analog[30] = (117100 - filteredAirPressure) / 100; |
92 | debugOut.analog[30] = (117100 - filteredAirPressure) / 100; |
93 | // Calculated 0 alt number: 108205 |
93 | // Calculated 0 alt number: 108205 |
94 | // Experimental 0 alt number: 117100 |
94 | // Experimental 0 alt number: 117100 |
95 | } |
95 | } |
96 | 96 | ||
97 | // takes 180-200 usec (with integral term). That is too heavy!!! |
97 | // takes 180-200 usec (with integral term). That is too heavy!!! |
98 | // takes 100 usec without integral term. |
98 | // takes 100 usec without integral term. |
99 | uint16_t HC_getThrottle(uint16_t throttle) { |
99 | uint16_t HC_getThrottle(uint16_t throttle) { |
100 | int32_t height = getHeight(); |
100 | int32_t height = getHeight(); |
101 | int32_t heightError = rampedTargetHeight - height; |
101 | int32_t heightError = rampedTargetHeight - height; |
102 | 102 | ||
103 | static int32_t lastHeight; |
103 | static int32_t lastHeight; |
104 | 104 | ||
105 | int16_t dHeight = height - lastHeight; |
105 | int16_t dHeight = height - lastHeight; |
106 | lastHeight = height; |
106 | lastHeight = height; |
107 | 107 | ||
108 | // DebugOut.Analog[20] = dHeight; |
108 | // DebugOut.Analog[20] = dHeight; |
109 | // DebugOut.Analog[21] = dynamicParams.MaxHeight; |
109 | // DebugOut.Analog[21] = dynamicParams.MaxHeight; |
110 | 110 | ||
111 | // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
111 | // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
112 | // iHeight += heightError; |
112 | // iHeight += heightError; |
113 | 113 | ||
114 | if (dHeight > 0) { |
114 | if (dHeight > 0) { |
115 | debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
115 | debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
116 | debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
116 | debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
117 | } else if (dHeight < 0) { |
117 | } else if (dHeight < 0) { |
118 | debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF; |
118 | debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF; |
119 | debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
119 | debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
120 | } |
120 | } |
121 | 121 | ||
122 | /* |
122 | /* |
123 | if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}} |
123 | if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}} |
124 | else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }} |
124 | else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }} |
125 | else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;} |
125 | else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;} |
126 | else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;} |
126 | else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;} |
127 | */ |
127 | */ |
128 | 128 | ||
129 | int16_t dThrottle = ((heightError * staticParams.heightP) << 10) |
129 | int16_t dThrottle = ((heightError * staticParams.heightP) << 10) |
130 | /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight * staticParams.heightD; |
130 | /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight * staticParams.heightD; |
131 | 131 | ||
132 | // the "minGas" is now a limit for how much up / down the throttle can be varied |
132 | // the "minGas" is now a limit for how much up / down the throttle can be varied |
133 | if (dThrottle > staticParams.heightControlMaxThrottleChange) |
133 | if (dThrottle > staticParams.heightControlMaxThrottleChange) |
134 | dThrottle = staticParams.heightControlMaxThrottleChange; |
134 | dThrottle = staticParams.heightControlMaxThrottleChange; |
135 | else if (dThrottle < -staticParams.heightControlMaxThrottleChange) |
135 | else if (dThrottle < -staticParams.heightControlMaxThrottleChange) |
136 | dThrottle = -staticParams.heightControlMaxThrottleChange; |
136 | dThrottle = -staticParams.heightControlMaxThrottleChange; |
137 | 137 | ||
138 | if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) { |
138 | if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) { |
139 | if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) |
139 | if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) |
140 | || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) { |
140 | || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) { |
141 | // If switch is not in use --> Just apply height control. |
141 | // If switch is not in use --> Just apply height control. |
142 | // If switch is in use --> only apply height control when switch is also ON. |
142 | // If switch is in use --> only apply height control when switch is also ON. |
143 | throttle += dThrottle; |
143 | throttle += dThrottle; |
144 | } |
144 | } |
145 | } |
145 | } |
146 | 146 | ||
147 | /* Experiment: Find hover-throttle */ |
147 | /* Experiment: Find hover-throttle */ |
148 | 148 | ||
149 | #define DEFAULT_HOVERTHROTTLE 50 |
149 | #define DEFAULT_HOVERTHROTTLE 50 |
150 | int32_t stronglyFilteredHeightDiff = 0; |
150 | int32_t stronglyFilteredHeightDiff = 0; |
151 | uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE; |
151 | uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE; |
152 | uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE; |
152 | uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE; |
153 | #define HOVERTHROTTLEFILTER 25 |
153 | #define HOVERTHROTTLEFILTER 25 |
154 | 154 | ||
155 | stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff |
155 | stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff |
156 | * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER; |
156 | * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER; |
157 | stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER |
157 | stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER |
158 | - 1) + throttle) / HOVERTHROTTLEFILTER; |
158 | - 1) + throttle) / HOVERTHROTTLEFILTER; |
159 | 159 | ||
160 | if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 |
160 | if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 |
161 | && stronglyFilteredHeightDiff > -3) { |
161 | && stronglyFilteredHeightDiff > -3) { |
162 | hoverThrottle = stronglyFilteredThrottle; |
162 | hoverThrottle = stronglyFilteredThrottle; |
163 | debugOut.digital[0] |= DEBUG_HOVERTHROTTLE; |
163 | debugOut.digital[0] |= DEBUG_HOVERTHROTTLE; |
164 | // DebugOut.Analog[18] = hoverThrottle; |
164 | // DebugOut.Analog[18] = hoverThrottle; |
165 | } else |
165 | } else |
166 | debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE; |
166 | debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE; |
167 | 167 | ||
168 | debugOut.analog[20] = dThrottle; |
- | |
169 | debugOut.analog[21] = hoverThrottle; |
168 | //debugOut.analog[20] = dThrottle; |
170 | 169 | ||
171 | return throttle; |
170 | return throttle; |
172 | } |
171 | } |
173 | 172 | ||
174 | /* |
173 | /* |
175 | For a variometer thingy: |
174 | For a variometer thingy: |
176 | When switch is thrown on, freeze throttle (capture it into variable) |
175 | When switch is thrown on, freeze throttle (capture it into variable) |
177 | For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping. |
176 | For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping. |
178 | Output = frozen throttle + whatever is computed +/-. Integral? |
177 | Output = frozen throttle + whatever is computed +/-. Integral? |
179 | */ |
178 | */ |
180 | 179 |