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