| 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; |