Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1959 → Rev 1960

/branches/dongfang_FC_rewrite/heightControl.c
46,60 → 46,60
*/
 
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;
 
if (height > maxHeight)
maxHeight = height;
 
if (staticParams.GlobalConfig & CFG_HEIGHT_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.
// 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;
int32_t height = getHeight();
static uint8_t setHeightLatch = 0;
if (height > maxHeight)
maxHeight = height;
if (staticParams.bitConfig & CFG_HEIGHT_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.
// DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH;
if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 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 (++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 / 10;
// 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.heightSetting * 100; //getHeight() + 10 * 100;
}
if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) {
heightRampingTimer = 0;
if (rampedTargetHeight + staticParams.heightSlewRate <= targetHeight) {
rampedTargetHeight += staticParams.heightSlewRate;
} else if (rampedTargetHeight - staticParams.heightSlewRate >= targetHeight) {
rampedTargetHeight -= staticParams.heightSlewRate;
}
}
// height, in meters (so the division factor is: 100)
debugOut.analog[30] = height / 10;
}
 
// ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
109,73 → 109,73
// 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;
 
int16_t dHeight = height - lastHeight;
lastHeight = height;
 
// DebugOut.Analog[20] = dHeight;
// DebugOut.Analog[21] = dynamicParams.MaxHeight;
 
// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
// iHeight += heightError;
 
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;
}
 
/*
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;}
*/
 
int16_t dThrottle = heightError * staticParams.HeightP / 1000
/*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight
* staticParams.HeightD;
 
// 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;
 
int32_t height = getHeight();
int32_t heightError = rampedTargetHeight - height;
static int32_t lastHeight;
int16_t dHeight = height - lastHeight;
lastHeight = height;
// DebugOut.Analog[20] = dHeight;
// DebugOut.Analog[21] = dynamicParams.MaxHeight;
// iHeight, at a difference of 5 meters and a freq. of 488 Hz, will grow with 244000 / sec....
// iHeight += heightError;
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;
}
/*
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;}
*/
int16_t dThrottle = heightError * staticParams.heightP / 1000
/*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight
* staticParams.heightD;
// the "minGas" is now a limit for how much up / down the throttle can be varied
if (dThrottle > staticParams.heightMaxThrottleChange)
dThrottle = staticParams.heightMaxThrottleChange;
else if (dThrottle < -staticParams.heightMaxThrottleChange)
dThrottle = -staticParams.heightMaxThrottleChange;
// TODO: Eliminate repetition.
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;
 
debugOut.analog[20] = dThrottle;
debugOut.analog[21] = hoverThrottle;
 
return throttle;
if (staticParams.bitConfig & CFG_HEIGHT_CONTROL) {
if (!(staticParams.bitConfig & CFG_HEIGHT_SWITCH)
|| (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 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;
debugOut.analog[20] = dThrottle;
debugOut.analog[21] = hoverThrottle;
return throttle;
}
 
/*