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