14,10 → 14,12 |
|
#define LATCH_TIME 3 |
|
int32_t setHeight; |
int32_t targetHeight; |
//int32_t rampedTargetHeight; |
|
//rampinguint8_t heightRampingTimer = 0; |
uint16_t testOscPrescaler = 0; |
uint8_t testOscTimer = 0; |
|
int32_t maxHeightThisFlight; |
int32_t iHeight; |
|
24,7 → 26,8 |
void HC_setGround(void) { |
analog_setGround(); |
// This should also happen when height control is enabled in-flight. |
/*rampedTargetHeight = */ targetHeight = analog_getHeight(); |
setHeight = targetHeight = analog_getHeight(); |
testOscTimer = 0; |
maxHeightThisFlight = 0; |
iHeight = 0; |
} |
48,7 → 51,8 |
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; |
setHeight = height; |
testOscTimer = 0; |
iHeight = 0; |
} |
// Time not yet reached. |
60,7 → 64,7 |
} |
} else { |
// Switch is not activated; take the "max-height" as the target height. |
targetHeight = (uint16_t) dynamicParams.heightSetting * 25L - 500L; // should be: 100 (or make a param out of it) |
setHeight = (uint16_t) dynamicParams.heightSetting * 25L - 500L; // should be: 100 (or make a param out of it) |
} |
|
/* |
83,7 → 87,27 |
} |
} |
*/ |
// uint8_t heightControlTestOscPeriod; |
// uint8_t heightControlTestOscAmplitude; |
|
testOscPrescaler++; |
if (testOscPrescaler == 488) { |
testOscPrescaler = 0; |
testOscTimer++; |
if (testOscTimer == staticParams.heightControlTestOscPeriod) { |
testOscTimer = 0; |
iHeight = 0; |
} else if (testOscTimer == staticParams.heightControlTestOscPeriod/2) { |
iHeight = 0; |
} |
} |
|
if (testOscTimer < staticParams.heightControlTestOscPeriod/2) |
targetHeight = setHeight; |
else |
targetHeight = setHeight + (uint16_t)staticParams.heightControlTestOscAmplitude * 25L; |
|
//if (staticParams.) |
// height, in meters (so the division factor is: 100) |
// debugOut.analog[24] = (117100 - filteredAirPressure) / 100; |
// Calculated 0 alt number: 108205 |
90,6 → 114,10 |
// Experimental 0 alt number: 117100 |
} |
|
#define LOG_PHEIGHT_SCALE 10 |
#define LOG_IHEIGHT_SCALE 24 |
#define LOG_DHEIGHT_SCALE 6 |
|
// takes 180-200 usec (with integral term). That is too heavy!!! |
// takes 100 usec without integral term. |
void HC_periodicTaskAndPRTY(int16_t* PRTY) { |
115,7 → 143,7 |
} |
|
int32_t heightErrorForIntegral = heightError; |
int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << 10; |
int32_t heightErrorForIntegralLimit = staticParams.heightControlMaxIntegralIn << LOG_PHEIGHT_SCALE; |
|
if (heightErrorForIntegral > heightErrorForIntegralLimit) { |
heightErrorForIntegral = heightErrorForIntegralLimit; |
138,8 → 166,8 |
iHeight = -((int32_t)staticParams.heightControlMaxIntegralOut << IHEIGHT_SCALE) / dynamicParams.heightI; |
} |
|
int16_t dThrottleP = (heightError * dynamicParams.heightP) >> 10; |
int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> 7; |
int16_t dThrottleP = (heightError * dynamicParams.heightP) >> LOG_PHEIGHT_SCALE; |
int16_t dThrottleD = (dHeight * dynamicParams.heightD) >> LOG_DHEIGHT_SCALE; |
|
debugOut.analog[10] = dThrottleP; |
debugOut.analog[11] = dThrottleI; |