Rev 1588 | Rev 1591 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1588 | Rev 1590 | ||
---|---|---|---|
Line 1238... | Line 1238... | ||
1238 | GasMischanteil *= STICK_GAIN; |
1238 | GasMischanteil *= STICK_GAIN; |
Line 1239... | Line 1239... | ||
1239 | 1239 | ||
1240 | // if height control is activated |
1240 | // if height control is activated |
1241 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
1241 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
1242 | { |
1242 | { |
1243 | #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
1243 | #define HOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
1244 | #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging |
1244 | #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging |
1245 | #define OPA_OFFSET_STEP 10 |
1245 | #define OPA_OFFSET_STEP 10 |
1246 | int HCGas, HeightDeviation = 0; |
1246 | int HCGas, HeightDeviation = 0; |
1247 | static int HeightTrimming = 0; // rate for change of height setpoint |
1247 | static int HeightTrimming = 0; // rate for change of height setpoint |
Line 1410... | Line 1410... | ||
1410 | { |
1410 | { |
1411 | SollHoehe = HoehenWert - 400; |
1411 | SollHoehe = HoehenWert - 400; |
1412 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint; |
1412 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint; |
1413 | else StickGasHover = 120; |
1413 | else StickGasHover = 120; |
1414 | } |
1414 | } |
1415 | HCGas = HoverGas; // take hoover gas (neutral point) |
1415 | HCGas = HoverGas; // take hover gas (neutral point) |
1416 | } |
1416 | } |
1417 | if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)) |
1417 | if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)) |
1418 | { |
1418 | { |
- | 1419 | // from this point the Heigth Control Algorithm is identical for both versions |
|
- | 1420 | if(BaroExpandActive) // baro range expanding active |
|
- | 1421 | { |
|
- | 1422 | HCGas = HoverGas; // hover while expanding baro adc range |
|
- | 1423 | HeightDeviation = 0; |
|
- | 1424 | } // EOF // baro range expanding active |
|
- | 1425 | else // valid data from air pressure sensor |
|
- | 1426 | { |
|
1419 | // ------------------------- P-Part ---------------------------- |
1427 | // ------------------------- P-Part ---------------------------- |
1420 | tmp_long = (HoehenWert - SollHoehe); // positive when too high |
1428 | tmp_long = (HoehenWert - SollHoehe); // positive when too high |
1421 | LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
1429 | LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
1422 | HeightDeviation = (int)(tmp_long); // positive when too high |
1430 | HeightDeviation = (int)(tmp_long); // positive when too high |
1423 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 16L; // p-part |
1431 | 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 |
1432 | 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; |
1433 | HCGas -= tmp_long; |
1426 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1434 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1427 | tmp_int = VarioMeter / 8; |
1435 | tmp_int = VarioMeter / 8; |
1428 | LIMIT_MIN_MAX(tmp_int, -180, 180); // avoid overflow when squared |
1436 | LIMIT_MIN_MAX(tmp_int, -181, 181); // avoid overflow when squared (181^2 = 32761) |
1429 | tmp_int2 = tmp_int; |
1437 | tmp_int2 = tmp_int; |
1430 | LIMIT_MAX(tmp_int2,8); // limit quadratic part on upward movement to avoid to much gas reduction |
1438 | LIMIT_MAX(tmp_int2, 8); // limit quadratic part on upward movement to avoid to much gas reduction |
1431 | if(tmp_int2 > 0) tmp_int = tmp_int + (tmp_int2 * tmp_int2) / 4L; |
1439 | if(tmp_int2 > 0) tmp_int = tmp_int + (tmp_int2 * tmp_int2) / 4; |
1432 | else tmp_int = tmp_int - (tmp_int2 * tmp_int2) / 4L; |
1440 | else tmp_int = tmp_int - (tmp_int2 * tmp_int2) / 4; |
1433 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 128L; // scale to d-gain parameter |
1441 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 128L; // scale to d-gain parameter |
1434 | LIMIT_MIN_MAX(tmp_int,-32 * STICK_GAIN, 64 * STICK_GAIN); |
1442 | LIMIT_MIN_MAX(tmp_int,-32 * STICK_GAIN, 64 * STICK_GAIN); |
1435 | HCGas -= tmp_int; |
1443 | HCGas -= tmp_int; |
- | 1444 | } // EOF no baro range expanding |
|
1436 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1445 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1437 | tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN); |
1446 | tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN); |
1438 | LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1447 | LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1439 | HCGas -= tmp_long; |
1448 | HCGas -= tmp_long; |
1440 | - | ||
1441 | if(BaroExpandActive) HCGas = HoverGas; |
- | |
1442 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1449 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1443 | tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
1450 | tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
1444 | LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1451 | LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1445 | HCGas -= tmp_int; |
1452 | HCGas -= tmp_int; |
Line 1483... | Line 1490... | ||
1483 | 1490 | ||
1484 | // Hover gas estimation by averaging gas control output on small z-velocities |
1491 | // Hover gas estimation by averaging gas control output on small z-velocities |
1485 | // this is done only if height contol option is selected in global config and aircraft is flying |
1492 | // this is done only if height contol option is selected in global config and aircraft is flying |
1486 | if((FCFlags & FCFLAG_FLY) && !(FCFlags & FCFLAG_NOTLANDUNG)) |
1493 | if((FCFlags & FCFLAG_FLY) && !(FCFlags & FCFLAG_NOTLANDUNG)) |
1487 | { |
1494 | { |
1488 | if(HoverGasFilter == 0) HoverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
1495 | if(HoverGasFilter == 0) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
1489 | if(abs(VarioMeter) < 100) // only on small vertical speed |
1496 | if(abs(VarioMeter) < 100) // only on small vertical speed |
1490 | { |
1497 | { |
1491 | tmp_long2 = (int32_t)GasMischanteil; // take current thrust |
1498 | tmp_long2 = (int32_t)GasMischanteil; // take current thrust |
1492 | tmp_long2 *= CosAttitude; // apply attitude projection |
1499 | tmp_long2 *= CosAttitude; // apply attitude projection |
Line 1493... | Line 1500... | ||
1493 | tmp_long2 /= 8192; |
1500 | tmp_long2 /= 8192; |
1494 | 1501 | ||
1495 | // average vertical projected thrust |
1502 | // average vertical projected thrust |
1496 | if(modell_fliegt < 2000) // the first 4 seconds |
1503 | if(modell_fliegt < 2000) // the first 4 seconds |
1497 | { // reduce the time constant of averaging by factor of 8 to get much faster a stable value |
1504 | { // reduce the time constant of averaging by factor of 8 to get much faster a stable value |
1498 | HoverGasFilter -= HoverGasFilter/(HOOVER_GAS_AVERAGE/8L); |
1505 | HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/8L); |
1499 | HoverGasFilter += 8L * tmp_long2; |
1506 | HoverGasFilter += 8L * tmp_long2; |
1500 | } |
1507 | } |
1501 | else if(modell_fliegt < 4000) // the first 8 seconds |
1508 | else if(modell_fliegt < 4000) // the first 8 seconds |
1502 | { // reduce the time constant of averaging by factor of 4 to get much faster a stable value |
1509 | { // reduce the time constant of averaging by factor of 4 to get much faster a stable value |
1503 | HoverGasFilter -= HoverGasFilter/(HOOVER_GAS_AVERAGE/4L); |
1510 | HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L); |
1504 | HoverGasFilter += 4L * tmp_long2; |
1511 | HoverGasFilter += 4L * tmp_long2; |
1505 | } |
1512 | } |
1506 | else if(modell_fliegt < 8000) // the first 16 seconds |
1513 | else if(modell_fliegt < 8000) // the first 16 seconds |
1507 | { // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
1514 | { // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
1508 | HoverGasFilter -= HoverGasFilter/(HOOVER_GAS_AVERAGE/2L); |
1515 | HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/2L); |
1509 | HoverGasFilter += 2L * tmp_long2; |
1516 | HoverGasFilter += 2L * tmp_long2; |
1510 | } |
1517 | } |
1511 | else //later |
1518 | else //later |
1512 | { |
1519 | { |
1513 | HoverGasFilter -= HoverGasFilter/HOOVER_GAS_AVERAGE; |
1520 | HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE; |
1514 | HoverGasFilter += tmp_long2; |
1521 | HoverGasFilter += tmp_long2; |
1515 | } |
1522 | } |
1516 | HoverGas = (int16_t)(HoverGasFilter/HOOVER_GAS_AVERAGE); |
1523 | HoverGas = (int16_t)(HoverGasFilter/HOVER_GAS_AVERAGE); |
1517 | if(EE_Parameter.Hoehe_HoverBand) |
1524 | if(EE_Parameter.Hoehe_HoverBand) |
1518 | { |
1525 | { |
1519 | int16_t band; |
1526 | int16_t band; |