| 1221,23 → 1221,21 |
|---|
| GasMischanteil *= STICK_GAIN; |
| |
| // if height control is activated |
| J4High; |
| if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
| { |
| // #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
| #define HOOVER_GAS_AVERAGE 1024L // 1024 * 10ms = 10s averaging |
| #define HC_GAS_AVERAGE 2 // 2 * 10ms= 20ms averaging |
| #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; |
| static int HeightTrimming = 0; // rate for change of height setpoint |
| static int FilterHCGas = 0; |
| static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023, LastHCGas = 0; |
| static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
| static unsigned long HooverGasFilter = 0; |
| static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
| int CosAttitude; // for projection of hoover gas |
| |
| // get the current hooverpoint |
| if(LoadHandler == 1) |
| // if(LoadHandler == 1) |
| { |
| DebugOut.Analog[21] = HooverGas; |
| DebugOut.Analog[18] = VarioMeter; |
| 1253,7 → 1251,7 |
|---|
| ExpandBaro -= 1; |
| OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
| beeptime = 300; |
| BaroExpandActive = 50; |
| BaroExpandActive = 250; |
| } |
| else |
| { |
| 1269,7 → 1267,7 |
|---|
| ExpandBaro += 1; |
| OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
| beeptime = 300; |
| BaroExpandActive = 50; |
| BaroExpandActive = 250; |
| } |
| else |
| { |
| 1315,14 → 1313,11 |
|---|
| } |
| |
| // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
| J4Low; |
| tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
| tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
| CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
| DebugOut.Analog[16] = CosAttitude; |
| LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
| CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
| J4High; |
| if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
| { |
| #define HEIGHT_TRIM_UP 0x01 |
| 1381,15 → 1376,19 |
|---|
| // Trim height set point |
| if(abs(HeightTrimming) > 256) |
| { |
| SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(512 / 2); // move setpoint |
| SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
| HeightTrimming = 0; |
| if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
| //update hoover gas stick value when setpoint is shifted |
| // StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
| // StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
| // if(StickGasHoover < 70) StickGasHoover = 70; |
| // else if(StickGasHoover > 150) StickGasHoover = 150; |
| 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; |
| } |
| } |
| |
| } //if MikroKopterFlags & MKFLAG_FLY |
| else |
| { |
| 1510,15 → 1509,9 |
|---|
| } |
| } |
| } |
| LastHCGas = GasMischanteil; |
| } |
| else |
| { |
| GasMischanteil = LastHCGas; |
| } |
| //DebugOut.Analog[26] = HooverGasMax; |
| }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
| J4Low; |
| |
| // limit gas to parameter setting |
| LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |