Rev 1941 | Rev 1943 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1941 | Rev 1942 | ||
---|---|---|---|
Line 99... | Line 99... | ||
99 | volatile unsigned char SenderOkay = 0; |
99 | volatile unsigned char SenderOkay = 0; |
100 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
100 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
101 | char MotorenEin = 0,StartTrigger = 0; |
101 | char MotorenEin = 0,StartTrigger = 0; |
102 | long HoehenWert = 0; |
102 | long HoehenWert = 0; |
103 | long SollHoehe = 0; |
103 | long SollHoehe = 0; |
- | 104 | signed int AltitudeSetpointTrimming = 0; |
|
104 | long FromNC_AltitudeSetpoint = 0; |
105 | long FromNC_AltitudeSetpoint = 0; |
105 | unsigned char FromNC_AltitudeSpeed = 0; |
106 | unsigned char FromNC_AltitudeSpeed = 0; |
106 | unsigned char carefree_old = 50; // to make the Beep when switching |
107 | unsigned char carefree_old = 50; // to make the Beep when switching |
107 | - | ||
- | 108 | signed char WaypointTrimming = 0; |
|
108 | int CompassGierSetpoint = 0; |
109 | int CompassGierSetpoint = 0; |
109 | int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0; |
110 | int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0; |
110 | //float Ki = FAKTOR_I; |
111 | //float Ki = FAKTOR_I; |
111 | int Ki = 10300 / 33; |
112 | int Ki = 10300 / 33; |
112 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
113 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
Line 205... | Line 206... | ||
205 | DebugOut.Analog[27] = KompassSollWert; |
206 | DebugOut.Analog[27] = KompassSollWert; |
206 | DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
207 | DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
207 | DebugOut.Analog[30] = GPS_Nick; |
208 | DebugOut.Analog[30] = GPS_Nick; |
208 | DebugOut.Analog[31] = GPS_Roll; |
209 | DebugOut.Analog[31] = GPS_Roll; |
209 | if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe; |
210 | if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe; |
- | 211 | ||
- | 212 | DebugOut.Analog[16] = AltitudeSetpointTrimming; |
|
- | 213 | DebugOut.Analog[17] = ElectricAirPacket.m_sec; |
|
210 | } |
214 | } |
Line 211... | Line 215... | ||
211 | 215 | ||
Line 1349... | Line 1353... | ||
1349 | int HCGas, HeightDeviation = 0,GasReduction = 0; |
1353 | int HCGas, HeightDeviation = 0,GasReduction = 0; |
1350 | static int HeightTrimming = 0; // rate for change of height setpoint |
1354 | static int HeightTrimming = 0; // rate for change of height setpoint |
1351 | static int FilterHCGas = 0; |
1355 | static int FilterHCGas = 0; |
1352 | static unsigned long HoverGasFilter = 0; |
1356 | static unsigned long HoverGasFilter = 0; |
1353 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1357 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1354 | static signed char WaypointTrimming = 0; |
- | |
1355 | int CosAttitude; // for projection of hoover gas |
1358 | int CosAttitude; // for projection of hoover gas |
Line 1356... | Line 1359... | ||
1356 | 1359 | ||
1357 | // get the current hooverpoint |
1360 | // get the current hooverpoint |
Line 1434... | Line 1437... | ||
1434 | tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
1437 | tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
1435 | CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
1438 | CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
1436 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
1439 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
1437 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1440 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1438 | VarioCharacter = ' '; |
1441 | VarioCharacter = ' '; |
- | 1442 | AltitudeSetpointTrimming = 0; |
|
1439 | if(HoehenReglerAktiv && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
1443 | if(HoehenReglerAktiv && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
1440 | { |
1444 | { |
1441 | #define HEIGHT_CONTROL_STICKTHRESHOLD 15 |
1445 | #define HEIGHT_CONTROL_STICKTHRESHOLD 15 |
1442 | // Holger original version |
1446 | // Holger original version |
1443 | // start of height control algorithm |
1447 | // start of height control algorithm |
Line 1447... | Line 1451... | ||
1447 | FC_StatusFlags2 |= FC_STATUS2_ALTITUDE_CONTROL; |
1451 | FC_StatusFlags2 |= FC_STATUS2_ALTITUDE_CONTROL; |
1448 | if((Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) || !(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER)) // Regler wird über Schalter gesteuert) |
1452 | if((Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT) || !(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER)) // Regler wird über Schalter gesteuert) |
1449 | { // old version |
1453 | { // old version |
1450 | HCGas = GasMischanteil; // take current stick gas as neutral point for the height control |
1454 | HCGas = GasMischanteil; // take current stick gas as neutral point for the height control |
1451 | HeightTrimming = 0; |
1455 | HeightTrimming = 0; |
- | 1456 | AltitudeSetpointTrimming = 0; |
|
1452 | // set both flags to indicate no vario mode |
1457 | // set both flags to indicate no vario mode |
1453 | FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
1458 | FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
1454 | } |
1459 | } |
1455 | else |
1460 | else |
1456 | { |
1461 | { |
1457 | // alternative height control |
1462 | // alternative height control |
1458 | // PD-Control with respect to hoover point |
1463 | // PD-Control with respect to hoover point |
1459 | // the thrust loss out of horizontal attitude is compensated |
1464 | // the thrust loss out of horizontal attitude is compensated |
1460 | // the setpoint will be fine adjusted with the gas stick position |
1465 | // the setpoint will be fine adjusted with the gas stick position |
1461 | if(FC_StatusFlags & FC_STATUS_FLY) // trim setpoint only when flying |
1466 | if(1 || FC_StatusFlags & FC_STATUS_FLY) // trim setpoint only when flying |
1462 | { // gas stick is above hoover point |
1467 | { // gas stick is above hoover point |
1463 | if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
1468 | if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
1464 | { |
1469 | { |
1465 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) |
1470 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) |
1466 | { |
1471 | { |
1467 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN; |
1472 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN; |
1468 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1473 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1469 | } |
1474 | } |
1470 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
1475 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
- | 1476 | AltitudeSetpointTrimming = abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
|
1471 | HeightTrimming += abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
1477 | // HeightTrimming += abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1472 | VarioCharacter = '+'; |
1478 | VarioCharacter = '+'; |
1473 | WaypointTrimming = 0; |
1479 | WaypointTrimming = 0; |
1474 | } // gas stick is below hoover point |
1480 | } // gas stick is below hoover point |
1475 | else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
1481 | else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
1476 | { |
1482 | { |
Line 1478... | Line 1484... | ||
1478 | { |
1484 | { |
1479 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
1485 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
1480 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1486 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1481 | } |
1487 | } |
1482 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
1488 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
- | 1489 | AltitudeSetpointTrimming = -abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
|
1483 | HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1490 | // HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1484 | VarioCharacter = '-'; |
1491 | VarioCharacter = '-'; |
1485 | WaypointTrimming = 0; |
1492 | WaypointTrimming = 0; |
1486 | } |
1493 | } |
1487 | else // Gas Stick in Hover Range |
1494 | else // Gas Stick in Hover Range |
1488 | { |
1495 | { |
1489 | VarioCharacter = '='; |
1496 | VarioCharacter = '='; |
1490 | if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint > SollHoehe) // von NC gesteuert -> Steigen |
1497 | if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint > SollHoehe) // von NC gesteuert -> Steigen |
1491 | { |
1498 | { |
1492 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
1499 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
- | 1500 | AltitudeSetpointTrimming = FromNC_AltitudeSpeed; |
|
1493 | HeightTrimming += FromNC_AltitudeSpeed; |
1501 | //HeightTrimming += FromNC_AltitudeSpeed; |
1494 | WaypointTrimming = 10; |
1502 | WaypointTrimming = 10; |
1495 | VarioCharacter = '^'; |
1503 | VarioCharacter = '^'; |
1496 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) // changed from sinking to rising |
1504 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) // changed from sinking to rising |
1497 | { |
1505 | { |
1498 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN; |
1506 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN; |
Line 1501... | Line 1509... | ||
1501 | } |
1509 | } |
1502 | else |
1510 | else |
1503 | if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint < SollHoehe) // von NC gesteuert -> sinken |
1511 | if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint < SollHoehe) // von NC gesteuert -> sinken |
1504 | { |
1512 | { |
1505 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
1513 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
- | 1514 | AltitudeSetpointTrimming = -FromNC_AltitudeSpeed; |
|
1506 | HeightTrimming -= FromNC_AltitudeSpeed; |
1515 | //HeightTrimming -= FromNC_AltitudeSpeed; |
1507 | WaypointTrimming = -10; |
1516 | WaypointTrimming = -10; |
1508 | VarioCharacter = 'v'; |
1517 | VarioCharacter = 'v'; |
1509 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) // changed from rising to sinking |
1518 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) // changed from rising to sinking |
1510 | { |
1519 | { |
1511 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
1520 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
Line 1525... | Line 1534... | ||
1525 | StartTrigger = 1; |
1534 | StartTrigger = 1; |
1526 | } |
1535 | } |
1527 | } |
1536 | } |
1528 | } |
1537 | } |
1529 | // Trim height set point |
1538 | // Trim height set point |
- | 1539 | HeightTrimming += AltitudeSetpointTrimming; |
|
1530 | if(abs(HeightTrimming) > 512) |
1540 | if(abs(HeightTrimming) > 500) // bei Waypoint-Flug ist das ca. die 500Hz |
1531 | { |
1541 | { |
1532 | if(WaypointTrimming) |
1542 | if(WaypointTrimming) |
1533 | { |
1543 | { |
1534 | if(abs(FromNC_AltitudeSetpoint - SollHoehe) < 10) SollHoehe = FromNC_AltitudeSetpoint; |
1544 | if(abs(FromNC_AltitudeSetpoint - SollHoehe) < 10) SollHoehe = FromNC_AltitudeSetpoint; |
1535 | else SollHoehe += WaypointTrimming; |
1545 | else SollHoehe += WaypointTrimming; |
1536 | } |
1546 | } |
1537 | else |
1547 | else |
- | 1548 | ||
1538 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
1549 | if(HeightTrimming > 0) SollHoehe += EE_Parameter.Hoehe_Verstaerkung / 3; |
- | 1550 | else SollHoehe -= EE_Parameter.Hoehe_Verstaerkung / 3; |
|
1539 | HeightTrimming = 0; |
1551 | HeightTrimming = 0; |
1540 | LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied |
1552 | LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied |
1541 | if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100; |
1553 | if(Parameter_ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100; |
1542 | //update hoover gas stick value when setpoint is shifted |
1554 | //update hoover gas stick value when setpoint is shifted |
1543 | if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0) |
1555 | if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0) |
Line 1599... | Line 1611... | ||
1599 | GasReduction += tmp_int; |
1611 | GasReduction += tmp_int; |
1600 | GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value |
1612 | GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value |
1601 | // ------------------------ ---------------------------------- |
1613 | // ------------------------ ---------------------------------- |
1602 | HCGas -= GasReduction; |
1614 | HCGas -= GasReduction; |
1603 | // limit deviation from hoover point within the target region |
1615 | // limit deviation from hoover point within the target region |
1604 | if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
1616 | if(!AltitudeSetpointTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
1605 | { |
1617 | { |
1606 | unsigned int tmp; |
1618 | unsigned int tmp; |
1607 | tmp = abs(HeightDeviation); |
1619 | tmp = abs(HeightDeviation); |
1608 | if(tmp <= 60) |
1620 | if(tmp <= 60) |
1609 | { |
1621 | { |