Rev 2073 | Rev 2075 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2073 | Rev 2074 | ||
|---|---|---|---|
| Line 12... | Line 12... | ||
| 12 | 12 | ||
| Line 13... | Line 13... | ||
| 13 | #define INTEGRAL_LIMIT 100000 |
13 | #define INTEGRAL_LIMIT 100000 |
| Line -... | Line 14... | ||
| - | 14 | ||
| 14 | 15 | #define LATCH_TIME 3 |
|
| 15 | #define LATCH_TIME 3 |
- | |
| Line -... | Line 16... | ||
| - | 16 | ||
| 16 | 17 | int32_t setHeight; |
|
| - | 18 | int32_t targetHeight; |
|
| 17 | int32_t targetHeight; |
19 | |
| 18 | //int32_t rampedTargetHeight; |
20 | uint16_t testOscPrescaler = 0; |
| Line 19... | Line 21... | ||
| 19 | 21 | uint8_t testOscTimer = 0; |
|
| 20 | //rampinguint8_t heightRampingTimer = 0; |
22 | |
| 21 | int32_t maxHeightThisFlight; |
23 | int32_t maxHeightThisFlight; |
| 22 | int32_t iHeight; |
24 | int32_t iHeight; |
| - | 25 | ||
| 23 | 26 | void HC_setGround(void) { |
|
| 24 | void HC_setGround(void) { |
27 | analog_setGround(); |
| 25 | analog_setGround(); |
28 | // This should also happen when height control is enabled in-flight. |
| Line 26... | Line 29... | ||
| 26 | // This should also happen when height control is enabled in-flight. |
29 | setHeight = targetHeight = analog_getHeight(); |
| Line 46... | Line 49... | ||
| 46 | if (HC_isSwitchOn()) { |
49 | if (HC_isSwitchOn()) { |
| 47 | // Switch is ON |
50 | // Switch is ON |
| 48 | if (setHeightLatch <= LATCH_TIME) { |
51 | if (setHeightLatch <= LATCH_TIME) { |
| 49 | if (setHeightLatch == LATCH_TIME) { |
52 | if (setHeightLatch == LATCH_TIME) { |
| 50 | // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
53 | // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
| 51 | /* rampedTargetHeight = */ targetHeight = height; |
54 | setHeight = height; |
| - | 55 | testOscTimer = 0; |
|
| 52 | iHeight = 0; |
56 | iHeight = 0; |
| 53 | } |
57 | } |
| 54 | // Time not yet reached. |
58 | // Time not yet reached. |
| 55 | setHeightLatch++; |
59 | setHeightLatch++; |
| 56 | } |
60 | } |
| Line 58... | Line 62... | ||
| 58 | // Switch is OFF. |
62 | // Switch is OFF. |
| 59 | setHeightLatch = 0; |
63 | setHeightLatch = 0; |
| 60 | } |
64 | } |
| 61 | } else { |
65 | } else { |
| 62 | // Switch is not activated; take the "max-height" as the target height. |
66 | // Switch is not activated; take the "max-height" as the target height. |
| 63 | targetHeight = (uint16_t) dynamicParams.heightSetting * 25L - 500L; // should be: 100 (or make a param out of it) |
67 | setHeight = (uint16_t) dynamicParams.heightSetting * 25L - 500L; // should be: 100 (or make a param out of it) |
| 64 | } |
68 | } |
| Line 65... | Line 69... | ||
| 65 | 69 | ||
| 66 | /* |
70 | /* |
| 67 | if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
71 | if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
| Line 81... | Line 85... | ||
| 81 | rampedTargetHeight = targetHeight; |
85 | rampedTargetHeight = targetHeight; |
| 82 | } |
86 | } |
| 83 | } |
87 | } |
| 84 | } |
88 | } |
| 85 | */ |
89 | */ |
| - | 90 | // uint8_t heightControlTestOscPeriod; |
|
| - | 91 | // uint8_t heightControlTestOscAmplitude; |
|
| - | 92 | ||
| - | 93 | testOscPrescaler++; |
|
| - | 94 | if (testOscPrescaler == 488) { |
|
| - | 95 | testOscPrescaler = 0; |
|
| - | 96 | testOscTimer++; |
|
| - | 97 | if (testOscTimer == staticParams.heightControlTestOscPeriod) { |
|
| - | 98 | testOscTimer = 0; |
|
| - | 99 | iHeight = 0; |
|
| - | 100 | } else if (testOscTimer == staticParams.heightControlTestOscPeriod/2) { |
|
| - | 101 | iHeight = 0; |
|
| - | 102 | } |
|
| - | 103 | } |
|
| - | 104 | ||
| - | 105 | if (testOscTimer < staticParams.heightControlTestOscPeriod/2) |
|
| - | 106 | targetHeight = setHeight; |
|
| - | 107 | else |
|
| - | 108 | targetHeight = setHeight + (uint16_t)staticParams.heightControlTestOscAmplitude * 25L; |
|
| Line -... | Line 109... | ||
| - | 109 | ||
| 86 | 110 | //if (staticParams.) |
|
| 87 | // height, in meters (so the division factor is: 100) |
111 | // height, in meters (so the division factor is: 100) |
| 88 | // debugOut.analog[24] = (117100 - filteredAirPressure) / 100; |
112 | // debugOut.analog[24] = (117100 - filteredAirPressure) / 100; |
| 89 | // Calculated 0 alt number: 108205 |
113 | // Calculated 0 alt number: 108205 |
| 90 | // Experimental 0 alt number: 117100 |
114 | // Experimental 0 alt number: 117100 |
| Line -... | Line 115... | ||
| - | 115 | } |
|
| - | 116 | ||
| - | 117 | #define LOG_PHEIGHT_SCALE 10 |
|
| - | 118 | #define LOG_IHEIGHT_SCALE 24 |
|
| 91 | } |
119 | #define LOG_DHEIGHT_SCALE 6 |
| 92 | 120 | ||
| 93 | // takes 180-200 usec (with integral term). That is too heavy!!! |
121 | // takes 180-200 usec (with integral term). That is too heavy!!! |
| 94 | // takes 100 usec without integral term. |
122 | // takes 100 usec without integral term. |
| 95 | void HC_periodicTaskAndPRTY(int16_t* PRTY) { |
123 | void HC_periodicTaskAndPRTY(int16_t* PRTY) { |
| Line 113... | Line 141... | ||
| 113 | } else { |
141 | } else { |
| 114 | debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
142 | debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
| 115 | } |
143 | } |
| Line 116... | Line 144... | ||
| 116 | 144 | ||
| 117 | int32_t heightErrorForIntegral = heightError; |
145 | int32_t heightErrorForIntegral = heightError; |
| Line 118... | Line 146... | ||
| 118 | int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << 10; |
146 | int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << LOG_PHEIGHT_SCALE; |
| 119 | 147 | ||
| 120 | if (heightErrorForIntegral > heightErrorForIntegralLimit) { |
148 | if (heightErrorForIntegral > heightErrorForIntegralLimit) { |
| 121 | heightErrorForIntegral = heightErrorForIntegralLimit; |
149 | heightErrorForIntegral = heightErrorForIntegralLimit; |
| Line 136... | Line 164... | ||
| 136 | } else if (dThrottleI < -staticParams.heightControlMaxIntegralOut) { |
164 | } else if (dThrottleI < -staticParams.heightControlMaxIntegralOut) { |
| 137 | dThrottleI = -staticParams.heightControlMaxIntegralOut; |
165 | dThrottleI = -staticParams.heightControlMaxIntegralOut; |
| 138 | iHeight = -((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI; |
166 | iHeight = -((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI; |
| 139 | } |
167 | } |
| Line 140... | Line 168... | ||
| 140 | 168 | ||
| 141 | int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10; |
169 | int16_t dThrottleP = (heightError * dynamicParams.heightP) >> LOG_PHEIGHT_SCALE; |
| Line 142... | Line 170... | ||
| 142 | int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7; |
170 | int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> LOG_DHEIGHT_SCALE; |
| 143 | 171 | ||
| 144 | debugOut.analog[10] = dThrottleP; |
172 | debugOut.analog[10] = dThrottleP; |
| 145 | debugOut.analog[11] = dThrottleI; |
173 | debugOut.analog[11] = dThrottleI; |