Rev 2032 | Rev 2035 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2032 | Rev 2033 | ||
|---|---|---|---|
| Line 1... | Line 1... | ||
| 1 | #include <inttypes.h> |
1 | #include <inttypes.h> |
| - | 2 | #include "analog.h" |
|
| 2 | #include "attitude.h" |
3 | #include "attitude.h" |
| 3 | #include "uart0.h" |
4 | #include "uart0.h" |
| 4 | #include "configuration.h" |
5 | #include "configuration.h" |
| Line 5... | Line 6... | ||
| 5 | 6 | ||
| Line 11... | Line 12... | ||
| 11 | 12 | ||
| Line 12... | Line 13... | ||
| 12 | #define INTEGRAL_LIMIT 100000 |
13 | #define INTEGRAL_LIMIT 100000 |
| Line 13... | Line -... | ||
| 13 | - | ||
| 14 | #define LATCH_TIME 40 |
14 | |
| 15 | 15 | #define LATCH_TIME 40 |
|
| Line 16... | Line 16... | ||
| 16 | int32_t groundPressure; |
16 | |
| 17 | int32_t targetHeight; |
17 | int32_t targetHeight; |
| 18 | int32_t rampedTargetHeight; |
18 | int32_t rampedTargetHeight; |
| Line 19... | Line -... | ||
| 19 | - | ||
| 20 | uint8_t heightRampingTimer = 0; |
- | |
| 21 | int32_t maxHeightThisFlight; |
- | |
| 22 | int32_t iHeight; |
- | |
| 23 | 19 | ||
| 24 | int32_t getHeight(void) { |
20 | uint8_t heightRampingTimer = 0; |
| 25 | return groundPressure - filteredAirPressure; |
21 | int32_t maxHeightThisFlight; |
| 26 | } |
22 | int32_t iHeight; |
| 27 | 23 | ||
| 28 | void HC_setGround(void) { |
24 | void HC_setGround(void) { |
| 29 | groundPressure = filteredAirPressure; |
25 | analog_setGround(); |
| Line 30... | Line 26... | ||
| 30 | // This should also happen when height control is enabled in-flight. |
26 | // This should also happen when height control is enabled in-flight. |
| 31 | rampedTargetHeight = getHeight(); |
27 | rampedTargetHeight = targetHeight = analog_getHeight(); |
| 32 | maxHeightThisFlight = 0; |
28 | maxHeightThisFlight = 0; |
| Line 33... | Line 29... | ||
| 33 | iHeight = 0; |
29 | iHeight = 0; |
| 34 | } |
30 | } |
| Line 45... | Line 41... | ||
| 45 | if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) { |
41 | if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) { |
| 46 | // Switch is ON |
42 | // Switch is ON |
| 47 | if (setHeightLatch <= LATCH_TIME) { |
43 | if (setHeightLatch <= LATCH_TIME) { |
| 48 | if (setHeightLatch == LATCH_TIME) { |
44 | if (setHeightLatch == LATCH_TIME) { |
| 49 | // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
45 | // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
| 50 | targetHeight = height; |
46 | rampedTargetHeight = targetHeight = height; |
| 51 | } |
47 | } |
| 52 | // Time not yet reached. |
48 | // Time not yet reached. |
| 53 | setHeightLatch++; |
49 | setHeightLatch++; |
| 54 | } |
50 | } |
| 55 | } else { |
51 | } else { |
| 56 | // Switch is OFF. |
52 | // Switch is OFF. |
| 57 | setHeightLatch = 0; |
53 | setHeightLatch = 0; |
| 58 | } |
54 | } |
| 59 | } else { |
55 | } else { |
| 60 | // Switch is not activated; take the "max-height" as the target height. |
56 | // Switch is not activated; take the "max-height" as the target height. |
| 61 | targetHeight = (uint16_t) dynamicParams.heightSetting * 50L - 3000L; // should be: 100 (or make a param out of it) |
57 | targetHeight = (uint16_t) dynamicParams.heightSetting * 25L - 500L; // should be: 100 (or make a param out of it) |
| 62 | } |
58 | } |
| Line 63... | Line 59... | ||
| 63 | 59 | ||
| 64 | if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
60 | if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
| 65 | heightRampingTimer = 0; |
61 | heightRampingTimer = 0; |
| Line 88... | Line 84... | ||
| 88 | } |
84 | } |
| Line 89... | Line 85... | ||
| 89 | 85 | ||
| 90 | // takes 180-200 usec (with integral term). That is too heavy!!! |
86 | // takes 180-200 usec (with integral term). That is too heavy!!! |
| 91 | // takes 100 usec without integral term. |
87 | // takes 100 usec without integral term. |
| 92 | uint16_t HC_getThrottle(uint16_t throttle) { |
88 | uint16_t HC_getThrottle(uint16_t throttle) { |
| 93 | int32_t height = getHeight(); |
89 | int32_t height = analog_getHeight(); |
| Line 94... | Line 90... | ||
| 94 | int32_t heightError = rampedTargetHeight - height; |
90 | int32_t heightError = rampedTargetHeight - height; |
| 95 | - | ||
| 96 | static int32_t lastHeight; |
91 | |
| 97 | 92 | //static int32_t lastHeight; |
|
| 98 | int16_t dHeight = height - lastHeight; |
- | |
| 99 | lastHeight = height; |
- | |
| 100 | - | ||
| Line 101... | Line 93... | ||
| 101 | // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
93 | int16_t dHeight = analog_getDHeight(); |
| 102 | iHeight += heightError; |
94 | //lastHeight = height; |
| 103 | 95 | ||
| 104 | if (heightError > 0) { |
96 | if (heightError > 0) { |
| 105 | debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
97 | debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
| 106 | debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
98 | debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
| 107 | } else if (heightError < 0) { |
99 | } else if (heightError < 0) { |
| 108 | debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF; |
- | |
| 109 | debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
- | |
| Line -... | Line 100... | ||
| - | 100 | debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF; |
|
| - | 101 | debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
|
| - | 102 | } |
|
| - | 103 | ||
| - | 104 | // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
|
| - | 105 | iHeight += heightError; |
|
| - | 106 | ||
| - | 107 | #define IHEIGHT_SCALE 24 |
|
| - | 108 | // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256 |
|
| - | 109 | int16_t dThrottle = (iHeight * (int32_t)staticParams.heightI) >> (IHEIGHT_SCALE); |
|
| - | 110 | ||
| 110 | } |
111 | // dt = (iHeight * staticParams.heightI) >> (IHEIGHT_SCALE) = staticParams.heightControlMaxIntegral |
| 111 | 112 | // (iHeight * staticParams.heightI) = staticParams.heightControlMaxIntegral << (IHEIGHT_SCALE) |
|
| - | 113 | // iHeight = staticParams.heightControlMaxIntegral << (IHEIGHT_SCALE) /staticParams.heightI |
|
| 112 | int16_t dThrottle = (iHeight * staticParams.heightI) / 10000L; |
114 | |
| 113 | 115 | if (dThrottle > staticParams.heightControlMaxIntegral) { |
|
| - | 116 | dThrottle = staticParams.heightControlMaxIntegral; |
|
| - | 117 | iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / staticParams.heightI; |
|
| - | 118 | } else if (dThrottle < -staticParams.heightControlMaxIntegral) { |
|
| - | 119 | dThrottle = -staticParams.heightControlMaxIntegral; |
|
| Line 114... | Line 120... | ||
| 114 | if (dThrottle > staticParams.heightControlMaxIntegralThrottleChange) |
120 | iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / staticParams.heightI; |
| Line 115... | Line 121... | ||
| 115 | dThrottle = staticParams.heightControlMaxIntegralThrottleChange; |
121 | } |
| 116 | else if (dThrottle < -staticParams.heightControlMaxIntegralThrottleChange) |
122 | |
| Line 124... | Line 130... | ||
| 124 | dThrottle = -staticParams.heightControlMaxThrottleChange; |
130 | dThrottle = -staticParams.heightControlMaxThrottleChange; |
| Line 125... | Line 131... | ||
| 125 | 131 | ||
| 126 | debugOut.analog[19] = rampedTargetHeight; |
132 | debugOut.analog[19] = rampedTargetHeight; |
| 127 | debugOut.analog[21] = dThrottle; |
133 | debugOut.analog[21] = dThrottle; |
| 128 | debugOut.analog[26] = height; |
- | |
| Line 129... | Line 134... | ||
| 129 | debugOut.analog[27] = (iHeight * staticParams.heightI) / 10000L; |
134 | debugOut.analog[26] = height; |
| 130 | 135 | ||
| 131 | if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) { |
136 | if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) { |
| 132 | if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) |
137 | if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) |