Rev 1175 | Rev 1210 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1175 | Rev 1209 | ||
---|---|---|---|
Line 144... | Line 144... | ||
144 | unsigned char Parameter_ExternalControl; |
144 | unsigned char Parameter_ExternalControl; |
145 | struct mk_param_struct EE_Parameter; |
145 | struct mk_param_struct EE_Parameter; |
146 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
146 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
147 | int MaxStickNick = 0,MaxStickRoll = 0; |
147 | int MaxStickNick = 0,MaxStickRoll = 0; |
148 | unsigned int modell_fliegt = 0; |
148 | unsigned int modell_fliegt = 0; |
149 | unsigned char MikroKopterFlags = 0; |
149 | volatile unsigned char MikroKopterFlags = 0; |
150 | long GIER_GRAD_FAKTOR = 1291; |
150 | long GIER_GRAD_FAKTOR = 1291; |
151 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
151 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
- | 152 | unsigned char RequiredMotors = 4; |
|
Line -... | Line 153... | ||
- | 153 | ||
- | 154 | /* |
|
- | 155 | signed char Mixer.Motor[MAX_MOTORS][4] = { |
|
- | 156 | {64, +64, 0, +64},//1 |
|
- | 157 | {64, +64, -64, -64},//2 |
|
- | 158 | {64, 0, -64, +64},//3 |
|
- | 159 | {64, -64, -64, -64},//4 |
|
- | 160 | {64, -64, 0, +64},//5 |
|
- | 161 | {64, -64, +64, -64},//6 |
|
- | 162 | {64, 0, +64, +64},//7 |
|
- | 163 | {64, +64, +64, -64},//8 |
|
- | 164 | { 0, 0, 0, 0},//9 |
|
- | 165 | { 0, 0, 0, 0},//10 |
|
- | 166 | { 0, 0, 0, 0},//11 |
|
- | 167 | { 0, 0, 0, 0}};//12 |
|
- | 168 | */ |
|
- | 169 | /* |
|
- | 170 | signed char Mixer.Motor[MAX_MOTORS][4] = { |
|
- | 171 | { 64, +64, 0, +64},//1 |
|
- | 172 | { 64, -64, 0, +64},//2 |
|
- | 173 | { 64, 0, -64, -64},//3 |
|
- | 174 | { 64, 0, +64, -64},//4 |
|
- | 175 | { 0, 0, 0, 0},//5 |
|
- | 176 | { 0, 0, 0, 0},//6 |
|
- | 177 | { 0, 0, 0, 0},//7 |
|
- | 178 | { 0, 0, 0, 0},//8 |
|
- | 179 | { 0, 0, 0, 0},//9 |
|
- | 180 | { 0, 0, 0, 0},//10 |
|
- | 181 | { 0, 0, 0, 0},//11 |
|
- | 182 | { 0, 0, 0, 0}};//12 |
|
- | 183 | */ |
|
- | 184 | unsigned char Motor[MAX_MOTORS]; |
|
Line 152... | Line 185... | ||
152 | 185 | signed int tmp_motorwert[MAX_MOTORS]; |
|
153 | 186 | ||
154 | int MotorSmoothing(int neu, int alt) |
187 | int MotorSmoothing(int neu, int alt) |
155 | { |
188 | { |
Line 456... | Line 489... | ||
456 | //############################################################################ |
489 | //############################################################################ |
457 | // Senden der Motorwerte per I2C-Bus |
490 | // Senden der Motorwerte per I2C-Bus |
458 | void SendMotorData(void) |
491 | void SendMotorData(void) |
459 | //############################################################################ |
492 | //############################################################################ |
460 | { |
493 | { |
- | 494 | unsigned char i; |
|
461 | if(!MotorenEin) |
495 | if(!MotorenEin) |
462 | { |
496 | { |
- | 497 | for(i=0;i<MAX_MOTORS;i++) Motor[i] = 0; |
|
463 | #ifndef QUADRO |
498 | #ifndef QUADRO |
464 | Motor1 = 0;Motor2 = 0;Motor3 = 0;Motor4 = 0;Motor5 = 0;Motor6 = 0;Motor7 = 0;Motor8 = 0; |
499 | Motor1 = 0;Motor2 = 0;Motor3 = 0;Motor4 = 0;Motor5 = 0;Motor6 = 0;Motor7 = 0;Motor8 = 0; |
465 | if(MotorTest[0]) {Motor1 = MotorTest[0]; Motor2 = MotorTest[0];} |
500 | if(MotorTest[0]) {Motor1 = MotorTest[0]; Motor2 = MotorTest[0];} |
466 | if(MotorTest[3]) {Motor3 = MotorTest[3]; Motor4 = MotorTest[3];} |
501 | if(MotorTest[3]) {Motor3 = MotorTest[3]; Motor4 = MotorTest[3];} |
467 | if(MotorTest[1]) {Motor5 = MotorTest[1]; Motor6 = MotorTest[1];} |
502 | if(MotorTest[1]) {Motor5 = MotorTest[1]; Motor6 = MotorTest[1];} |
Line 471... | Line 506... | ||
471 | if(MotorTest[0]) Motor_Vorne = MotorTest[0]; |
506 | if(MotorTest[0]) Motor_Vorne = MotorTest[0]; |
472 | if(MotorTest[1]) Motor_Hinten = MotorTest[1]; |
507 | if(MotorTest[1]) Motor_Hinten = MotorTest[1]; |
473 | if(MotorTest[2]) Motor_Links = MotorTest[2]; |
508 | if(MotorTest[2]) Motor_Links = MotorTest[2]; |
474 | if(MotorTest[3]) Motor_Rechts = MotorTest[3]; |
509 | if(MotorTest[3]) Motor_Rechts = MotorTest[3]; |
475 | #endif |
510 | #endif |
- | 511 | if(MotorTest[0]) Motor[0] = MotorTest[0]; |
|
- | 512 | if(MotorTest[1]) Motor[1] = MotorTest[1]; |
|
- | 513 | if(MotorTest[2]) Motor[2] = MotorTest[2]; |
|
- | 514 | if(MotorTest[3]) Motor[3] = MotorTest[3]; |
|
Line 476... | Line 515... | ||
476 | 515 | ||
477 | MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY); |
516 | MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY); |
Line 478... | Line 517... | ||
478 | } else MikroKopterFlags |= FLAG_MOTOR_RUN; |
517 | } else MikroKopterFlags |= FLAG_MOTOR_RUN; |
Line 534... | Line 573... | ||
534 | //############################################################################ |
573 | //############################################################################ |
535 | // |
574 | // |
536 | void MotorRegler(void) |
575 | void MotorRegler(void) |
537 | //############################################################################ |
576 | //############################################################################ |
538 | { |
577 | { |
539 | int motorwert,pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int; |
578 | int pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int; |
540 | int GierMischanteil,GasMischanteil; |
579 | int GierMischanteil,GasMischanteil; |
541 | static long SummeNick=0,SummeRoll=0; |
580 | static long SummeNick=0,SummeRoll=0; |
542 | static long sollGier = 0,tmp_long,tmp_long2; |
581 | static long sollGier = 0,tmp_long,tmp_long2; |
543 | static long IntegralFehlerNick = 0; |
582 | static long IntegralFehlerNick = 0; |
544 | static long IntegralFehlerRoll = 0; |
583 | static long IntegralFehlerRoll = 0; |
545 | static unsigned int RcLostTimer; |
584 | static unsigned int RcLostTimer; |
546 | static unsigned char delay_neutral = 0; |
585 | static unsigned char delay_neutral = 0; |
547 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
586 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
548 | static int hoehenregler = 0; |
587 | static int hoehenregler = 0; |
549 | static int motorwert1,motorwert2,motorwert3,motorwert4,motorwert5,motorwert6,motorwert7,motorwert8; |
588 | // static int motorwert1,motorwert2,motorwert3,motorwert4,motorwert5,motorwert6,motorwert7,motorwert8; |
550 | static char TimerWerteausgabe = 0; |
589 | static char TimerWerteausgabe = 0; |
551 | static char NeueKompassRichtungMerken = 0; |
590 | static char NeueKompassRichtungMerken = 0; |
552 | static long ausgleichNick, ausgleichRoll; |
591 | static long ausgleichNick, ausgleichRoll; |
553 | int IntegralNickMalFaktor,IntegralRollMalFaktor; |
592 | int IntegralNickMalFaktor,IntegralRollMalFaktor; |
- | 593 | unsigned char i; |
|
554 | Mittelwert(); |
594 | Mittelwert(); |
Line 555... | Line 595... | ||
555 | 595 | ||
556 | GRN_ON; |
596 | GRN_ON; |
557 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
597 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 1396... | Line 1436... | ||
1396 | pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
1436 | pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
1397 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1437 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1398 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
1438 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
1399 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
1439 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
Line 1400... | Line -... | ||
1400 | - | ||
1401 | #ifdef QUADRO |
1440 | |
1402 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1441 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1403 | // Quadro-Mischer |
- | |
1404 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
1405 | motorwert = GasMischanteil + pd_ergebnis_nick + GierMischanteil; // Mischer |
- | |
1406 | motorwert1 = MotorSmoothing(motorwert,motorwert1); |
- | |
1407 | motorwert = motorwert1 / STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1408 | Motor_Vorne = motorwert; |
- | |
1409 | - | ||
1410 | motorwert = GasMischanteil - pd_ergebnis_nick + GierMischanteil; |
- | |
1411 | motorwert2 = MotorSmoothing(motorwert,motorwert2); |
- | |
1412 | motorwert = motorwert2 / STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1413 | Motor_Hinten = motorwert; |
- | |
1414 | - | ||
1415 | motorwert = GasMischanteil + pd_ergebnis_roll - GierMischanteil; |
- | |
1416 | motorwert3 = MotorSmoothing(motorwert,motorwert3); |
- | |
1417 | motorwert = motorwert3 / STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1418 | Motor_Links = motorwert; |
- | |
1419 | - | ||
1420 | motorwert = GasMischanteil - pd_ergebnis_roll - GierMischanteil; |
- | |
1421 | motorwert4 = MotorSmoothing(motorwert,motorwert4); |
- | |
1422 | motorwert = motorwert4 / STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1423 | Motor_Rechts = motorwert; |
- | |
1424 | // +++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
1425 | #endif |
- | |
1426 | #ifdef OCTO |
- | |
1427 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
1428 | // Octo-Mischer |
- | |
1429 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
1430 | motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll + GierMischanteil; |
- | |
1431 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1432 | Motor1 = motorwert; |
- | |
1433 | - | ||
1434 | motorwert = GasMischanteil + pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil; |
- | |
1435 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1436 | Motor2 = motorwert; |
- | |
1437 | - | ||
1438 | motorwert = GasMischanteil + pd_ergebnis_nick - pd_ergebnis_roll + GierMischanteil; |
- | |
1439 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1440 | Motor3 = motorwert; |
- | |
1441 | - | ||
1442 | motorwert = GasMischanteil - pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil; |
- | |
1443 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1444 | Motor4 = motorwert; |
- | |
1445 | - | ||
1446 | motorwert = GasMischanteil - pd_ergebnis_nick - pd_ergebnis_roll + GierMischanteil; |
- | |
1447 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1448 | Motor5 = motorwert; |
- | |
1449 | - | ||
1450 | motorwert = GasMischanteil - pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil; |
- | |
1451 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1452 | Motor6 = motorwert; |
- | |
1453 | - | ||
1454 | motorwert = GasMischanteil - pd_ergebnis_nick + pd_ergebnis_roll + GierMischanteil; |
- | |
1455 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1456 | Motor7 = motorwert; |
- | |
1457 | - | ||
1458 | motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil; |
- | |
1459 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1460 | Motor8 = motorwert; |
- | |
1461 | // +++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
1462 | #endif |
- | |
1463 | #ifdef OCTO2 |
1442 | // Universal Mixer |
1464 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
1465 | // Octo-Mischer |
- | |
1466 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
1467 | motorwert = GasMischanteil + pd_ergebnis_nick + GierMischanteil; |
- | |
1468 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1469 | Motor1 = motorwert; |
- | |
1470 | - | ||
1471 | motorwert = GasMischanteil + pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil; |
- | |
1472 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1473 | Motor2 = motorwert; |
- | |
1474 | - | ||
1475 | motorwert = GasMischanteil - pd_ergebnis_roll + GierMischanteil; |
- | |
1476 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
1443 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1477 | Motor3 = motorwert; |
- | |
1478 | - | ||
1479 | motorwert = GasMischanteil - pd_ergebnis_nick - pd_ergebnis_roll - GierMischanteil; |
- | |
1480 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1481 | Motor4 = motorwert; |
- | |
1482 | - | ||
1483 | motorwert = GasMischanteil - pd_ergebnis_roll + GierMischanteil; |
- | |
1484 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1485 | Motor5 = motorwert; |
- | |
1486 | - | ||
1487 | motorwert = GasMischanteil - pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil; |
- | |
1488 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1489 | Motor6 = motorwert; |
1444 | for(i=0; i<MAX_MOTORS; i++) |
1490 | - | ||
1491 | motorwert = GasMischanteil + pd_ergebnis_roll + GierMischanteil; |
- | |
1492 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
1445 | { |
1493 | Motor7 = motorwert; |
- | |
1494 | - | ||
1495 | motorwert = GasMischanteil + pd_ergebnis_nick + pd_ergebnis_roll - GierMischanteil; |
- | |
1496 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
1446 | signed int tmp_int; |
1497 | Motor8 = motorwert; |
- | |
1498 | // +++++++++++++++++++++++++++++++++++++++++++++++ |
1447 | if(Mixer.Motor[i][0] > 0) |
1499 | #endif |
- | |
1500 | #ifdef OCTO3 |
- | |
1501 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
1502 | // Octo-Mischer |
- | |
1503 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1448 | { |
1504 | motorwert = GasMischanteil + pd_ergebnis_nick + GierMischanteil; |
- | |
1505 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1506 | Motor1 = motorwert; |
- | |
1507 | 1449 | tmp_int = ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L; |
|
1508 | motorwert = GasMischanteil + pd_ergebnis_nick - GierMischanteil; |
- | |
1509 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1510 | Motor2 = motorwert; |
- | |
1511 | 1450 | tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L; |
|
1512 | motorwert = GasMischanteil - pd_ergebnis_roll + GierMischanteil; |
- | |
1513 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1514 | Motor3 = motorwert; |
- | |
1515 | 1451 | tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L; |
|
1516 | motorwert = GasMischanteil - pd_ergebnis_roll - GierMischanteil; |
- | |
1517 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1518 | Motor4 = motorwert; |
- | |
1519 | - | ||
1520 | motorwert = GasMischanteil - pd_ergebnis_nick + GierMischanteil; |
- | |
1521 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
- | |
1522 | Motor5 = motorwert; |
- | |
1523 | - | ||
1524 | motorwert = GasMischanteil - pd_ergebnis_nick - GierMischanteil; |
1452 | tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
1525 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
1453 | tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]); // Filter |
1526 | Motor6 = motorwert; |
- | |
1527 | - | ||
1528 | motorwert = GasMischanteil + pd_ergebnis_roll + GierMischanteil; |
1454 | tmp_int = tmp_motorwert[i] / STICK_GAIN; |
1529 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
1455 | CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS); |
1530 | Motor7 = motorwert; |
1456 | Motor[i] = tmp_int; |
1531 | - | ||
1532 | motorwert = GasMischanteil + pd_ergebnis_roll - GierMischanteil; |
- | |
1533 | motorwert /= STICK_GAIN; CHECK_MIN_MAX(motorwert,MIN_GAS,MAX_GAS); |
1457 | } |
1534 | Motor8 = motorwert; |
- | |
1535 | // +++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
1536 | #endif |
1458 | else Motor[i] = 0; |
1537 | 1459 | } |
|
1538 | /* |
1460 | /* |
1539 | if(Poti1 > 20) Motor1 = 0; |
1461 | if(Poti1 > 20) Motor1 = 0; |
1540 | if(Poti1 > 90) Motor6 = 0; |
1462 | if(Poti1 > 90) Motor6 = 0; |
1541 | if(Poti1 > 140) Motor2 = 0; |
1463 | if(Poti1 > 140) Motor2 = 0; |