Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1586 → Rev 1587

/trunk/fc.c
1243,16 → 1243,16
#define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging
#define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging
#define OPA_OFFSET_STEP 10
int HCGas, HeightDeviation;
int HCGas, HeightDeviation = 0;
static int HeightTrimming = 0; // rate for change of height setpoint
static int FilterHCGas = 0;
static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023;
static unsigned long HooverGasFilter = 0;
static int StickGasHover = 120, HoverGas = 0, HoverGasMin = 0, HoverGasMax = 1023;
static unsigned long HoverGasFilter = 0;
static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0;
int CosAttitude; // for projection of hoover gas
 
// get the current hooverpoint
DebugOut.Analog[21] = HooverGas;
DebugOut.Analog[21] = HoverGas;
DebugOut.Analog[18] = VarioMeter;
 
// Expand the measurement
1358,7 → 1358,7
// the setpoint will be fine adjusted with the gas stick position
if(FCFlags & FCFLAG_FLY) // trim setpoint only when flying
{ // gas stick is above hoover point
if(StickGas > (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit)
if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit)
{
if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN)
{
1366,9 → 1366,9
SollHoehe = HoehenWert; // update setpoint to current heigth
}
HeightTrimmingFlag |= HEIGHT_TRIM_UP;
HeightTrimming += abs(StickGas - (StickGasHoover + HEIGHT_CONTROL_STICKTHRESHOLD));
HeightTrimming += abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD));
} // gas stick is below hoover point
else if(StickGas < (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit )
else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit )
{
if(HeightTrimmingFlag & HEIGHT_TRIM_UP)
{
1376,9 → 1376,9
SollHoehe = HoehenWert; // update setpoint to current heigth
}
HeightTrimmingFlag |= HEIGHT_TRIM_DOWN;
HeightTrimming -= abs(StickGas - (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD));
HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD));
}
else // Gas Stick in Hoover Range
else // Gas Stick in Hover Range
{
if(HeightTrimmingFlag & (HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN))
{
1393,15 → 1393,15
{
SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint
HeightTrimming = 0;
LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied
LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied
if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75;
//update hoover gas stick value when setpoint is shifted
if(!EE_Parameter.Hoehe_StickNeutralPoint)
{
StickGasHoover = HooverGas/STICK_GAIN; //rescale back to stick value
StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
if(StickGasHoover < 70) StickGasHoover = 70;
else if(StickGasHoover > 150) StickGasHoover = 150;
StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value
StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning;
if(StickGasHover < 70) StickGasHover = 70;
else if(StickGasHover > 150) StickGasHover = 150;
}
}
if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active
1409,42 → 1409,45
else
{
SollHoehe = HoehenWert - 400;
if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint;
else StickGasHoover = 120;
if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint;
else StickGasHover = 120;
}
HCGas = HooverGas; // take hoover gas (neutral point)
HCGas = HoverGas; // take hoover gas (neutral point)
}
if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT))
{
// ------------------------- P-Part ----------------------------
tmp_long = (HoehenWert - SollHoehe); // positive when too high
LIMIT_MIN_MAX(tmp_long, -2000, 2000); // avoid overflov when casting to int
LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t
HeightDeviation = (int)(tmp_long); // positive when too high
tmp_int = (HeightDeviation * (long)Parameter_Hoehe_P) / 16; // p-part
HCGas -= tmp_int;
tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 16L; // p-part
LIMIT_MIN_MAX(tmp_long, -255 * STICK_GAIN, 255 * STICK_GAIN); // more than 2 times the full range makes no sense
HCGas -= tmp_long;
// ------------------------- D-Part 1: Vario Meter ----------------------------
tmp_int = VarioMeter / 8;
if(tmp_int > 8) tmp_int = 8; // limit quadratic part on upward movement to avoid to much gas reduction
if(tmp_int > 0) tmp_int = VarioMeter + (tmp_int * tmp_int) / 4;
else tmp_int = VarioMeter - (tmp_int * tmp_int) / 4;
tmp_int = (Parameter_Luftdruck_D * (long)(tmp_int)) / 128L; // scale to d-gain parameter
LIMIT_MIN_MAX(tmp_int, -127, 255);
HCGas -= tmp_int;
tmp_long = VarioMeter / 8;
if(tmp_long > 8) tmp_long = 8; // limit quadratic part on upward movement to avoid to much gas reduction
if(tmp_long > 0) tmp_long = VarioMeter + (tmp_long * tmp_long) / 4L;
else tmp_long = VarioMeter - (tmp_long * tmp_long) / 4L;
tmp_long = (tmp_long * (long)Parameter_Luftdruck_D) / 128L; // scale to d-gain parameter
LIMIT_MIN_MAX(tmp_long,-32 * STICK_GAIN, 64 * STICK_GAIN);
HCGas -= tmp_long;
// ------------------------ D-Part 2: ACC-Z Integral ------------------------
tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
LIMIT_MIN_MAX(tmp_int, -127, 255);
tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN);
LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN);
HCGas -= tmp_long;
 
if(BaroExpandActive) HCGas = HoverGas;
// ------------------------ D-Part 3: GpsZ ----------------------------------
tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L;
LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN);
HCGas -= tmp_int;
 
// limit deviation from hoover point within the target region
if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero
if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HoverGas > 0)) // height setpoint is not changed and hoover gas not zero
{
LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point
LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point
}
 
if(BaroExpandActive) HCGas = HooverGas;
// ------------------------ D-Part 3: GpsZ ----------------------------------
tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L;
LIMIT_MIN_MAX(tmp_int, -127, 255);
HCGas -= tmp_int;
// strech control output by inverse attitude projection 1/cos
// + 1/cos(angle) ++++++++++++++++++++++++++
tmp_long2 = (int32_t)HCGas;
1457,9 → 1460,9
LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN);
// set GasMischanteil to HeightControlGasFilter
if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)
{ // old version
if(FilterHCGas > GasMischanteil) FilterHCGas = GasMischanteil; // nicht mehr als Gas
}
{ // old version
LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
}
GasMischanteil = FilterHCGas;
}
}// EOF height control active
1466,22 → 1469,21
else // HC not active
{
//update hoover gas stick value when HC is not active
if(!EE_Parameter.Hoehe_StickNeutralPoint)
{
StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
if(!EE_Parameter.Hoehe_StickNeutralPoint)
{
StickGasHover = HoverGas/STICK_GAIN; // rescale back to stick value
StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning;
}
else StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint;
if(StickGasHoover < 70) StickGasHoover = 70;
else if(StickGasHoover > 150) StickGasHoover = 150;
else StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint;
LIMIT_MIN_MAX(StickGasHover, 70, 150); // reserve some range for trim up and down
FilterHCGas = GasMischanteil;
}
 
// Hoover gas estimation by averaging gas control output on small z-velocities
// Hover gas estimation by averaging gas control output on small z-velocities
// this is done only if height contol option is selected in global config and aircraft is flying
if((FCFlags & FCFLAG_FLY) && !(FCFlags & FCFLAG_NOTLANDUNG))
{
if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
if(HoverGasFilter == 0) HoverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
if(abs(VarioMeter) < 100) // only on small vertical speed
{
tmp_long2 = (int32_t)GasMischanteil; // take current thrust
1491,36 → 1493,36
// average vertical projected thrust
if(modell_fliegt < 2000) // the first 4 seconds
{ // reduce the time constant of averaging by factor of 8 to get much faster a stable value
HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/8L);
HooverGasFilter += 8L * tmp_long2;
HoverGasFilter -= HoverGasFilter/(HOOVER_GAS_AVERAGE/8L);
HoverGasFilter += 8L * tmp_long2;
}
else if(modell_fliegt < 4000) // the first 8 seconds
{ // reduce the time constant of averaging by factor of 4 to get much faster a stable value
HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/4L);
HooverGasFilter += 4L * tmp_long2;
HoverGasFilter -= HoverGasFilter/(HOOVER_GAS_AVERAGE/4L);
HoverGasFilter += 4L * tmp_long2;
}
else if(modell_fliegt < 8000) // the first 16 seconds
{ // reduce the time constant of averaging by factor of 2 to get much faster a stable value
HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/2L);
HooverGasFilter += 2L * tmp_long2;
HoverGasFilter -= HoverGasFilter/(HOOVER_GAS_AVERAGE/2L);
HoverGasFilter += 2L * tmp_long2;
}
else //later
{
HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE;
HooverGasFilter += tmp_long2;
HoverGasFilter -= HoverGasFilter/HOOVER_GAS_AVERAGE;
HoverGasFilter += tmp_long2;
}
HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE);
HoverGas = (int16_t)(HoverGasFilter/HOOVER_GAS_AVERAGE);
if(EE_Parameter.Hoehe_HoverBand)
{
int16_t band;
band = HooverGas / EE_Parameter.Hoehe_HoverBand; // the higher the parameter the smaller the range
HooverGasMin = HooverGas - band;
HooverGasMax = HooverGas + band;
band = HoverGas / EE_Parameter.Hoehe_HoverBand; // the higher the parameter the smaller the range
HoverGasMin = HoverGas - band;
HoverGasMax = HoverGas + band;
}
else
{ // no limit
HooverGasMin = 0;
HooverGasMax = 1023;
HoverGasMin = 0;
HoverGasMax = 1023;
}
}
}