Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1820 → Rev 1821

/branches/dongfang_FC_rewrite/heightControl.c
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?
*/