Rev 1562 | Rev 1587 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1562 | Rev 1586 | ||
|---|---|---|---|
| Line 1250... | Line 1250... | ||
| 1250 | static unsigned long HooverGasFilter = 0; |
1250 | static unsigned long HooverGasFilter = 0; |
| 1251 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1251 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
| 1252 | int CosAttitude; // for projection of hoover gas |
1252 | int CosAttitude; // for projection of hoover gas |
| Line 1253... | Line 1253... | ||
| 1253 | 1253 | ||
| 1254 | // get the current hooverpoint |
- | |
| 1255 | // if(LoadHandler == 1) |
- | |
| 1256 | { |
1254 | // get the current hooverpoint |
| 1257 | DebugOut.Analog[21] = HooverGas; |
1255 | DebugOut.Analog[21] = HooverGas; |
| Line 1258... | Line 1256... | ||
| 1258 | DebugOut.Analog[18] = VarioMeter; |
1256 | DebugOut.Analog[18] = VarioMeter; |
| 1259 | 1257 | ||
| Line 1393... | Line 1391... | ||
| 1393 | // Trim height set point |
1391 | // Trim height set point |
| 1394 | if(abs(HeightTrimming) > 512) |
1392 | if(abs(HeightTrimming) > 512) |
| 1395 | { |
1393 | { |
| 1396 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
1394 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
| 1397 | HeightTrimming = 0; |
1395 | HeightTrimming = 0; |
| - | 1396 | LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied |
|
| 1398 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
1397 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
| 1399 | //update hoover gas stick value when setpoint is shifted |
1398 | //update hoover gas stick value when setpoint is shifted |
| 1400 | if(!EE_Parameter.Hoehe_StickNeutralPoint) |
1399 | if(!EE_Parameter.Hoehe_StickNeutralPoint) |
| 1401 | { |
1400 | { |
| 1402 | StickGasHoover = HooverGas/STICK_GAIN; //rescale back to stick value |
1401 | StickGasHoover = HooverGas/STICK_GAIN; //rescale back to stick value |
| Line 1413... | Line 1412... | ||
| 1413 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
1412 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
| 1414 | else StickGasHoover = 120; |
1413 | else StickGasHoover = 120; |
| 1415 | } |
1414 | } |
| 1416 | HCGas = HooverGas; // take hoover gas (neutral point) |
1415 | HCGas = HooverGas; // take hoover gas (neutral point) |
| 1417 | } |
1416 | } |
| 1418 | - | ||
| 1419 | if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)) |
1417 | if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)) |
| 1420 | { |
1418 | { |
| 1421 | // ------------------------- P-Part ---------------------------- |
1419 | // ------------------------- P-Part ---------------------------- |
| 1422 | tmp_long = (HoehenWert - SollHoehe); // positive when too high |
1420 | tmp_long = (HoehenWert - SollHoehe); // positive when too high |
| 1423 | LIMIT_MIN_MAX(tmp_long, -32000, 32000); // avoid overflov when casting to int |
1421 | LIMIT_MIN_MAX(tmp_long, -2000, 2000); // avoid overflov when casting to int |
| 1424 | HeightDeviation = (int)(tmp_long); // positive when too high |
1422 | HeightDeviation = (int)(tmp_long); // positive when too high |
| 1425 | tmp_int = (HeightDeviation * (int)Parameter_Hoehe_P) / 16; // p-part |
1423 | tmp_int = (HeightDeviation * (long)Parameter_Hoehe_P) / 16; // p-part |
| 1426 | HCGas -= tmp_int; |
1424 | HCGas -= tmp_int; |
| 1427 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1425 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
| 1428 | tmp_int = VarioMeter / 8; |
1426 | tmp_int = VarioMeter / 8; |
| 1429 | if(tmp_int > 8) tmp_int = 8; // limit quadratic part on upward movement to avoid to much gas reduction |
1427 | if(tmp_int > 8) tmp_int = 8; // limit quadratic part on upward movement to avoid to much gas reduction |
| 1430 | if(tmp_int > 0) tmp_int = VarioMeter + (tmp_int * tmp_int) / 4; |
1428 | if(tmp_int > 0) tmp_int = VarioMeter + (tmp_int * tmp_int) / 4; |
| Line 1434... | Line 1432... | ||
| 1434 | HCGas -= tmp_int; |
1432 | HCGas -= tmp_int; |
| 1435 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1433 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
| 1436 | tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
1434 | tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
| 1437 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
1435 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
| 1438 | HCGas -= tmp_int; |
1436 | HCGas -= tmp_int; |
| 1439 | - | ||
| 1440 | // limit deviation from hoover point within the target region |
1437 | // limit deviation from hoover point within the target region |
| 1441 | if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
1438 | if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
| 1442 | { |
1439 | { |
| 1443 | LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
1440 | LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
| 1444 | } |
1441 | } |
| Line 1446... | Line 1443... | ||
| 1446 | if(BaroExpandActive) HCGas = HooverGas; |
1443 | if(BaroExpandActive) HCGas = HooverGas; |
| 1447 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1444 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
| 1448 | tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; |
1445 | tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; |
| 1449 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
1446 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
| 1450 | HCGas -= tmp_int; |
1447 | HCGas -= tmp_int; |
| 1451 | - | ||
| 1452 | // strech control output by inverse attitude projection 1/cos |
1448 | // strech control output by inverse attitude projection 1/cos |
| 1453 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
1449 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
| 1454 | tmp_long2 = (int32_t)HCGas; |
1450 | tmp_long2 = (int32_t)HCGas; |
| 1455 | tmp_long2 *= 8192L; |
1451 | tmp_long2 *= 8192L; |
| 1456 | tmp_long2 /= CosAttitude; |
1452 | tmp_long2 /= CosAttitude; |
| Line 1526... | Line 1522... | ||
| 1526 | HooverGasMin = 0; |
1522 | HooverGasMin = 0; |
| 1527 | HooverGasMax = 1023; |
1523 | HooverGasMax = 1023; |
| 1528 | } |
1524 | } |
| 1529 | } |
1525 | } |
| 1530 | } |
1526 | } |
| 1531 | } |
- | |
| 1532 | //DebugOut.Analog[26] = HooverGasMax; |
- | |
| 1533 | }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
1527 | }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
| Line 1534... | Line 1528... | ||
| 1534 | 1528 | ||
| 1535 | // limit gas to parameter setting |
1529 | // limit gas to parameter setting |
| 1536 | LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |
1530 | LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |