| 1,4 → 1,5 |
|---|
| #include <inttypes.h> |
| #include "analog.h" |
| #include "attitude.h" |
| #include "uart0.h" |
| #include "configuration.h" |
| 13,7 → 14,6 |
|---|
| |
| #define LATCH_TIME 40 |
| |
| int32_t groundPressure; |
| int32_t targetHeight; |
| int32_t rampedTargetHeight; |
| |
| 21,20 → 21,16 |
|---|
| int32_t maxHeightThisFlight; |
| int32_t iHeight; |
| |
| int32_t getHeight(void) { |
| return groundPressure - filteredAirPressure; |
| } |
| |
| void HC_setGround(void) { |
| groundPressure = filteredAirPressure; |
| analog_setGround(); |
| // This should also happen when height control is enabled in-flight. |
| rampedTargetHeight = getHeight(); |
| rampedTargetHeight = targetHeight = analog_getHeight(); |
| maxHeightThisFlight = 0; |
| iHeight = 0; |
| } |
| |
| void HC_update(void) { |
| int32_t height = getHeight(); |
| int32_t height = analog_getHeight(); |
| static uint8_t setHeightLatch = 0; |
| |
| if (height > maxHeightThisFlight) |
| 47,7 → 43,7 |
|---|
| if (setHeightLatch <= LATCH_TIME) { |
| if (setHeightLatch == LATCH_TIME) { |
| // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
| targetHeight = height; |
| rampedTargetHeight = targetHeight = height; |
| } |
| // Time not yet reached. |
| setHeightLatch++; |
| 58,7 → 54,7 |
|---|
| } |
| } else { |
| // Switch is not activated; take the "max-height" as the target height. |
| targetHeight = (uint16_t) dynamicParams.heightSetting * 50L - 3000L; // should be: 100 (or make a param out of it) |
| targetHeight = (uint16_t) dynamicParams.heightSetting * 25L - 500L; // should be: 100 (or make a param out of it) |
| } |
| |
| if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
| 90,17 → 86,13 |
|---|
| // takes 180-200 usec (with integral term). That is too heavy!!! |
| // takes 100 usec without integral term. |
| uint16_t HC_getThrottle(uint16_t throttle) { |
| int32_t height = getHeight(); |
| int32_t height = analog_getHeight(); |
| int32_t heightError = rampedTargetHeight - height; |
| |
| static int32_t lastHeight; |
| //static int32_t lastHeight; |
| int16_t dHeight = analog_getDHeight(); |
| //lastHeight = height; |
| |
| int16_t dHeight = height - lastHeight; |
| lastHeight = height; |
| |
| // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
| iHeight += heightError; |
| |
| if (heightError > 0) { |
| debugOut.digital[0] |= DEBUG_HEIGHT_DIFF; |
| debugOut.digital[1] &= ~DEBUG_HEIGHT_DIFF; |
| 108,14 → 100,28 |
|---|
| debugOut.digital[0] &= ~DEBUG_HEIGHT_DIFF; |
| debugOut.digital[1] |= DEBUG_HEIGHT_DIFF; |
| } |
| |
| int16_t dThrottle = (iHeight * staticParams.heightI) / 10000L; |
| |
| if (dThrottle > staticParams.heightControlMaxIntegralThrottleChange) |
| dThrottle = staticParams.heightControlMaxIntegralThrottleChange; |
| else if (dThrottle < -staticParams.heightControlMaxIntegralThrottleChange) |
| dThrottle = -staticParams.heightControlMaxIntegralThrottleChange; |
| // iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
| iHeight += heightError; |
| |
| #define IHEIGHT_SCALE 24 |
| // dThrottle is in the range between +/- 1<<(IHEIGHT_SCALE+8)>>(IHEIGHT_SCALE) = +/- 256 |
| int16_t dThrottle = (iHeight * (int32_t)staticParams.heightI) >> (IHEIGHT_SCALE); |
| |
| // dt = (iHeight * staticParams.heightI) >> (IHEIGHT_SCALE) = staticParams.heightControlMaxIntegral |
| // (iHeight * staticParams.heightI) = staticParams.heightControlMaxIntegral << (IHEIGHT_SCALE) |
| // iHeight = staticParams.heightControlMaxIntegral << (IHEIGHT_SCALE) /staticParams.heightI |
| |
| if (dThrottle > staticParams.heightControlMaxIntegral) { |
| dThrottle = staticParams.heightControlMaxIntegral; |
| iHeight = ((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / staticParams.heightI; |
| } else if (dThrottle < -staticParams.heightControlMaxIntegral) { |
| dThrottle = -staticParams.heightControlMaxIntegral; |
| iHeight = -((int32_t)staticParams.heightControlMaxIntegral << IHEIGHT_SCALE) / staticParams.heightI; |
| } |
| |
| debugOut.analog[27] = (iHeight * (int32_t)staticParams.heightI) >> (IHEIGHT_SCALE); |
| |
| dThrottle += ((heightError * staticParams.heightP) >> 10) + ((dHeight * staticParams.heightD) >> 7); |
| |
| if (dThrottle > staticParams.heightControlMaxThrottleChange) |
| 126,7 → 132,6 |
|---|
| debugOut.analog[19] = rampedTargetHeight; |
| debugOut.analog[21] = dThrottle; |
| debugOut.analog[26] = height; |
| debugOut.analog[27] = (iHeight * staticParams.heightI) / 10000L; |
| |
| if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) { |
| if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) |