Rev 1312 | Rev 1316 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1312 | Rev 1314 | ||
|---|---|---|---|
| Line 1171... | Line 1171... | ||
| 1171 | DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); |
1171 | DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); |
| 1172 | DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
1172 | DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
| 1173 | DebugOut.Analog[2] = Mittelwert_AccNick / 4; |
1173 | DebugOut.Analog[2] = Mittelwert_AccNick / 4; |
| 1174 | DebugOut.Analog[3] = Mittelwert_AccRoll / 4; |
1174 | DebugOut.Analog[3] = Mittelwert_AccRoll / 4; |
| 1175 | DebugOut.Analog[4] = MesswertGier; |
1175 | DebugOut.Analog[4] = MesswertGier; |
| 1176 | DebugOut.Analog[5] = HoehenWert/5; |
1176 | DebugOut.Analog[5] = HoehenWert/5; |
| 1177 | DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
1177 | DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
| 1178 | DebugOut.Analog[8] = KompassValue; |
1178 | DebugOut.Analog[8] = KompassValue; |
| 1179 | DebugOut.Analog[9] = UBat; |
1179 | DebugOut.Analog[9] = UBat; |
| 1180 | DebugOut.Analog[10] = SenderOkay; |
1180 | DebugOut.Analog[10] = SenderOkay; |
| 1181 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
1181 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
| Line 1336... | Line 1336... | ||
| 1336 | // Holger original version |
1336 | // Holger original version |
| 1337 | // start of height control algorithm |
1337 | // start of height control algorithm |
| 1338 | // the height control is only an attenuation of the actual gas stick. |
1338 | // the height control is only an attenuation of the actual gas stick. |
| 1339 | // I.e. it will work only if the gas stick is higher than the hover gas |
1339 | // I.e. it will work only if the gas stick is higher than the hover gas |
| 1340 | // and the hover height will be allways larger than height setpoint. |
1340 | // and the hover height will be allways larger than height setpoint. |
| 1341 | if((EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) || !(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)) // Regler wird über Schalter gesteuert) |
1341 | if((EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) || !(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)) // Regler wird über Schalter gesteuert) |
| 1342 | { // old version |
1342 | { // old version |
| 1343 | HCGas = GasMischanteil; // take current stick gas as neutral point for the height control |
1343 | HCGas = GasMischanteil; // take current stick gas as neutral point for the height control |
| 1344 | HeightTrimming = 0; |
1344 | HeightTrimming = 0; |
| 1345 | } |
1345 | } |
| 1346 | else |
1346 | else |
| 1347 | { |
1347 | { |
| 1348 | // alternative height control |
1348 | // alternative height control |
| 1349 | // PD-Control with respect to hoover point |
1349 | // PD-Control with respect to hoover point |
| 1350 | // the thrust loss out of horizontal attitude is compensated |
1350 | // the thrust loss out of horizontal attitude is compensated |
| 1351 | // the setpoint will be fine adjusted with the gas stick position |
1351 | // the setpoint will be fine adjusted with the gas stick position |
| Line 1390... | Line 1390... | ||
| 1390 | } |
1390 | } |
| 1391 | } //if MikroKopterFlags & MKFLAG_FLY |
1391 | } //if MikroKopterFlags & MKFLAG_FLY |
| 1392 | else SollHoehe = HoehenWert - 200; |
1392 | else SollHoehe = HoehenWert - 200; |
| 1393 | HCGas = HooverGas; // take hoover gas (neutral point) |
1393 | HCGas = HooverGas; // take hoover gas (neutral point) |
| 1394 | } |
1394 | } |
| 1395 | 1395 | ||
| 1396 | if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)) |
1396 | if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)) |
| 1397 | { |
1397 | { |
| 1398 | // ------------------------- P-Part ---------------------------- |
1398 | // ------------------------- P-Part ---------------------------- |
| 1399 | HeightDeviation = (int)(HoehenWert - SollHoehe); // positive when too high |
1399 | HeightDeviation = (int)(HoehenWert - SollHoehe); // positive when too high |
| 1400 | tmp_int = (HeightDeviation * (int)Parameter_Hoehe_P) / 16; // p-part |
1400 | tmp_int = (HeightDeviation * (int)Parameter_Hoehe_P) / 16; // p-part |
| 1401 | HCGas -= tmp_int; |
1401 | HCGas -= tmp_int; |
| Line 1412... | Line 1412... | ||
| 1412 | DebugOut.Analog[23] = -tmp_int; |
1412 | DebugOut.Analog[23] = -tmp_int; |
| 1413 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
1413 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
| 1414 | HCGas -= tmp_int; |
1414 | HCGas -= tmp_int; |
| Line 1415... | Line 1415... | ||
| 1415 | 1415 | ||
| 1416 | // limit deviation from hoover point within the target region |
1416 | // limit deviation from hoover point within the target region |
| 1417 | if( (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
1417 | if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
| 1418 | { |
1418 | { |
| 1419 | LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
1419 | LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
| Line 1420... | Line 1420... | ||
| 1420 | } |
1420 | } |
| Line 1433... | Line 1433... | ||
| 1433 | // update height control gas averaging |
1433 | // update height control gas averaging |
| 1434 | FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
1434 | FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
| 1435 | // limit height control gas pd-control output |
1435 | // limit height control gas pd-control output |
| 1436 | LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN); |
1436 | LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN); |
| 1437 | // set GasMischanteil to HeightControlGasFilter |
1437 | // set GasMischanteil to HeightControlGasFilter |
| 1438 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) |
1438 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) |
| 1439 | { // old version |
1439 | { // old version |
| 1440 | if(FilterHCGas > GasMischanteil) FilterHCGas = GasMischanteil; // nicht mehr als Gas |
1440 | if(FilterHCGas > GasMischanteil) FilterHCGas = GasMischanteil; // nicht mehr als Gas |
| 1441 | } |
1441 | } |
| 1442 | GasMischanteil = FilterHCGas; |
1442 | GasMischanteil = FilterHCGas; |
| 1443 | } |
1443 | } |
| 1444 | }// EOF height control active |
1444 | }// EOF height control active |
| Line 1445... | Line 1445... | ||
| 1445 | 1445 | ||
| 1446 | // Hoover gas estimation by averaging gas control output on small z-velocities |
1446 | // Hoover gas estimation by averaging gas control output on small z-velocities |
| 1447 | // this is done only if height contol option is selected in global config and aircraft is flying |
1447 | // this is done only if height contol option is selected in global config and aircraft is flying |