95,7 → 95,7 |
unsigned char Poti[9] = {0,0,0,0,0,0,0,0}; |
volatile unsigned char SenderOkay = 0; |
int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
char MotorenEin = 0; |
char MotorenEin = 0,StartTrigger = 0; |
long HoehenWert = 0; |
long SollHoehe = 0; |
int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0; |
1285,7 → 1285,7 |
// if height control is activated |
if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
{ |
#define HOVER_GAS_AVERAGE 16384L // 4096 * 2ms = 8.2s averaging |
#define HOVER_GAS_AVERAGE 16384L // 16384 * 2ms = 32s averaging |
#define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging |
|
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
1438,6 → 1438,10 |
HeightTrimming = 0; |
SollHoehe = HoehenWert; // update setpoint to current height |
if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
if(!StartTrigger && HoehenWert > 50) |
{ |
StartTrigger = 1; |
} |
} |
VarioCharacter = '='; |
} |
1464,6 → 1468,7 |
SollHoehe = HoehenWert - 400; |
if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint; |
else StickGasHover = 120; |
HoverGas = GasMischanteil; |
} |
HCGas = HoverGas; // take hover gas (neutral point) |
} |
1482,10 → 1487,9 |
LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
HeightDeviation = (int)(tmp_long); // positive when too high |
tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part |
LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 255 * STICK_GAIN); // more than the full range makes no sense |
// HCGas -= tmp_long; |
LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense |
DebugOut.Analog[25] = tmp_long; |
GasReduction = tmp_long; |
GasReduction = tmp_long; |
// ------------------------- D-Part 1: Vario Meter ---------------------------- |
tmp_int = VarioMeter / 8; |
LIMIT_MIN_MAX(tmp_int, -127, 128); |
1495,23 → 1499,31 |
// if(tmp_int > 0) tmp_int = tmp_int + (tmp_int2 * tmp_int2); |
// else tmp_int = tmp_int - (tmp_int2 * tmp_int2); |
|
tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
// HCGas -= tmp_int; |
if(!HeightTrimmingFlag) |
{ |
tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
} |
|
else { |
tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 16L; // weniger D zum Höhe ändern |
LIMIT_MIN_MAX(tmp_int,-16 * STICK_GAIN, 16 * STICK_GAIN); |
} |
|
DebugOut.Analog[19] = tmp_int; |
GasReduction += tmp_int; |
|
GasReduction += tmp_int; |
} // EOF no baro range expanding |
// ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
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; |
GasReduction += tmp_long; |
if(Parameter_Hoehe_ACC_Wirkung) |
{ |
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); |
GasReduction += tmp_long; |
} |
// ------------------------ D-Part 3: GpsZ ---------------------------------- |
tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
// HCGas -= tmp_int; |
GasReduction += tmp_int; |
GasReduction += tmp_int; |
|
//DebugOut.Analog[16] = GasReduction; |
GasReduction = (long)((long)GasReduction * HoverGas) / 512; |
1559,7 → 1571,7 |
{ // old version |
LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
} |
GasMischanteil = FilterHCGas; |
GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; |
} |
}// EOF height control active |
else // HC not active |
1577,32 → 1589,26 |
|
// 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((FCFlags & FCFLAG_FLY))// && !(FCFlags & FCFLAG_NOTLANDUNG)) |
{ |
if(HoverGasFilter == 0) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
if(abs(VarioMeter) < 100) // only on small vertical speed |
{ |
if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
if(StartTrigger == 1) StartTrigger = 2; |
tmp_long2 = (int32_t)GasMischanteil; // take current thrust |
tmp_long2 *= CosAttitude; // apply attitude projection |
tmp_long2 /= 8192; |
|
// 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 |
HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/8L); |
HoverGasFilter += 8L * tmp_long2; |
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 |
HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/16L); |
HoverGasFilter += 16L * 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 |
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 |
HoverGasFilter -= HoverGasFilter/(HOVER_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 |
HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/2L); |
HoverGasFilter += 2L * tmp_long2; |
} |
else //later |
else //later |
if(abs(VarioMeter) < 100) // only on small vertical speed |
{ |
HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE; |
HoverGasFilter += tmp_long2; |
1620,10 → 1626,15 |
HoverGasMin = 0; |
HoverGasMax = 1023; |
} |
} |
} |
} |
else |
{ |
StartTrigger = 0; |
HoverGasFilter = 0; |
HoverGas = 0; |
} |
}// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
|
DebugOut.Analog[16] = StartTrigger; |
// limit gas to parameter setting |
LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |
if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |