13,11 → 13,11 |
#define INTEGRAL_LIMIT 100000 |
|
/* |
#define DEBUGINTEGRAL 0 |
#define DEBUGDIFFERENTIAL 0 |
#define DEBUGHOVERTHROTTLE 0 |
#define DEBUGHEIGHTSWITCH 0 |
*/ |
#define DEBUGINTEGRAL 0 |
#define DEBUGDIFFERENTIAL 0 |
#define DEBUGHOVERTHROTTLE 0 |
#define DEBUGHEIGHTSWITCH 0 |
*/ |
|
#define LATCH_TIME 40 |
|
36,70 → 36,70 |
int32_t maxHeight; |
int32_t iHeight; |
/* |
These parameters are free to take: |
uint8_t HeightMinGas; // Value : 0-100 |
uint8_t HeightD; // Value : 0-250 |
uint8_t MaxHeight; // Value : 0-32 |
uint8_t HeightP; // Value : 0-32 |
uint8_t Height_Gain; // Value : 0-50 |
uint8_t Height_ACC_Effect; // Value : 0-250 |
These parameters are free to take: |
uint8_t HeightMinGas; // Value : 0-100 |
uint8_t HeightD; // Value : 0-250 |
uint8_t MaxHeight; // Value : 0-32 |
uint8_t HeightP; // Value : 0-32 |
uint8_t Height_Gain; // Value : 0-50 |
uint8_t Height_ACC_Effect; // Value : 0-250 |
*/ |
|
int32_t getHeight(void) { |
return groundPressure - filteredAirPressure; |
return groundPressure - filteredAirPressure; |
} |
|
void HC_setGround(void) { |
groundPressure = filteredAirPressure; |
// This should also happen when height control is enabled in-flight. |
rampedTargetHeight = getHeight(); |
maxHeight = 0; |
iHeight = 0; |
groundPressure = filteredAirPressure; |
// This should also happen when height control is enabled in-flight. |
rampedTargetHeight = getHeight(); |
maxHeight = 0; |
iHeight = 0; |
} |
|
void HC_update(void) { |
int32_t height = getHeight(); |
static uint8_t setHeightLatch = 0; |
int32_t height = getHeight(); |
static uint8_t setHeightLatch = 0; |
|
if (height > maxHeight) |
maxHeight = height; |
if (height > maxHeight) |
maxHeight = height; |
|
if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) { |
// If switch is activated in config |
DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH; |
if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255-40) { |
// 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. |
targetHeight = height; |
DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH; |
if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) { |
// If switch is activated in config |
DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH; |
if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40) { |
// 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. |
targetHeight = height; |
DebugOut.Digital[1] |= DEBUG_HEIGHT_SWITCH; |
} |
// Time not yet reached. |
setHeightLatch++; |
} |
} else { |
// Switch is OFF. |
setHeightLatch = 0; |
DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH; |
} |
} else { |
// Switch is not activated; take the "max-height" as the target height. |
DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH; |
targetHeight = (uint16_t) dynamicParams.MaxHeight * 100; //getHeight() + 10 * 100; |
} |
// Time not yet reached. |
setHeightLatch++; |
} |
} else { |
// Switch is OFF. |
setHeightLatch = 0; |
DebugOut.Digital[1] &= ~DEBUG_HEIGHT_SWITCH; |
} |
} else { |
// Switch is not activated; take the "max-height" as the target height. |
DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH; |
targetHeight = (uint16_t)dynamicParams.MaxHeight * 100; //getHeight() + 10 * 100; |
} |
|
if (++heightRampingTimer == INTEGRATION_FREQUENCY/10) { |
heightRampingTimer = 0; |
if (rampedTargetHeight + staticParams.Height_Gain <= targetHeight) { |
rampedTargetHeight += staticParams.Height_Gain; |
} else if (rampedTargetHeight - staticParams.Height_Gain >= targetHeight) { |
rampedTargetHeight -= staticParams.Height_Gain; |
} |
} |
|
// height, in meters (so the division factor is: 100) |
DebugOut.Analog[30] = height / 100; |
if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
heightRampingTimer = 0; |
if (rampedTargetHeight + staticParams.Height_Gain <= targetHeight) { |
rampedTargetHeight += staticParams.Height_Gain; |
} else if (rampedTargetHeight - staticParams.Height_Gain >= targetHeight) { |
rampedTargetHeight -= staticParams.Height_Gain; |
} |
} |
|
// height, in meters (so the division factor is: 100) |
DebugOut.Analog[30] = height / 100; |
} |
|
// ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
109,69 → 109,77 |
// 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 heightError = rampedTargetHeight - height; |
|
static int32_t lastHeight; |
int32_t height = getHeight(); |
int32_t heightError = rampedTargetHeight - height; |
|
int16_t dHeight = height - lastHeight; |
lastHeight = height; |
static int32_t lastHeight; |
|
// DebugOut.Analog[20] = dHeight; |
// DebugOut.Analog[21] = dynamicParams.MaxHeight; |
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; |
// DebugOut.Analog[20] = dHeight; |
// DebugOut.Analog[21] = dynamicParams.MaxHeight; |
|
if (dHeight > 0) { |
DebugOut.Digital[0] |= DEBUG_HEIGHT_DIFF; |
DebugOut.Digital[1] &= ~DEBUG_HEIGHT_DIFF; |
} else if (dHeight < 0) { |
DebugOut.Digital[1] |= DEBUG_HEIGHT_DIFF; |
DebugOut.Digital[0] &= ~DEBUG_HEIGHT_DIFF; |
} |
// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec.... |
// iHeight += heightError; |
|
/* |
if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}} |
else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }} |
else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;} |
else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;} |
*/ |
if (dHeight > 0) { |
DebugOut.Digital[0] |= DEBUG_HEIGHT_DIFF; |
DebugOut.Digital[1] &= ~DEBUG_HEIGHT_DIFF; |
} else if (dHeight < 0) { |
DebugOut.Digital[1] |= DEBUG_HEIGHT_DIFF; |
DebugOut.Digital[0] &= ~DEBUG_HEIGHT_DIFF; |
} |
|
int16_t dThrottle = heightError * staticParams.HeightP / 1000 /*+ iHeight / 10000L * staticParams.Height_ACC_Effect */ - dHeight * staticParams.HeightD; |
/* |
if (iHeight > INTEGRAL_LIMIT) { iHeight = INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 1; DebugOut.Digital[1] = 1;}} |
else if (iHeight < -INTEGRAL_LIMIT) { iHeight = -INTEGRAL_LIMIT; if (DEBUGINTEGRAL) {DebugOut.Digital[0] = 0; DebugOut.Digital[1] = 0; }} |
else if (iHeight > 0) { if (DEBUGINTEGRAL) DebugOut.Digital[0] = 1;} |
else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;} |
*/ |
|
// the "minGas" is now a limit for how much up / down the throttle can be varied |
if (dThrottle > staticParams.HeightMinGas) dThrottle = staticParams.HeightMinGas; |
else if (dThrottle < -staticParams.HeightMinGas) dThrottle = -staticParams.HeightMinGas; |
int16_t dThrottle = heightError * staticParams.HeightP / 1000 |
/*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight |
* staticParams.HeightD; |
|
//DebugOut.Analog[18] = dThrottle; |
//DebugOut.Analog[19] = iHeight / 10000L; |
// the "minGas" is now a limit for how much up / down the throttle can be varied |
if (dThrottle > staticParams.HeightMinGas) |
dThrottle = staticParams.HeightMinGas; |
else if (dThrottle < -staticParams.HeightMinGas) |
dThrottle = -staticParams.HeightMinGas; |
|
// TODO: Eliminate repitition. |
if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) { |
if (!(staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) || (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255-40)) { |
// 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; |
} |
} |
|
/* Experiment: Find hover-throttle */ |
stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff * (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER; |
stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER - 1) + throttle) / HOVERTHROTTLEFILTER; |
//DebugOut.Analog[18] = dThrottle; |
//DebugOut.Analog[19] = iHeight / 10000L; |
|
if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 && stronglyFilteredHeightDiff > -3) { |
hoverThrottle = stronglyFilteredThrottle; |
DebugOut.Digital[0] |= DEBUG_HOVERTHROTTLE; |
// DebugOut.Analog[18] = hoverThrottle; |
} else |
DebugOut.Digital[0] &= ~DEBUG_HOVERTHROTTLE; |
return throttle; |
// TODO: Eliminate repitition. |
if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) { |
if (!(staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) |
|| (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40)) { |
// 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; |
} |
} |
|
/* Experiment: Find hover-throttle */ |
stronglyFilteredHeightDiff = (stronglyFilteredHeightDiff |
* (HOVERTHROTTLEFILTER - 1) + dHeight) / HOVERTHROTTLEFILTER; |
stronglyFilteredThrottle = (stronglyFilteredThrottle * (HOVERTHROTTLEFILTER |
- 1) + throttle) / HOVERTHROTTLEFILTER; |
|
if (isFlying >= 1000 && stronglyFilteredHeightDiff < 3 |
&& stronglyFilteredHeightDiff > -3) { |
hoverThrottle = stronglyFilteredThrottle; |
DebugOut.Digital[0] |= DEBUG_HOVERTHROTTLE; |
// DebugOut.Analog[18] = hoverThrottle; |
} else |
DebugOut.Digital[0] &= ~DEBUG_HOVERTHROTTLE; |
return throttle; |
} |
|
/* |
For a variometer thingy: |
When switch is thrown on, freeze throttle (capture it into variable) |
For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping. |
Output = frozen throttle + whatever is computed +/-. Integral? |
*/ |
For a variometer thingy: |
When switch is thrown on, freeze throttle (capture it into variable) |
For each iter., add (throttle - frozen throttle) to target height. Maybe don't do ramping. |
Output = frozen throttle + whatever is computed +/-. Integral? |
*/ |