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