Rev 1328 | Rev 1332 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1328 | Rev 1330 | ||
|---|---|---|---|
| Line 54... | Line 54... | ||
| 54 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
54 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 55... | Line 55... | ||
| 55 | 55 | ||
| 56 | #include "main.h" |
56 | #include "main.h" |
| 57 | #include "eeprom.c" |
57 | #include "eeprom.c" |
| - | 58 | #include "mymath.h" |
|
| Line 58... | Line 59... | ||
| 58 | #include "mymath.h" |
59 | #include "isqrt.h" |
| 59 | 60 | ||
| 60 | unsigned char h,m,s,BaroExpandActive = 0; |
61 | unsigned char h,m,s,BaroExpandActive = 0; |
| 61 | volatile unsigned int I2CTimeout = 100; |
62 | volatile unsigned int I2CTimeout = 100; |
| Line 540... | Line 541... | ||
| 540 | //############################################################################ |
541 | //############################################################################ |
| 541 | // |
542 | // |
| 542 | void MotorRegler(void) |
543 | void MotorRegler(void) |
| 543 | //############################################################################ |
544 | //############################################################################ |
| 544 | { |
545 | { |
| 545 | int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int; |
546 | int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2; |
| 546 | int GierMischanteil,GasMischanteil; |
547 | int GierMischanteil,GasMischanteil; |
| 547 | static long SummeNick=0,SummeRoll=0; |
548 | static long SummeNick=0,SummeRoll=0; |
| 548 | static long sollGier = 0,tmp_long,tmp_long2; |
549 | static long sollGier = 0,tmp_long,tmp_long2; |
| 549 | static long IntegralFehlerNick = 0; |
550 | static long IntegralFehlerNick = 0; |
| 550 | static long IntegralFehlerRoll = 0; |
551 | static long IntegralFehlerRoll = 0; |
| Line 764... | Line 765... | ||
| 764 | if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
765 | if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
| 765 | } |
766 | } |
| 766 | if(StickGas < 0) StickGas = 0; |
767 | if(StickGas < 0) StickGas = 0; |
| Line 767... | Line 768... | ||
| 767 | 768 | ||
| 768 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
769 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
| 769 | if(GyroFaktor < 0) GyroFaktor = 0; |
770 | //if(GyroFaktor < 0) GyroFaktor = 0; |
| Line 770... | Line 771... | ||
| 770 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
771 | //if(IntegralFaktor < 0) IntegralFaktor = 0; |
| 771 | 772 | ||
| 772 | if(abs(StickNick/STICK_GAIN) > MaxStickNick) |
773 | if(abs(StickNick/STICK_GAIN) > MaxStickNick) |
| 773 | { |
774 | { |
| Line 1232... | Line 1233... | ||
| 1232 | static int FilterHCGas = 0; |
1233 | static int FilterHCGas = 0; |
| 1233 | static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023, LastHCGas = 0; |
1234 | static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023, LastHCGas = 0; |
| 1234 | static unsigned long HooverGasFilter = 0; |
1235 | static unsigned long HooverGasFilter = 0; |
| 1235 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1236 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
| 1236 | int CosAttitude; // for projection of hoover gas |
1237 | int CosAttitude; // for projection of hoover gas |
| 1237 | long tmp_long3; |
- | |
| 1238 | // const unsigned char GAS_TAB[31] = {128,128,128,129,129,130,131,132,133,135,136,138,140,142,145,148,151,154,158,162,167,172,178,184,191,199,208,218,229,241,255}; |
- | |
| - | 1238 | ||
| 1239 | // get the current hooverpoint |
1239 | // get the current hooverpoint |
| 1240 | if(LoadHandler == 1) |
1240 | if(LoadHandler == 1) |
| 1241 | { |
1241 | { |
| 1242 | DebugOut.Analog[21] = HooverGas; |
1242 | DebugOut.Analog[21] = HooverGas; |
| 1243 | DebugOut.Analog[18] = VarioMeter; |
1243 | DebugOut.Analog[18] = VarioMeter; |
| Line 1314... | Line 1314... | ||
| 1314 | HoehenReglerAktiv = 1; |
1314 | HoehenReglerAktiv = 1; |
| 1315 | } |
1315 | } |
| Line 1316... | Line 1316... | ||
| 1316 | 1316 | ||
| 1317 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
1317 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
| 1318 | J4Low; |
1318 | J4Low; |
| 1319 | tmp_long = IntegralNick/GIER_GRAD_FAKTOR; // nick angle in deg |
- | |
| 1320 | tmp_long *= tmp_long; |
1319 | tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
| 1321 | tmp_long2 = IntegralRoll/GIER_GRAD_FAKTOR; // roll angle in deg |
- | |
| 1322 | tmp_long2 *= tmp_long2; |
1320 | tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
| 1323 | CosAttitude = (int16_t)c_sqrt(tmp_long + tmp_long2); // phytagoras gives effective attitude angle in deg |
1321 | CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
| 1324 | DebugOut.Analog[16] = CosAttitude; |
1322 | DebugOut.Analog[16] = CosAttitude; |
| 1325 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
1323 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
| 1326 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1324 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
| 1327 | J4High; |
1325 | J4High; |
| Line 1433... | Line 1431... | ||
| 1433 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
1431 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
| 1434 | HCGas -= tmp_int; |
1432 | HCGas -= tmp_int; |
| Line 1435... | Line 1433... | ||
| 1435 | 1433 | ||
| 1436 | // strech control output by inverse attitude projection 1/cos |
1434 | // strech control output by inverse attitude projection 1/cos |
| 1437 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
1435 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
| 1438 | tmp_long3 = (int32_t)HCGas; |
1436 | tmp_long2 = (int32_t)HCGas; |
| 1439 | tmp_long3 *= 8192L; |
1437 | tmp_long2 *= 8192L; |
| 1440 | tmp_long3 /= CosAttitude; |
1438 | tmp_long2 /= CosAttitude; |
| 1441 | HCGas = (int16_t)tmp_long3; |
1439 | HCGas = (int16_t)tmp_long2; |
| 1442 | // update height control gas averaging |
1440 | // update height control gas averaging |
| 1443 | FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
1441 | FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE; |
| 1444 | // limit height control gas pd-control output |
1442 | // limit height control gas pd-control output |
| 1445 | LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN); |
1443 | LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN); |
| Line 1470... | Line 1468... | ||
| 1470 | if((MikroKopterFlags & FLAG_FLY) && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
1468 | if((MikroKopterFlags & FLAG_FLY) && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
| 1471 | { |
1469 | { |
| 1472 | if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
1470 | if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation |
| 1473 | if(abs(VarioMeter) < 100) // only on small vertical speed |
1471 | if(abs(VarioMeter) < 100) // only on small vertical speed |
| 1474 | { |
1472 | { |
| 1475 | tmp_long3 = (int32_t)GasMischanteil; // take current thrust |
1473 | tmp_long2 = (int32_t)GasMischanteil; // take current thrust |
| 1476 | tmp_long3 *= CosAttitude; // apply attitude projection |
1474 | tmp_long2 *= CosAttitude; // apply attitude projection |
| 1477 | tmp_long3 /= 8192; |
1475 | tmp_long2 /= 8192; |
| Line 1478... | Line 1476... | ||
| 1478 | 1476 | ||
| 1479 | // average vertical projected thrust |
1477 | // average vertical projected thrust |
| 1480 | if(modell_fliegt < 2000) // the first 4 seconds |
1478 | if(modell_fliegt < 2000) // the first 4 seconds |
| 1481 | { // reduce the time constant of averaging by factor of 8 to get much faster a stable value |
1479 | { // reduce the time constant of averaging by factor of 8 to get much faster a stable value |
| 1482 | HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/8L); |
1480 | HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/8L); |
| 1483 | HooverGasFilter += 8L * tmp_long3; |
1481 | HooverGasFilter += 8L * tmp_long2; |
| 1484 | } |
1482 | } |
| 1485 | else if(modell_fliegt < 4000) // the first 8 seconds |
1483 | else if(modell_fliegt < 4000) // the first 8 seconds |
| 1486 | { // reduce the time constant of averaging by factor of 4 to get much faster a stable value |
1484 | { // reduce the time constant of averaging by factor of 4 to get much faster a stable value |
| 1487 | HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/4L); |
1485 | HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/4L); |
| 1488 | HooverGasFilter += 4L * tmp_long3; |
1486 | HooverGasFilter += 4L * tmp_long2; |
| 1489 | } |
1487 | } |
| 1490 | else if(modell_fliegt < 8000) // the first 16 seconds |
1488 | else if(modell_fliegt < 8000) // the first 16 seconds |
| 1491 | { // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
1489 | { // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
| 1492 | HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/2L); |
1490 | HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/2L); |
| 1493 | HooverGasFilter += 2L * tmp_long3; |
1491 | HooverGasFilter += 2L * tmp_long2; |
| 1494 | } |
1492 | } |
| 1495 | else //later |
1493 | else //later |
| 1496 | { |
1494 | { |
| 1497 | HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE; |
1495 | HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE; |
| 1498 | HooverGasFilter += tmp_long3; |
1496 | HooverGasFilter += tmp_long2; |
| 1499 | } |
1497 | } |
| 1500 | HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE); |
1498 | HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE); |
| 1501 | if(EE_Parameter.Hoehe_HoverBand) |
1499 | if(EE_Parameter.Hoehe_HoverBand) |
| 1502 | { |
1500 | { |