Rev 1587 | Rev 1590 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1587 | Rev 1588 | ||
|---|---|---|---|
| Line 1422... | Line 1422... | ||
| 1422 | HeightDeviation = (int)(tmp_long); // positive when too high |
1422 | HeightDeviation = (int)(tmp_long); // positive when too high |
| 1423 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 16L; // p-part |
1423 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 16L; // p-part |
| 1424 | LIMIT_MIN_MAX(tmp_long, -255 * STICK_GAIN, 255 * STICK_GAIN); // more than 2 times the full range makes no sense |
1424 | LIMIT_MIN_MAX(tmp_long, -255 * STICK_GAIN, 255 * STICK_GAIN); // more than 2 times the full range makes no sense |
| 1425 | HCGas -= tmp_long; |
1425 | HCGas -= tmp_long; |
| 1426 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1426 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
| 1427 | tmp_long = VarioMeter / 8; |
1427 | tmp_int = VarioMeter / 8; |
| - | 1428 | LIMIT_MIN_MAX(tmp_int, -180, 180); // avoid overflow when squared |
|
| - | 1429 | tmp_int2 = tmp_int; |
|
| 1428 | if(tmp_long > 8) tmp_long = 8; // limit quadratic part on upward movement to avoid to much gas reduction |
1430 | LIMIT_MAX(tmp_int2,8); // limit quadratic part on upward movement to avoid to much gas reduction |
| 1429 | if(tmp_long > 0) tmp_long = VarioMeter + (tmp_long * tmp_long) / 4L; |
1431 | if(tmp_int2 > 0) tmp_int = tmp_int + (tmp_int2 * tmp_int2) / 4L; |
| 1430 | else tmp_long = VarioMeter - (tmp_long * tmp_long) / 4L; |
1432 | else tmp_int = tmp_int - (tmp_int2 * tmp_int2) / 4L; |
| 1431 | tmp_long = (tmp_long * (long)Parameter_Luftdruck_D) / 128L; // scale to d-gain parameter |
1433 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 128L; // scale to d-gain parameter |
| 1432 | LIMIT_MIN_MAX(tmp_long,-32 * STICK_GAIN, 64 * STICK_GAIN); |
1434 | LIMIT_MIN_MAX(tmp_int,-32 * STICK_GAIN, 64 * STICK_GAIN); |
| 1433 | HCGas -= tmp_long; |
1435 | HCGas -= tmp_int; |
| 1434 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1436 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
| 1435 | tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN); |
1437 | tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN); |
| 1436 | LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1438 | LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); |
| 1437 | HCGas -= tmp_long; |
1439 | HCGas -= tmp_long; |
| Line 1438... | Line 1440... | ||
| 1438 | 1440 | ||
| 1439 | if(BaroExpandActive) HCGas = HoverGas; |
1441 | if(BaroExpandActive) HCGas = HoverGas; |
| 1440 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1442 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
| 1441 | tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; |
1443 | tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
| 1442 | LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1444 | LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
| Line 1443... | Line 1445... | ||
| 1443 | HCGas -= tmp_int; |
1445 | HCGas -= tmp_int; |
| 1444 | 1446 | ||
| 1445 | // limit deviation from hoover point within the target region |
1447 | // limit deviation from hoover point within the target region |