Rev 901 | Rev 909 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 901 | Rev 903 | ||
---|---|---|---|
Line 127... | Line 127... | ||
127 | volatile int16_t DiffPitch, DiffRoll; |
127 | volatile int16_t DiffPitch, DiffRoll; |
Line 128... | Line 128... | ||
128 | 128 | ||
Line 129... | Line 129... | ||
129 | int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
129 | int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
- | 130 | ||
- | 131 | // setpoints for motors |
|
- | 132 | #ifdef HEXAKOPTER |
|
130 | 133 | volatile uint8_t Motor_FrontLeft, Motor_FrontRight, Motor_RearLeft, Motor_RearRight, Motor_Right, Motor_Left; |
|
- | 134 | #else |
|
Line 131... | Line 135... | ||
131 | // setpoints for motors |
135 | volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; //used by twimaster isr |
132 | volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; |
136 | #endif |
133 | 137 | ||
Line 378... | Line 382... | ||
378 | /************************************************************************/ |
382 | /************************************************************************/ |
379 | void SendMotorData(void) |
383 | void SendMotorData(void) |
380 | { |
384 | { |
381 | if(MOTOR_OFF || !MotorsOn) |
385 | if(MOTOR_OFF || !MotorsOn) |
382 | { |
386 | { |
- | 387 | #ifdef HEXAKOPTER |
|
- | 388 | Motor_RearLeft = 0; |
|
- | 389 | Motor_FrontLeft = 0; |
|
- | 390 | Motor_RearRight = 0; |
|
- | 391 | Motor_FrontRight = 0; |
|
- | 392 | Motor_Right = 0; |
|
- | 393 | Motor_Left = 0; |
|
- | 394 | if(MotorTest[0]) Motor_FrontLeft = Motor_FrontRight = MotorTest[0]; |
|
- | 395 | if(MotorTest[1]) Motor_RearLeft = Motor_RearRight = MotorTest[1]; |
|
- | 396 | if(MotorTest[2]) Motor_Left = MotorTest[2]; |
|
- | 397 | if(MotorTest[3]) Motor_Right = MotorTest[3]; |
|
- | 398 | } |
|
- | 399 | ||
- | 400 | #else |
|
383 | Motor_Rear = 0; |
401 | Motor_Rear = 0; |
384 | Motor_Front = 0; |
402 | Motor_Front = 0; |
385 | Motor_Right = 0; |
403 | Motor_Right = 0; |
386 | Motor_Left = 0; |
404 | Motor_Left = 0; |
387 | if(MotorTest[0]) Motor_Front = MotorTest[0]; |
405 | if(MotorTest[0]) Motor_Front = MotorTest[0]; |
388 | if(MotorTest[1]) Motor_Rear = MotorTest[1]; |
406 | if(MotorTest[1]) Motor_Rear = MotorTest[1]; |
389 | if(MotorTest[2]) Motor_Left = MotorTest[2]; |
407 | if(MotorTest[2]) Motor_Left = MotorTest[2]; |
390 | if(MotorTest[3]) Motor_Right = MotorTest[3]; |
408 | if(MotorTest[3]) Motor_Right = MotorTest[3]; |
391 | } |
409 | } |
Line 392... | Line -... | ||
392 | - | ||
393 | DebugOut.Analog[12] = Motor_Front; |
- | |
394 | DebugOut.Analog[13] = Motor_Rear; |
- | |
395 | DebugOut.Analog[14] = Motor_Left; |
- | |
Line -... | Line 410... | ||
- | 410 | ||
396 | DebugOut.Analog[15] = Motor_Right; |
411 | |
397 | 412 | #endif |
|
398 | //Start I2C Interrupt Mode |
413 | //Start I2C Interrupt Mode |
399 | twi_state = 0; |
414 | twi_state = 0; |
400 | motor = 0; |
415 | motor = 0; |
Line 461... | Line 476... | ||
461 | /* MotorControl */ |
476 | /* MotorControl */ |
462 | /************************************************************************/ |
477 | /************************************************************************/ |
463 | void MotorControl(void) |
478 | void MotorControl(void) |
464 | { |
479 | { |
465 | int16_t MotorValue, pd_result, h, tmp_int; |
480 | int16_t MotorValue, pd_result, h, tmp_int; |
466 | int16_t YawMixFraction, ThrustMixFraction; |
481 | int16_t YawMixFraction, ThrustMixFraction, PitchMixFraction, RollMixFraction; |
467 | static int32_t SumPitch = 0, SumRoll = 0; |
482 | static int32_t SumPitch = 0, SumRoll = 0; |
468 | static int32_t SetPointYaw = 0; |
483 | static int32_t SetPointYaw = 0; |
469 | static int32_t IntegralErrorPitch = 0; |
484 | static int32_t IntegralErrorPitch = 0; |
470 | static int32_t IntegralErrorRoll = 0; |
485 | static int32_t IntegralErrorRoll = 0; |
471 | static uint16_t RcLostTimer; |
486 | static uint16_t RcLostTimer; |
Line 558... | Line 573... | ||
558 | if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255; |
573 | if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255; |
559 | if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255; |
574 | if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255; |
560 | if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255; |
575 | if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255; |
Line 561... | Line 576... | ||
561 | 576 | ||
562 | // if motors are off and the thrust stick is in the upper position |
577 | // if motors are off and the thrust stick is in the upper position |
563 | if((PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] > 80) && MotorsOn == 0) |
578 | if(MotorsOn == 0 && (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] > 80)) |
564 | { |
579 | { |
565 | // and if the yaw stick is in the leftmost position |
580 | // and if the yaw stick is in the leftmost position |
566 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
581 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
567 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
582 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 676... | Line 691... | ||
676 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
691 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
677 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
692 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
678 | { |
693 | { |
679 | if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
694 | if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
680 | { |
695 | { |
681 | delay_startmotors = 200; // do not repeat if once executed |
696 | delay_startmotors = 0; // do not repeat if once executed |
682 | Model_Is_Flying = 1; |
697 | Model_Is_Flying = 1; |
683 | MotorsOn = 1; |
698 | MotorsOn = 1; |
684 | SetPointYaw = 0; |
699 | SetPointYaw = 0; |
685 | Reading_IntegralGyroYaw = 0; |
700 | Reading_IntegralGyroYaw = 0; |
686 | Reading_IntegralGyroPitch = 0; |
701 | Reading_IntegralGyroPitch = 0; |
Line 703... | Line 718... | ||
703 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
718 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
704 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
719 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
705 | { |
720 | { |
706 | if(++delay_stopmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
721 | if(++delay_stopmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
707 | { |
722 | { |
708 | delay_stopmotors = 200; // do not repeat if once executed |
723 | delay_stopmotors = 0; // do not repeat if once executed |
709 | Model_Is_Flying = 0; |
724 | Model_Is_Flying = 0; |
710 | MotorsOn = 0; |
725 | MotorsOn = 0; |
711 | #ifdef USE_KILLAGREG |
726 | #ifdef USE_KILLAGREG |
712 | if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE) |
727 | if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE) |
713 | { |
728 | { |
Line 1227... | Line 1242... | ||
1227 | DebugOut.Analog[2] = Mean_AccPitch; |
1242 | DebugOut.Analog[2] = Mean_AccPitch; |
1228 | DebugOut.Analog[3] = Mean_AccRoll; |
1243 | DebugOut.Analog[3] = Mean_AccRoll; |
1229 | DebugOut.Analog[4] = Reading_GyroYaw; |
1244 | DebugOut.Analog[4] = Reading_GyroYaw; |
1230 | DebugOut.Analog[5] = ReadingHeight; |
1245 | DebugOut.Analog[5] = ReadingHeight; |
1231 | DebugOut.Analog[6] = (Reading_Integral_Top / 512); |
1246 | DebugOut.Analog[6] = (Reading_Integral_Top / 512); |
1232 | DebugOut.Analog[8] = CompassHeading; |
1247 | // DebugOut.Analog[8] = CompassHeading; |
- | 1248 | DebugOut.Analog[8] = RC_Quality; |
|
1233 | DebugOut.Analog[9] = UBat; |
1249 | DebugOut.Analog[9] = UBat; |
- | 1250 | ||
- | 1251 | #ifdef HEXAKOPTER |
|
- | 1252 | DebugOut.Analog[10] = Motor_FrontLeft; |
|
- | 1253 | DebugOut.Analog[11] = Motor_FrontRight; |
|
- | 1254 | DebugOut.Analog[12] = Motor_RearLeft; |
|
- | 1255 | DebugOut.Analog[13] = Motor_RearRight; |
|
- | 1256 | DebugOut.Analog[14] = Motor_Left; |
|
1234 | DebugOut.Analog[10] = RC_Quality; |
1257 | DebugOut.Analog[15] = Motor_Right; |
- | 1258 | #else |
|
1235 | DebugOut.Analog[11] = YawGyroHeading / YAW_GYRO_DEG_FACTOR; |
1259 | DebugOut.Analog[11] = YawGyroHeading / YAW_GYRO_DEG_FACTOR; |
- | 1260 | ||
- | 1261 | DebugOut.Analog[12] = Motor_Front; |
|
- | 1262 | DebugOut.Analog[13] = Motor_Rear; |
|
- | 1263 | DebugOut.Analog[14] = Motor_Left; |
|
- | 1264 | DebugOut.Analog[15] = Motor_Right; |
|
- | 1265 | #endif |
|
- | 1266 | ||
1236 | DebugOut.Analog[16] = Mean_AccTop; |
1267 | DebugOut.Analog[16] = Mean_AccTop; |
Line 1237... | Line 1268... | ||
1237 | 1268 | ||
Line 1384... | Line 1415... | ||
1384 | 1415 | ||
1385 | tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(ThrustMixFraction + abs(YawMixFraction)/2)) / 64; |
1416 | tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(ThrustMixFraction + abs(YawMixFraction)/2)) / 64; |
1386 | if(pd_result > tmp_int) pd_result = tmp_int; |
1417 | if(pd_result > tmp_int) pd_result = tmp_int; |
Line 1387... | Line -... | ||
1387 | if(pd_result < -tmp_int) pd_result = -tmp_int; |
- | |
1388 | - | ||
1389 | // Motor Front |
1418 | if(pd_result < -tmp_int) pd_result = -tmp_int; |
1390 | MotorValue = ThrustMixFraction + pd_result + YawMixFraction; // Mixer |
- | |
1391 | MotorValue /= STICK_GAIN; |
- | |
1392 | if ((MotorValue < 0)) MotorValue = 0; |
- | |
1393 | else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
- | |
Line 1394... | Line -... | ||
1394 | if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
- | |
1395 | Motor_Front = MotorValue; |
- | |
1396 | - | ||
1397 | // Motor Rear |
- | |
1398 | MotorValue = ThrustMixFraction - pd_result + YawMixFraction; // Mixer |
- | |
1399 | MotorValue /= STICK_GAIN; |
- | |
1400 | if ((MotorValue < 0)) MotorValue = 0; |
- | |
1401 | else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
1419 | |
1402 | if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
1420 | PitchMixFraction = pd_result; |
1403 | Motor_Rear = MotorValue; |
1421 | |
1404 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1422 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1405 | // Roll-Axis |
1423 | // Roll-Axis |
Line 1412... | Line 1430... | ||
1412 | pd_result = DiffRoll + Ki * SumRoll; // PI-controller for roll |
1430 | pd_result = DiffRoll + Ki * SumRoll; // PI-controller for roll |
1413 | tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(ThrustMixFraction + abs(YawMixFraction)/2)) / 64; |
1431 | tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(ThrustMixFraction + abs(YawMixFraction)/2)) / 64; |
1414 | if(pd_result > tmp_int) pd_result = tmp_int; |
1432 | if(pd_result > tmp_int) pd_result = tmp_int; |
1415 | if(pd_result < -tmp_int) pd_result = -tmp_int; |
1433 | if(pd_result < -tmp_int) pd_result = -tmp_int; |
Line -... | Line 1434... | ||
- | 1434 | ||
- | 1435 | RollMixFraction = pd_result; |
|
- | 1436 | ||
- | 1437 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 1438 | // Calculate Motor Mixes |
|
- | 1439 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 1440 | ||
- | 1441 | #ifdef HEXAKOPTER |
|
- | 1442 | // Motor FrontLeft |
|
- | 1443 | MotorValue = ThrustMixFraction |
|
- | 1444 | + PitchMixFraction |
|
- | 1445 | + RollMixFraction/2 |
|
- | 1446 | - YawMixFraction; // Mixer |
|
- | 1447 | MotorValue /= STICK_GAIN; |
|
- | 1448 | if (MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
|
- | 1449 | else if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
|
- | 1450 | Motor_FrontLeft = MotorValue; |
|
- | 1451 | ||
- | 1452 | // Motor FrontRight |
|
- | 1453 | MotorValue = ThrustMixFraction |
|
- | 1454 | + PitchMixFraction |
|
- | 1455 | - RollMixFraction/2 |
|
- | 1456 | + YawMixFraction; // Mixer |
|
- | 1457 | MotorValue /= STICK_GAIN; |
|
- | 1458 | if (MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
|
- | 1459 | else if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
|
- | 1460 | Motor_FrontRight = MotorValue; |
|
- | 1461 | ||
- | 1462 | // Motor RearLeft |
|
- | 1463 | MotorValue = ThrustMixFraction |
|
- | 1464 | - PitchMixFraction |
|
- | 1465 | + RollMixFraction/2 |
|
- | 1466 | - YawMixFraction; // Mixer |
|
- | 1467 | MotorValue /= STICK_GAIN; |
|
- | 1468 | if (MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
|
- | 1469 | else if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
|
- | 1470 | Motor_RearLeft = MotorValue; |
|
- | 1471 | ||
- | 1472 | // Motor RearRight |
|
- | 1473 | MotorValue = ThrustMixFraction |
|
- | 1474 | - PitchMixFraction |
|
- | 1475 | - RollMixFraction/2 |
|
- | 1476 | + YawMixFraction; // Mixer |
|
- | 1477 | MotorValue /= STICK_GAIN; |
|
- | 1478 | if (MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
|
- | 1479 | else if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
|
- | 1480 | Motor_RearRight= MotorValue; |
|
- | 1481 | ||
- | 1482 | // Motor Left |
|
- | 1483 | MotorValue = ThrustMixFraction |
|
- | 1484 | + RollMixFraction |
|
- | 1485 | + YawMixFraction; // Mixer |
|
- | 1486 | MotorValue /= STICK_GAIN; |
|
- | 1487 | if (MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
|
- | 1488 | else if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
|
- | 1489 | Motor_Left = MotorValue; |
|
- | 1490 | ||
- | 1491 | // Motor Right |
|
- | 1492 | MotorValue = ThrustMixFraction |
|
- | 1493 | - RollMixFraction |
|
- | 1494 | - YawMixFraction; // Mixer |
|
- | 1495 | MotorValue /= STICK_GAIN; |
|
- | 1496 | if (MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
|
- | 1497 | else if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
|
- | 1498 | Motor_Right = MotorValue; |
|
- | 1499 | ||
- | 1500 | #else |
|
- | 1501 | ||
- | 1502 | // Motor Front |
|
- | 1503 | MotorValue = ThrustMixFraction + PitchMixFraction + YawMixFraction; // Mixer |
|
- | 1504 | MotorValue /= STICK_GAIN; |
|
- | 1505 | if ((MotorValue < 0)) MotorValue = 0; |
|
- | 1506 | else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
|
- | 1507 | if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
|
- | 1508 | Motor_Front = MotorValue; |
|
- | 1509 | ||
- | 1510 | // Motor Rear |
|
- | 1511 | MotorValue = ThrustMixFraction - PitchMixFraction + YawMixFraction; // Mixer |
|
- | 1512 | MotorValue /= STICK_GAIN; |
|
- | 1513 | if ((MotorValue < 0)) MotorValue = 0; |
|
- | 1514 | else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
|
- | 1515 | if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
|
- | 1516 | Motor_Rear = MotorValue; |
|
- | 1517 | ||
1416 | 1518 | ||
1417 | // Motor Left |
1519 | // Motor Left |
1418 | MotorValue = ThrustMixFraction + pd_result - YawMixFraction; // Mixer |
1520 | MotorValue = ThrustMixFraction + RollMixFraction - YawMixFraction; // Mixer |
1419 | MotorValue /= STICK_GAIN; |
1521 | MotorValue /= STICK_GAIN; |
1420 | if ((MotorValue < 0)) MotorValue = 0; |
1522 | if ((MotorValue < 0)) MotorValue = 0; |
1421 | else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
1523 | else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
1422 | if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
1524 | if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
Line 1423... | Line 1525... | ||
1423 | Motor_Left = MotorValue; |
1525 | Motor_Left = MotorValue; |
1424 | 1526 | ||
1425 | // Motor Right |
1527 | // Motor Right |
1426 | MotorValue = ThrustMixFraction - pd_result - YawMixFraction; // Mixer |
1528 | MotorValue = ThrustMixFraction - RollMixFraction - YawMixFraction; // Mixer |
1427 | MotorValue /= STICK_GAIN; |
1529 | MotorValue /= STICK_GAIN; |
1428 | if ((MotorValue < 0)) MotorValue = 0; |
1530 | if ((MotorValue < 0)) MotorValue = 0; |
1429 | else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
1531 | else if(MotorValue > ParamSet.Trust_Max) MotorValue = ParamSet.Trust_Max; |
- | 1532 | if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
|
1430 | if (MotorValue < ParamSet.Trust_Min) MotorValue = ParamSet.Trust_Min; |
1533 | Motor_Right = MotorValue; |