98,7 → 98,7 |
char MotorenEin = 0; |
long HoehenWert = 0; |
long SollHoehe = 0; |
int LageKorrekturRoll = 0,LageKorrekturNick = 0; |
int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0; |
//float Ki = FAKTOR_I; |
int Ki = 10300 / 33; |
unsigned char Looping_Nick = 0,Looping_Roll = 0; |
1295,7 → 1295,7 |
int HCGas, HeightDeviation = 0; |
static int HeightTrimming = 0; // rate for change of height setpoint |
static int FilterHCGas = 0; |
static int StickGasHover = 120, HoverGas = 0, HoverGasMin = 0, HoverGasMax = 1023; |
static int StickGasHover = 120, 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 |
1504,11 → 1504,25 |
HCGas -= tmp_int; |
|
// limit deviation from hoover point within the target region |
if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HoverGas > 0)) // height setpoint is not changed and hoover gas not zero |
if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
{ |
LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point |
unsigned int tmp,min,max; |
if(abs(HeightDeviation) < 60) |
{ |
LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point |
} |
else |
{ |
tmp = (abs(HeightDeviation) - 60)/32; |
if(tmp > 15) tmp = 15; |
max = (HoverGasMax * (tmp + 16)) / 16; |
min = (HoverGasMin * (16 - tmp)) / 16; |
LIMIT_MIN_MAX(HCGas, min, max); // limit gas around the hoover point |
DebugOut.Analog[16] = min; |
DebugOut.Analog[17] = max; |
DebugOut.Analog[18] = SollHoehe; |
} |
} |
|
// strech control output by inverse attitude projection 1/cos |
// + 1/cos(angle) ++++++++++++++++++++++++++ |
tmp_long2 = (int32_t)HCGas; |
1634,10 → 1648,10 |
if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
|
if(Parameter_UserParam5 > 50) |
//if(Parameter_UserParam5 > 50) |
pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 + SummeNick / Ki; // PI-Regler für Nick |
else |
pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick |
//else |
// pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick |
|
// Motor Vorn |
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |