Rev 2052 | Rev 2058 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1775 | - | 1 | #include <inttypes.h> |
2033 | - | 2 | #include "analog.h" |
1775 | - | 3 | #include "attitude.h" |
4 | #include "configuration.h" |
||
2048 | - | 5 | #include "controlMixer.h" |
1775 | - | 6 | |
7 | // for digital / LED debug. |
||
8 | #include "output.h" |
||
9 | |||
10 | // For scope debugging only! |
||
11 | #include "rc.h" |
||
12 | |||
13 | #define INTEGRAL_LIMIT 100000 |
||
14 | |||
15 | #define LATCH_TIME 40 |
||
16 | |||
17 | int32_t targetHeight; |
||
18 | int32_t rampedTargetHeight; |
||
19 | |||
20 | uint8_t heightRampingTimer = 0; |
||
1961 | - | 21 | int32_t maxHeightThisFlight; |
1775 | - | 22 | int32_t iHeight; |
23 | |||
24 | void HC_setGround(void) { |
||
2033 | - | 25 | analog_setGround(); |
1960 | - | 26 | // This should also happen when height control is enabled in-flight. |
2033 | - | 27 | rampedTargetHeight = targetHeight = analog_getHeight(); |
1961 | - | 28 | maxHeightThisFlight = 0; |
1960 | - | 29 | iHeight = 0; |
1775 | - | 30 | } |
31 | |||
2039 | - | 32 | void HC_periodicTask(void) { |
2033 | - | 33 | int32_t height = analog_getHeight(); |
1960 | - | 34 | static uint8_t setHeightLatch = 0; |
35 | |||
1961 | - | 36 | if (height > maxHeightThisFlight) |
37 | maxHeightThisFlight = height; |
||
1960 | - | 38 | |
1961 | - | 39 | if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) { |
1960 | - | 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. |
41 | if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) { |
||
42 | // Switch is ON |
||
43 | if (setHeightLatch <= LATCH_TIME) { |
||
44 | if (setHeightLatch == LATCH_TIME) { |
||
45 | // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
||
2033 | - | 46 | rampedTargetHeight = targetHeight = height; |
1775 | - | 47 | } |
1960 | - | 48 | // Time not yet reached. |
49 | setHeightLatch++; |
||
50 | } |
||
51 | } else { |
||
52 | // Switch is OFF. |
||
53 | setHeightLatch = 0; |
||
54 | } |
||
55 | } else { |
||
56 | // Switch is not activated; take the "max-height" as the target height. |
||
2033 | - | 57 | targetHeight = (uint16_t) dynamicParams.heightSetting * 25L - 500L; // should be: 100 (or make a param out of it) |
1960 | - | 58 | } |
59 | |||
60 | if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
||
61 | heightRampingTimer = 0; |
||
1961 | - | 62 | if (rampedTargetHeight < targetHeight) { |
63 | // climbing |
||
64 | if (rampedTargetHeight < targetHeight - staticParams.heightSlewRate) { |
||
65 | rampedTargetHeight += staticParams.heightSlewRate; |
||
66 | } else { |
||
67 | rampedTargetHeight = targetHeight; |
||
68 | } |
||
2026 | - | 69 | } else { |
1961 | - | 70 | // descending |
71 | if (rampedTargetHeight > targetHeight + staticParams.heightSlewRate) { |
||
72 | rampedTargetHeight -= staticParams.heightSlewRate; |
||
73 | } else { |
||
74 | rampedTargetHeight = targetHeight; |
||
75 | } |
||
1960 | - | 76 | } |
77 | } |
||
1970 | - | 78 | |
1960 | - | 79 | // height, in meters (so the division factor is: 100) |
2045 | - | 80 | // debugOut.analog[31] = (117100 - filteredAirPressure) / 100; |
1971 | - | 81 | // Calculated 0 alt number: 108205 |
1980 | - | 82 | // Experimental 0 alt number: 117100 |
1775 | - | 83 | } |
84 | |||
85 | // takes 180-200 usec (with integral term). That is too heavy!!! |
||
86 | // takes 100 usec without integral term. |
||
2048 | - | 87 | void HC_periodicTaskAndPRTY(int16_t* PRTY) { |
88 | HC_periodicTask(); |
||
89 | int16_t throttle = PRTY[CONTROL_THROTTLE]; |
||
2033 | - | 90 | int32_t height = analog_getHeight(); |
1960 | - | 91 | int32_t heightError = rampedTargetHeight - height; |
2033 | - | 92 | int16_t dHeight = analog_getDHeight(); |
1960 | - | 93 | |
2055 | - | 94 | debugOut.analog[14] = height/100L; |
95 | debugOut.analog[15] = dHeight; |
||
96 | |||
2026 | - | 97 | if (heightError > 0) { |
1960 | - | 98 | debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
2035 | - | 99 | } else { |
1961 | - | 100 | debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF; |
2035 | - | 101 | } |
102 | |||
103 | if (dHeight > 0) { |
||
1960 | - | 104 | debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
2035 | - | 105 | } else { |
106 | debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
||
1960 | - | 107 | } |
2032 | - | 108 | |
2033 | - | 109 | // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
110 | iHeight += heightError; |
||
2032 | - | 111 | |
2033 | - | 112 | #define IHEIGHT_SCALE 24 |
113 | // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256 |
||
2035 | - | 114 | int16_t dThrottleI = (iHeight * (int32_t)dynamicParams.heightI) >> (IHEIGHT_SCALE); |
2033 | - | 115 | |
2035 | - | 116 | if (dThrottleI > staticParams.heightControlMaxIntegral) { |
117 | dThrottleI = staticParams.heightControlMaxIntegral; |
||
118 | iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI; |
||
119 | } else if (dThrottleI < -staticParams.heightControlMaxIntegral) { |
||
120 | dThrottleI = -staticParams.heightControlMaxIntegral; |
||
121 | iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / dynamicParams.heightI; |
||
2033 | - | 122 | } |
123 | |||
2035 | - | 124 | int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10; |
125 | int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7; |
||
2033 | - | 126 | |
2055 | - | 127 | //debugOut.analog[24] = dThrottleP; |
128 | //debugOut.analog[25] = dThrottleI; |
||
129 | //debugOut.analog[26] = dThrottleD; |
||
2032 | - | 130 | |
2045 | - | 131 | //debugOut.analog[27] = dynamicParams.heightP; |
132 | //debugOut.analog[28] = dynamicParams.heightI; |
||
133 | //debugOut.analog[29] = dynamicParams.heightD; |
||
2035 | - | 134 | |
135 | int16_t dThrottle = dThrottleI + dThrottleP + dThrottleD; |
||
136 | |||
1961 | - | 137 | if (dThrottle > staticParams.heightControlMaxThrottleChange) |
138 | dThrottle = staticParams.heightControlMaxThrottleChange; |
||
139 | else if (dThrottle < -staticParams.heightControlMaxThrottleChange) |
||
140 | dThrottle = -staticParams.heightControlMaxThrottleChange; |
||
2026 | - | 141 | |
2055 | - | 142 | /* |
2035 | - | 143 | debugOut.analog[19] = throttle; |
144 | debugOut.analog[20] = dThrottle; |
||
145 | debugOut.analog[21] = height; |
||
146 | debugOut.analog[22] = rampedTargetHeight; |
||
147 | debugOut.analog[23] = heightError; |
||
2055 | - | 148 | */ |
2026 | - | 149 | |
1961 | - | 150 | if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) { |
151 | if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) |
||
1960 | - | 152 | || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) { |
153 | // 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. |
||
155 | throttle += dThrottle; |
||
156 | } |
||
157 | } |
||
1961 | - | 158 | |
159 | /* Experiment: Find hover-throttle */ |
||
160 | |||
161 | #define DEFAULT_HOVERTHROTTLE 50 |
||
162 | int32_t stronglyFilteredHeightDiff = 0; |
||
2036 | - | 163 | // uint16_t hoverThrottle = 0; // DEFAULT_HOVERTHROTTLE; |
1961 | - | 164 | uint16_t stronglyFilteredThrottle = DEFAULT_HOVERTHROTTLE; |
165 | #define HOVERTHROTTLEFILTER 25 |
||
1960 | - | 166 | |
167 | stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff |
||
168 | * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER; |
||
169 | stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER |
||
170 | - 1) + throttle) / HOVERTHROTTLEFILTER; |
||
171 | |||
2035 | - | 172 | /* |
1960 | - | 173 | if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 |
174 | && stronglyFilteredHeightDiff > -3) { |
||
175 | hoverThrottle = stronglyFilteredThrottle; |
||
176 | debugOut.digital[0] |= DEBUG_HOVERTHROTTLE; |
||
177 | } else |
||
178 | debugOut.digital[0] &= ~DEBUG_HOVERTHROTTLE; |
||
2035 | - | 179 | |
180 | */ |
||
1960 | - | 181 | |
2048 | - | 182 | PRTY[CONTROL_THROTTLE] = throttle; |
1775 | - | 183 | } |
184 | |||
185 | /* |
||
1821 | - | 186 | For a variometer thingy: |
187 | 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. |
||
189 | Output = frozen throttle + whatever is computed +/-. Integral? |
||
190 | */ |