| 1481,7 → 1481,7 |
|---|
| 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) / 16L; // p-part |
| LIMIT_MIN_MAX(tmp_long, -255 * STICK_GAIN, 255 * STICK_GAIN); // more than 2 times the full range makes no sense |
| LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 255 * STICK_GAIN); // more than the full range makes no sense |
| HCGas -= tmp_long; |
| // ------------------------- D-Part 1: Vario Meter ---------------------------- |
| tmp_int = VarioMeter / 8; |
| 1502,11 → 1502,10 |
|---|
| 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; |
| |
| // limit deviation from hoover point within the target region |
| if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
| { |
| unsigned int tmp,min,max; |
| signed int tmp,min,max; |
| if(abs(HeightDeviation) < 60) |
| { |
| LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point |
| 1515,12 → 1514,16 |
|---|
| { |
| 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; |
| if(HeightDeviation > 0) |
| { |
| min = (HoverGasMin * (16 - tmp)) / 16; |
| LIMIT_MIN_MAX(HCGas, min, HoverGasMax); // limit gas around the hoover point |
| } |
| else |
| { |
| max = (HoverGasMax * (tmp + 16)) / 16; |
| LIMIT_MIN_MAX(HCGas, HoverGasMin, max); // limit gas around the hoover point |
| } |
| } |
| } |
| // strech control output by inverse attitude projection 1/cos |
| 1647,12 → 1650,7 |
|---|
| else SummeNick += DiffNick; // I-Anteil bei HH |
| if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
| if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
| |
| //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 |
| |
| // Motor Vorn |
| tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
| if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
| 1666,12 → 1664,7 |
|---|
| else SummeRoll += DiffRoll; // I-Anteil bei HH |
| if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
| if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
| |
| //if(Parameter_UserParam5 > 50) |
| pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8 + SummeRoll / Ki; // PI-Regler für Roll |
| //else |
| // pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
| |
| tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
| if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
| if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
| 1704,15 → 1697,7 |
|---|
| if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
| else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
| } |
| |
| /* |
| if(Parameter_UserParam5 > 50) |
| { |
| if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
| // else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
| else tmp_int -= Parameter_UserParam6 * (tmp_motorwert[i] - tmp_int); // MotorSmoothing |
| } |
| */ |
| |
| LIMIT_MIN_MAX(tmp_int,MIN_GAS * 4,MAX_GAS * 4); |
| Motor[i].SetPoint = tmp_int / 4; |
| Motor[i].SetPointLowerBits = (tmp_int % 4)<<1; // (3 bits total) |