| 12,12 → 12,12 |
|---|
| |
| #define INTEGRAL_LIMIT 100000 |
| |
| #define LATCH_TIME 40 |
| #define LATCH_TIME 3 |
| |
| int32_t targetHeight; |
| int32_t rampedTargetHeight; |
| //int32_t rampedTargetHeight; |
| |
| uint8_t heightRampingTimer = 0; |
| //rampinguint8_t heightRampingTimer = 0; |
| int32_t maxHeightThisFlight; |
| int32_t iHeight; |
| |
| 24,11 → 24,15 |
|---|
| void HC_setGround(void) { |
| analog_setGround(); |
| // This should also happen when height control is enabled in-flight. |
| rampedTargetHeight = targetHeight = analog_getHeight(); |
| /*rampedTargetHeight = */ targetHeight = analog_getHeight(); |
| maxHeightThisFlight = 0; |
| iHeight = 0; |
| } |
| |
| uint8_t HC_isSwitchOn() { |
| return (dynamicParams.heightSetting >= 255/3); |
| } |
| |
| void HC_periodicTask(void) { |
| int32_t height = analog_getHeight(); |
| static uint8_t setHeightLatch = 0; |
| 39,17 → 43,15 |
|---|
| // debugOut.analog[25] = dynamicParams.heightSetting; |
| |
| if (staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) { |
| // If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle. |
| // if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) { |
| if (dynamicParams.heightSetting >= 255/3) { |
| if (HC_isSwitchOn()) { |
| // Switch is ON |
| 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. |
| rampedTargetHeight = targetHeight = height; |
| } |
| // Time not yet reached. |
| setHeightLatch++; |
| if (setHeightLatch == LATCH_TIME) { |
| // Freeze the height as target. We want to do this exactly once each time the switch is thrown ON. |
| /* rampedTargetHeight = */ targetHeight = height; |
| } |
| // Time not yet reached. |
| setHeightLatch++; |
| } |
| } else { |
| // Switch is OFF. |
| 60,6 → 62,7 |
|---|
| targetHeight = (uint16_t) dynamicParams.heightSetting * 25L - 500L; // should be: 100 (or make a param out of it) |
| } |
| |
| /* |
| if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
| heightRampingTimer = 0; |
| if (rampedTargetHeight < targetHeight) { |
| 78,6 → 81,7 |
|---|
| } |
| } |
| } |
| */ |
| |
| // height, in meters (so the division factor is: 100) |
| // debugOut.analog[24] = (117100 - filteredAirPressure) / 100; |
| 91,7 → 95,7 |
|---|
| HC_periodicTask(); |
| int16_t throttle = PRTY[CONTROL_THROTTLE]; |
| int32_t height = analog_getHeight(); |
| int32_t heightError = rampedTargetHeight - height; |
| int32_t heightError = /*ramped*/ targetHeight - height; |
| int16_t dHeight = analog_getDHeight(); |
| |
| debugOut.analog[22] = height/10L; |
| 151,8 → 155,7 |
|---|
| */ |
| |
| if (staticParams.bitConfig & CFG_SIMPLE_HEIGHT_CONTROL) { |
| if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) |
| || (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) { |
| if (!(staticParams.bitConfig & CFG_SIMPLE_HC_HOLD_SWITCH) || HC_isSwitchOn()) { |
| // If switch is not in use --> Just apply height control. |
| // If switch is in use --> only apply height control when switch is also ON. |
| throttle += dThrottle; |