Rev 1693 | Rev 1697 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1693 | Rev 1695 | ||
---|---|---|---|
Line 181... | Line 181... | ||
181 | DebugOut.Analog[10] = SenderOkay; |
181 | DebugOut.Analog[10] = SenderOkay; |
182 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
182 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
183 | DebugOut.Analog[20] = ServoNickValue; |
183 | DebugOut.Analog[20] = ServoNickValue; |
184 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
184 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
185 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
185 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
- | 186 | ||
- | 187 | DebugOut.Analog[24] = AdWertAccHoch; |
|
186 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
188 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
187 | // DebugOut.Analog[24] = MesswertNick/2; |
189 | // DebugOut.Analog[24] = MesswertNick/2; |
188 | // DebugOut.Analog[25] = MesswertRoll/2; |
190 | // DebugOut.Analog[25] = MesswertRoll/2; |
189 | // DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
191 | // DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
190 | // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
192 | // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
Line 1120... | Line 1122... | ||
1120 | if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift; |
1122 | if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift; |
1121 | if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
1123 | if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
1122 | if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
1124 | if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
Line 1123... | Line 1125... | ||
1123 | 1125 | ||
1124 | // Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
1126 | // Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
1125 | cnt = 1;// + labs(IntegralFehlerNick) / 4096; |
1127 | cnt = 1;// + labs(IntegralFehlerRoll) / 4096; |
1126 | if(labs(IntegralFehlerRoll) > FEHLER_LIMIT1) cnt = 4; |
- | |
1127 | ausgleichRoll = 0; |
1128 | if(labs(IntegralFehlerRoll) > FEHLER_LIMIT1) cnt = 4; |
1128 | if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8)) |
1129 | if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8)) |
1129 | { |
1130 | { |
1130 | if(IntegralFehlerRoll > FEHLER_LIMIT2) |
1131 | if(IntegralFehlerRoll > FEHLER_LIMIT2) |
1131 | { |
1132 | { |
Line 1290... | Line 1291... | ||
1290 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
1291 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
1291 | #define OPA_OFFSET_STEP 15 |
1292 | #define OPA_OFFSET_STEP 15 |
1292 | #else |
1293 | #else |
1293 | #define OPA_OFFSET_STEP 10 |
1294 | #define OPA_OFFSET_STEP 10 |
1294 | #endif |
1295 | #endif |
1295 | int HCGas, HeightDeviation = 0; |
1296 | int HCGas, HeightDeviation = 0,GasReduction; |
1296 | static int HeightTrimming = 0; // rate for change of height setpoint |
1297 | static int HeightTrimming = 0; // rate for change of height setpoint |
1297 | static int FilterHCGas = 0; |
1298 | static int FilterHCGas = 0; |
1298 | static int StickGasHover = 120, HoverGasMin = 0, HoverGasMax = 1023; |
1299 | static int StickGasHover = 120, HoverGasMin = 0, HoverGasMax = 1023; |
1299 | static unsigned long HoverGasFilter = 0; |
1300 | static unsigned long HoverGasFilter = 0; |
1300 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1301 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
Line 1478... | Line 1479... | ||
1478 | { |
1479 | { |
1479 | // ------------------------- P-Part ---------------------------- |
1480 | // ------------------------- P-Part ---------------------------- |
1480 | tmp_long = (HoehenWert - SollHoehe); // positive when too high |
1481 | tmp_long = (HoehenWert - SollHoehe); // positive when too high |
1481 | LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
1482 | LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
1482 | HeightDeviation = (int)(tmp_long); // positive when too high |
1483 | HeightDeviation = (int)(tmp_long); // positive when too high |
1483 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 16L; // p-part |
1484 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part |
1484 | LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 255 * STICK_GAIN); // more than the full range makes no sense |
1485 | LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 255 * STICK_GAIN); // more than the full range makes no sense |
1485 | HCGas -= tmp_long; |
1486 | // HCGas -= tmp_long; |
- | 1487 | DebugOut.Analog[25] = tmp_long; |
|
- | 1488 | GasReduction = tmp_long; |
|
1486 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1489 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1487 | tmp_int = VarioMeter / 8; |
1490 | tmp_int = VarioMeter / 8; |
1488 | LIMIT_MIN_MAX(tmp_int, -181, 181); // avoid overflow when squared (181^2 = 32761) |
1491 | LIMIT_MIN_MAX(tmp_int, -127, 128); |
- | 1492 | DebugOut.Analog[18] = tmp_int; |
|
1489 | tmp_int2 = tmp_int; |
1493 | // tmp_int2 = tmp_int/4; |
1490 | LIMIT_MAX(tmp_int2, 8); // limit quadratic part on upward movement to avoid to much gas reduction |
1494 | // LIMIT_MAX(tmp_int2, 64); |
1491 | if(tmp_int2 > 0) tmp_int = tmp_int + (tmp_int2 * tmp_int2) / 4; |
1495 | // if(tmp_int > 0) tmp_int = tmp_int + (tmp_int2 * tmp_int2); |
1492 | else tmp_int = tmp_int - (tmp_int2 * tmp_int2) / 4; |
1496 | // else tmp_int = tmp_int - (tmp_int2 * tmp_int2); |
- | 1497 | ||
1493 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 128L; // scale to d-gain parameter |
1498 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 64L; // scale to d-gain parameter |
1494 | LIMIT_MIN_MAX(tmp_int,-32 * STICK_GAIN, 64 * STICK_GAIN); |
1499 | LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
1495 | HCGas -= tmp_int; |
1500 | // HCGas -= tmp_int; |
- | 1501 | DebugOut.Analog[19] = tmp_int; |
|
- | 1502 | GasReduction += tmp_int; |
|
- | 1503 | ||
1496 | } // EOF no baro range expanding |
1504 | } // EOF no baro range expanding |
1497 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1505 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1498 | tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN); |
1506 | tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN); |
1499 | LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1507 | LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1500 | HCGas -= tmp_long; |
1508 | // HCGas -= tmp_long; |
- | 1509 | GasReduction += tmp_long; |
|
1501 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1510 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1502 | tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
1511 | tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
1503 | LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1512 | LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1504 | HCGas -= tmp_int; |
1513 | // HCGas -= tmp_int; |
- | 1514 | GasReduction += tmp_int; |
|
- | 1515 | ||
- | 1516 | //DebugOut.Analog[16] = GasReduction; |
|
- | 1517 | GasReduction = (long)((long)GasReduction * HoverGas) / 512; |
|
- | 1518 | //GasReduction = (long)((long)GasReduction * Poti3) / 64; |
|
- | 1519 | DebugOut.Analog[17] = GasReduction; |
|
- | 1520 | ||
- | 1521 | HCGas -= GasReduction; |
|
- | 1522 | ||
1505 | // limit deviation from hoover point within the target region |
1523 | // limit deviation from hoover point within the target region |
1506 | if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
1524 | if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
1507 | { |
1525 | { |
1508 | signed int tmp,min,max; |
1526 | signed int tmp,min,max; |
1509 | if(abs(HeightDeviation) < 60) |
1527 | if(abs(HeightDeviation) < 60) |