Subversion Repositories FlightCtrl

Rev

Rev 1221 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1221 Rev 1222
Line 124... Line 124...
124
int16_t NaviAccNick = 0, NaviAccRoll = 0, NaviCntAcc = 0;
124
int16_t NaviAccNick = 0, NaviAccRoll = 0, NaviCntAcc = 0;
Line 125... Line 125...
125
 
125
 
126
 
126
 
127
// MK flags
127
// MK flags
Line 128... Line 128...
128
uint16_t ModelIsFlying = 0;
128
uint16_t ModelIsFlying = 0;
Line 129... Line 129...
129
uint8_t  MKFlags = 0;
129
uint8_t  volatile MKFlags = 0;
-
 
130
 
-
 
131
int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L;
130
 
132
 
Line 131... Line 133...
131
int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L;
133
uint8_t GyroPFactor, GyroIFactor;       // the PD factors for the attitude control
Line 132... Line -...
132
 
-
 
Line 133... Line 134...
133
uint8_t GyroPFactor, GyroIFactor; // the PD factors for the attitude control
134
uint8_t GyroYawPFactor, GyroYawIFactor; // the PD factors for the yae control
Line 134... Line 135...
134
int16_t Ki = 10300 / 33;
135
 
135
 
136
int16_t Ki = 10300 / 33;
136
int16_t  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
137
 
Line 156... Line 157...
156
 
157
 
157
uint8_t LoopingNick = 0, LoopingRoll = 0;
158
uint8_t LoopingNick = 0, LoopingRoll = 0;
Line 158... Line 159...
158
uint8_t LoopingLeft = 0, LoopingRight = 0, LoopingDown = 0, LoopingTop = 0;
159
uint8_t LoopingLeft = 0, LoopingRight = 0, LoopingDown = 0, LoopingTop = 0;
Line 159... Line 160...
159
 
160
 
160
 
161
 
Line 527... Line 528...
527
/************************************************************************/
528
/************************************************************************/
528
/*  Transmit Motor Data via I2C                                         */
529
/*  Transmit Motor Data via I2C                                         */
529
/************************************************************************/
530
/************************************************************************/
530
void SendMotorData(void)
531
void SendMotorData(void)
531
{
532
{
-
 
533
        uint8_t i;
532
    if(!(MKFlags & MKFLAG_MOTOR_RUN))
534
    if(!(MKFlags & MKFLAG_MOTOR_RUN))
533
    {
535
    {
534
                #ifdef USE_QUADRO
-
 
535
                Motor1 = 0;
-
 
536
                Motor2 = 0;
-
 
537
                Motor3 = 0;
-
 
538
                Motor4 = 0;
-
 
539
                if(MotorTest[0]) Motor1 = MotorTest[0];
-
 
540
                if(MotorTest[1]) Motor2 = MotorTest[1];
-
 
541
                if(MotorTest[2]) Motor3 = MotorTest[2];
-
 
542
                if(MotorTest[3]) Motor4 = MotorTest[3];
-
 
543
                #else
-
 
544
                Motor1 = 0;
-
 
545
                Motor2 = 0;
-
 
546
                Motor3 = 0;
-
 
547
                Motor4 = 0;
-
 
548
                Motor5 = 0;
-
 
549
                Motor6 = 0;
-
 
550
                Motor7 = 0;
-
 
551
                Motor8 = 0;
-
 
552
                if(MotorTest[0]) {Motor1 = MotorTest[0]; Motor2 = MotorTest[0];}
-
 
553
                if(MotorTest[3]) {Motor3 = MotorTest[3]; Motor4 = MotorTest[3];}
-
 
554
                if(MotorTest[1]) {Motor5 = MotorTest[1]; Motor6 = MotorTest[1];}
-
 
555
                if(MotorTest[2]) {Motor7 = MotorTest[2]; Motor8 = MotorTest[2];}
-
 
556
 
-
 
557
                #endif
-
 
558
                MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off
536
                MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off
-
 
537
                for(i = 0; i < MAX_MOTORS; i++)
-
 
538
                {
-
 
539
                        if(!MotorTest_Active) Motor[i].SetPoint = 0;
-
 
540
                        else                  Motor[i].SetPoint = MotorTest[i];
-
 
541
                }
-
 
542
                if(MotorTest_Active) MotorTest_Active--;
559
        }
543
        }
560
        #ifdef USE_QUADRO
-
 
Line 561... Line 544...
561
 
544
 
562
        DebugOut.Analog[12] = Motor1; // Front
545
        DebugOut.Analog[12] = Motor[0].SetPoint; // Front
563
        DebugOut.Analog[13] = Motor2; // Rear
546
        DebugOut.Analog[13] = Motor[1].SetPoint; // Rear
564
        DebugOut.Analog[14] = Motor4; // Left
547
        DebugOut.Analog[14] = Motor[3].SetPoint; // Left
565
        DebugOut.Analog[15] = Motor3; // Right
-
 
566
        #else // OCTO Motor addresses are counted clockwise starting at the head
-
 
567
        DebugOut.Analog[12] = (Motor1 + Motor2) / 2;
-
 
568
        DebugOut.Analog[13] = (Motor5 + Motor6) / 2;
-
 
569
        DebugOut.Analog[14] = (Motor7 + Motor8) / 2;
-
 
570
        DebugOut.Analog[15] = (Motor3 + Motor4) / 2;
-
 
571
        #endif
548
        DebugOut.Analog[15] = Motor[2].SetPoint; // Right
572
    //Start I2C Interrupt Mode
549
    //Start I2C Interrupt Mode
573
    twi_state = TWI_STATE_MOTOR_TX;
-
 
574
    I2C_Start();
550
    I2C_Start(TWI_STATE_MOTOR_TX);
Line 575... Line -...
575
}
-
 
576
 
551
}
577
 
552
 
578
 
553
 
579
/************************************************************************/
554
/************************************************************************/
580
/*  Map the parameter to poti values                                    */
555
/*  Map the parameter to poti values                                    */
Line 650... Line 625...
650
/************************************************************************/
625
/************************************************************************/
651
/*  MotorControl                                                        */
626
/*  MotorControl                                                        */
652
/************************************************************************/
627
/************************************************************************/
653
void MotorControl(void)
628
void MotorControl(void)
654
{
629
{
655
        int16_t MotorValue, h, tmp_int;
630
        int16_t h, tmp_int;
Line 656... Line 631...
656
 
631
 
657
        // Mixer Fractions that are combined for Motor Control
632
        // Mixer Fractions that are combined for Motor Control
Line 658... Line 633...
658
        int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction;
633
        int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction;
Line 670... Line 645...
670
        static uint8_t HeightControlActive = 0;
645
        static uint8_t HeightControlActive = 0;
671
        static int16_t HeightControlGas = 0;
646
        static int16_t HeightControlGas = 0;
672
        static int8_t  TimerDebugOut = 0;
647
        static int8_t  TimerDebugOut = 0;
673
        static uint16_t UpdateCompassCourse = 0;
648
        static uint16_t UpdateCompassCourse = 0;
674
        // high resolution motor values for smoothing of PID motor outputs
649
        // high resolution motor values for smoothing of PID motor outputs
675
        static int16_t MotorValue1 = 0, MotorValue2 = 0, MotorValue3 = 0, MotorValue4 = 0;
-
 
676
        #ifndef USE_QUADRO
-
 
677
        static int16_t MotorValue5 = 0, MotorValue6 = 0, MotorValue7 = 0, MotorValue8 = 0;
650
        static int16_t MotorValue[MAX_MOTORS];
678
        #endif
651
        uint8_t i;
Line 679... Line 652...
679
 
652
 
680
        Mean();
653
        Mean();
Line 681... Line 654...
681
        GRN_ON;
654
        GRN_ON;
Line 960... Line 933...
960
                StickGas  = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;// shift to positive numbers
933
                StickGas  = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;// shift to positive numbers
Line 961... Line 934...
961
 
934
 
962
                // update gyro control loop factors
935
                // update gyro control loop factors
963
                GyroPFactor = FCParam.GyroP + 10;
936
                GyroPFactor = FCParam.GyroP + 10;
-
 
937
                GyroIFactor = FCParam.GyroI;
-
 
938
                GyroYawPFactor = FCParam.GyroP + 10;
Line 964... Line 939...
964
                GyroIFactor = FCParam.GyroI;
939
                GyroYawIFactor = FCParam.GyroI;
965
 
940
 
966
 
941
 
Line 1052... Line 1027...
1052
        {
1027
        {
1053
                StickYaw = 0;
1028
                StickYaw = 0;
1054
                StickNick = 0;
1029
                StickNick = 0;
1055
                StickRoll = 0;
1030
                StickRoll = 0;
1056
                GyroPFactor  = 90;
1031
                GyroPFactor  = 90;
1057
                GyroIFactor = 120;
1032
                GyroIFactor  = 120;
-
 
1033
                GyroYawPFactor = 90;
-
 
1034
                GyroYawIFactor = 120;
1058
                LoopingRoll = 0;
1035
                LoopingRoll = 0;
1059
                LoopingNick = 0;
1036
                LoopingNick = 0;
1060
                MaxStickNick = 0;
1037
                MaxStickNick = 0;
1061
                MaxStickRoll = 0;
1038
                MaxStickRoll = 0;
1062
        }
1039
        }
Line 1444... Line 1421...
1444
                DebugOut.Analog[19] = CompassCalState;
1421
                DebugOut.Analog[19] = CompassCalState;
1445
        //      DebugOut.Analog[24] = GyroNick/2;
1422
        //      DebugOut.Analog[24] = GyroNick/2;
1446
        //      DebugOut.Analog[25] = GyroRoll/2;
1423
        //      DebugOut.Analog[25] = GyroRoll/2;
1447
                DebugOut.Analog[27] = (int16_t)FCParam.KalmanMaxDrift;
1424
                DebugOut.Analog[27] = (int16_t)FCParam.KalmanMaxDrift;
1448
        //      DebugOut.Analog[28] = (int16_t)FCParam.KalmanMaxFusion;
1425
        //      DebugOut.Analog[28] = (int16_t)FCParam.KalmanMaxFusion;
1449
        //      DebugOut.Analog[29] = (int16_t)FCParam.KalmanK;
-
 
1450
                DebugOut.Analog[30] = GPSStickNick;
1426
                DebugOut.Analog[30] = GPSStickNick;
1451
                DebugOut.Analog[31] = GPSStickRoll;
1427
                DebugOut.Analog[31] = GPSStickRoll;
1452
        }
1428
        }
Line 1453... Line 1429...
1453
 
1429
 
Line 1483... Line 1459...
1483
        {
1459
        {
1484
                PPartRoll = 0;
1460
                PPartRoll = 0;
1485
        }
1461
        }
1486
        PDPartRoll = PPartRoll + (int32_t)((int32_t)GyroRoll * GyroPFactor +  (int32_t)TrimRoll * 128L) / (256L / STICK_GAIN); // +D-Part
1462
        PDPartRoll = PPartRoll + (int32_t)((int32_t)GyroRoll * GyroPFactor +  (int32_t)TrimRoll * 128L) / (256L / STICK_GAIN); // +D-Part
Line 1487... Line -...
1487
 
-
 
1488
        // octo has a double yaw momentum because of the doubled motor number
-
 
1489
        // therefore double D-Part and halfen P-Part for the same result
-
 
1490
        #ifdef USE_OCTO
-
 
1491
        PDPartYaw =  (int32_t)(GyroYaw * 4 * (int32_t)GyroPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroIFactor) / (4 * (44000 / STICK_GAIN));
-
 
1492
        #else
1463
 
1493
        PDPartYaw =  (int32_t)(GyroYaw * 2 * (int32_t)GyroPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroIFactor) / (2 * (44000 / STICK_GAIN));
-
 
Line 1494... Line 1464...
1494
        #endif
1464
        PDPartYaw =  (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / STICK_GAIN));
1495
 
1465
 
Line 1496... Line 1466...
1496
        //DebugOut.Analog[21] = PDPartNick;
1466
        //DebugOut.Analog[21] = PDPartNick;
Line 1501... Line 1471...
1501
        CHECK_MIN_MAX(PDPartNick, -SENSOR_LIMIT, SENSOR_LIMIT);
1471
        CHECK_MIN_MAX(PDPartNick, -SENSOR_LIMIT, SENSOR_LIMIT);
1502
        CHECK_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT);
1472
        CHECK_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT);
1503
        CHECK_MIN_MAX(PDPartYaw,  -SENSOR_LIMIT, SENSOR_LIMIT);
1473
        CHECK_MIN_MAX(PDPartYaw,  -SENSOR_LIMIT, SENSOR_LIMIT);
Line 1504... Line 1474...
1504
 
1474
 
-
 
1475
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1476
// all BL-Ctrl connected?
-
 
1477
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1478
        if(MissingMotor)
-
 
1479
        {
-
 
1480
                // if we are in the lift off condition
-
 
1481
                if( (ModelIsFlying > 1) && (ModelIsFlying < 50) && (GasMixFraction > 0) )
-
 
1482
                ModelIsFlying = 1; // keep within lift off condition
-
 
1483
                GasMixFraction = ParamSet.GasMin; // reduce gas to min to avoid lift of
-
 
1484
        }
-
 
1485
 
1505
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1486
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1506
// Height Control
1487
// Height Control
1507
// The height control algorithm reduces the gas but does not increase the gas.
1488
// The height control algorithm reduces the gas but does not increase the gas.
Line 1508... Line 1489...
1508
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1489
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 1636... Line 1617...
1636
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1617
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1637
        tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction) / 2)) / 64;
1618
        tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction) / 2)) / 64;
1638
        CHECK_MIN_MAX(NickMixFraction, -tmp_int, tmp_int);
1619
        CHECK_MIN_MAX(NickMixFraction, -tmp_int, tmp_int);
1639
        CHECK_MIN_MAX(RollMixFraction, -tmp_int, tmp_int);
1620
        CHECK_MIN_MAX(RollMixFraction, -tmp_int, tmp_int);
Line 1640... Line -...
1640
 
-
 
1641
#ifdef USE_QUADRO
-
 
1642
 
-
 
1643
        // QuadroKopter Mixer
-
 
1644
 
-
 
1645
        // Motor Front (++)
1621
 
1646
    MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;
-
 
1647
    MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
-
 
1648
    MotorValue = MotorValue1 / STICK_GAIN;
-
 
1649
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1650
        Motor1 = MotorValue;
-
 
1651
 
-
 
1652
        // Motor Rear (-+)
-
 
1653
        MotorValue = GasMixFraction - NickMixFraction + YawMixFraction;
-
 
1654
        MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
-
 
1655
        MotorValue = MotorValue2 / STICK_GAIN;
-
 
1656
        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1657
        Motor2 = MotorValue;
-
 
1658
 
-
 
1659
        // Motor Right (--)
-
 
1660
        MotorValue = GasMixFraction - RollMixFraction - YawMixFraction;
-
 
1661
        MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
-
 
1662
        MotorValue = MotorValue3 / STICK_GAIN;
-
 
1663
        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1664
    Motor3 = MotorValue;
-
 
1665
 
-
 
1666
        // Motor Left (+-)
-
 
1667
    MotorValue = GasMixFraction + RollMixFraction - YawMixFraction;
-
 
1668
    MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
-
 
1669
    MotorValue = MotorValue4 / STICK_GAIN;
-
 
1670
        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1671
    Motor4 = MotorValue;
-
 
1672
 
-
 
1673
#endif
-
 
1674
 
-
 
1675
#ifdef USE_OCTO
-
 
1676
 
1622
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1677
        // OctoKopter Mixer
-
 
1678
 
-
 
1679
        // Motor 1 (+++)
1623
// Universal Mixer
1680
    MotorValue = GasMixFraction + NickMixFraction + RollMixFraction + YawMixFraction;
-
 
1681
    MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
-
 
1682
    MotorValue = MotorValue1 / STICK_GAIN;
-
 
1683
        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1684
        Motor1= MotorValue;
-
 
1685
 
-
 
1686
        // Motor 2 (+--)
-
 
1687
        MotorValue = GasMixFraction + NickMixFraction - RollMixFraction - YawMixFraction;
-
 
1688
        MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
-
 
1689
    MotorValue = MotorValue2 / STICK_GAIN;
-
 
1690
        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1691
        Motor2 = MotorValue;
-
 
1692
 
-
 
1693
        // Motor 3 (+-+)
-
 
1694
        MotorValue = GasMixFraction + NickMixFraction - RollMixFraction + YawMixFraction;
-
 
1695
        MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
-
 
1696
    MotorValue = MotorValue3 / STICK_GAIN;
-
 
1697
        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1698
        Motor3 = MotorValue;
-
 
1699
 
-
 
1700
        // Motor 4 (---)
-
 
1701
        MotorValue = GasMixFraction - NickMixFraction - RollMixFraction - YawMixFraction;
-
 
1702
        MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
-
 
1703
    MotorValue = MotorValue4 / STICK_GAIN;
-
 
1704
        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1705
        Motor4 = MotorValue;
-
 
1706
 
-
 
1707
        // Motor 5 (--+)
-
 
1708
        MotorValue = GasMixFraction - NickMixFraction - RollMixFraction + YawMixFraction;
-
 
1709
        MotorValue5 = MotorSmoothing(MotorValue, MotorValue5);
-
 
1710
    MotorValue = MotorValue5 / STICK_GAIN;
-
 
1711
        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
1624
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1712
    Motor5 = MotorValue;
1625
        for(i = 0; i < MAX_MOTORS; i++)
1713
 
1626
        {
1714
        // Motor 6 (-+-)
-
 
1715
        MotorValue = GasMixFraction - NickMixFraction + RollMixFraction - YawMixFraction;
-
 
1716
        MotorValue6 = MotorSmoothing(MotorValue, MotorValue6);
-
 
1717
    MotorValue = MotorValue6 / STICK_GAIN;
-
 
1718
        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1719
    Motor6 = MotorValue;
-
 
1720
 
-
 
1721
        // Motor7 (-++)
-
 
1722
        MotorValue = GasMixFraction - NickMixFraction + RollMixFraction + YawMixFraction;
-
 
1723
        MotorValue7 = MotorSmoothing(MotorValue, MotorValue7);
-
 
1724
    MotorValue = MotorValue7 / STICK_GAIN;
-
 
1725
        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1726
    Motor7 = MotorValue;
-
 
1727
 
-
 
1728
    // Motor8 (++-)
-
 
1729
        MotorValue = GasMixFraction + NickMixFraction + RollMixFraction - YawMixFraction;
-
 
1730
        MotorValue8 = MotorSmoothing(MotorValue, MotorValue8);
-
 
1731
    MotorValue = MotorValue8 / STICK_GAIN;
1627
                int16_t tmp;
1732
        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1733
    Motor8 = MotorValue;
-
 
1734
#endif
-
 
1735
 
-
 
1736
#ifdef USE_OCTO2
-
 
1737
 
-
 
1738
        // Octokopter Mixer alternativ setup
1628
                if(Mixer.Motor[i][MIX_GAS] > 0) // if gas then mixer
1739
 
1629
                {
1740
    MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;
-
 
1741
    MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
-
 
1742
    MotorValue = MotorValue1 / STICK_GAIN;
-
 
1743
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1744
        Motor1 = MotorValue;
-
 
1745
 
-
 
1746
    MotorValue = GasMixFraction + NickMixFraction - RollMixFraction - YawMixFraction;
-
 
1747
    MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
-
 
1748
    MotorValue = MotorValue2 / STICK_GAIN;
-
 
1749
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1750
        Motor2 = MotorValue;
-
 
1751
 
-
 
1752
        MotorValue = GasMixFraction - RollMixFraction + YawMixFraction;
-
 
1753
    MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
-
 
1754
    MotorValue = MotorValue3 / STICK_GAIN;
-
 
1755
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1756
    Motor3 = MotorValue;
-
 
1757
 
-
 
1758
        MotorValue = GasMixFraction - NickMixFraction - RollMixFraction - YawMixFraction;
-
 
1759
    MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
-
 
1760
    MotorValue = MotorValue4 / STICK_GAIN;
-
 
1761
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1762
    Motor4 = MotorValue;
-
 
1763
 
-
 
1764
        MotorValue = GasMixFraction - RollMixFraction + YawMixFraction;
-
 
1765
    MotorValue5 = MotorSmoothing(MotorValue, MotorValue5);
-
 
1766
    MotorValue = MotorValue5 / STICK_GAIN;
-
 
1767
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1768
        Motor5 = MotorValue;
-
 
1769
 
1630
                        tmp =  ((int32_t)GasMixFraction  * Mixer.Motor[i][MIX_GAS] ) / 64L;
1770
        MotorValue = GasMixFraction - NickMixFraction + RollMixFraction - YawMixFraction;
-
 
1771
    MotorValue6 = MotorSmoothing(MotorValue, MotorValue6);
-
 
1772
    MotorValue = MotorValue6 / STICK_GAIN;
-
 
1773
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1774
        Motor6 = MotorValue;
-
 
1775
 
-
 
1776
    MotorValue = GasMixFraction + RollMixFraction + YawMixFraction;
-
 
1777
    MotorValue7 = MotorSmoothing(MotorValue, MotorValue7);
-
 
1778
    MotorValue = MotorValue7 / STICK_GAIN;
-
 
1779
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1780
    Motor7 = MotorValue;
-
 
1781
 
-
 
1782
    MotorValue = GasMixFraction + NickMixFraction + RollMixFraction - YawMixFraction;
-
 
1783
    MotorValue8 = MotorSmoothing(MotorValue, MotorValue8);
-
 
1784
    MotorValue = MotorValue8 / STICK_GAIN;
-
 
1785
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1786
    Motor8 = MotorValue;
-
 
1787
#endif
-
 
1788
 
-
 
1789
#ifdef USE_OCTO3
-
 
1790
 
-
 
1791
        // Octokopter Mixer alternativ setup
-
 
1792
 
1631
                        tmp += ((int32_t)NickMixFraction * Mixer.Motor[i][MIX_NICK]) / 64L;
1793
    MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;
-
 
1794
    MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
-
 
1795
    MotorValue = MotorValue1 / STICK_GAIN;
-
 
1796
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1797
        Motor1 = MotorValue;
-
 
1798
 
1632
                        tmp += ((int32_t)RollMixFraction * Mixer.Motor[i][MIX_ROLL]) / 64L;
1799
    MotorValue = GasMixFraction + NickMixFraction - YawMixFraction;
-
 
1800
    MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
-
 
1801
    MotorValue = MotorValue2 / STICK_GAIN;
-
 
1802
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1803
        Motor2 = MotorValue;
-
 
1804
 
-
 
1805
        MotorValue = GasMixFraction - RollMixFraction + YawMixFraction;
-
 
1806
    MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
-
 
1807
    MotorValue = MotorValue3 / STICK_GAIN;
-
 
1808
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1809
    Motor3 = MotorValue;
-
 
1810
 
-
 
1811
        MotorValue = GasMixFraction - RollMixFraction - YawMixFraction;
-
 
1812
    MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
-
 
1813
    MotorValue = MotorValue4 / STICK_GAIN;
-
 
1814
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1815
    Motor4 = MotorValue;
-
 
1816
 
-
 
1817
        MotorValue = GasMixFraction - NickMixFraction + YawMixFraction;
-
 
1818
    MotorValue5 = MotorSmoothing(MotorValue, MotorValue5);
-
 
1819
    MotorValue = MotorValue5 / STICK_GAIN;
-
 
1820
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1821
        Motor5 = MotorValue;
-
 
1822
 
-
 
1823
        MotorValue = GasMixFraction - NickMixFraction - YawMixFraction;
1633
                        tmp += ((int32_t)YawMixFraction  * Mixer.Motor[i][MIX_YAW] ) / 64L;
1824
    MotorValue6 = MotorSmoothing(MotorValue, MotorValue6);
1634
                        MotorValue[i] = MotorSmoothing(tmp, MotorValue[i]);  // Spike Filter
1825
    MotorValue = MotorValue6 / STICK_GAIN;
1635
                        tmp = MotorValue[i] / STICK_GAIN;
1826
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
1636
                        CHECK_MIN_MAX(tmp, ParamSet.GasMin, ParamSet.GasMax);
1827
        Motor6 = MotorValue;
-
 
1828
 
-
 
1829
    MotorValue = GasMixFraction + RollMixFraction + YawMixFraction;
-
 
1830
    MotorValue7 = MotorSmoothing(MotorValue, MotorValue7);
-
 
1831
    MotorValue = MotorValue7 / STICK_GAIN;
-
 
1832
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-
 
1833
    Motor7 = MotorValue;
1637
                        Motor[i].SetPoint = tmp;
1834
 
-
 
1835
    MotorValue = GasMixFraction + RollMixFraction - YawMixFraction;
-
 
1836
    MotorValue8 = MotorSmoothing(MotorValue, MotorValue8);
-
 
1837
    MotorValue = MotorValue8 / STICK_GAIN;
-
 
1838
    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
1638
                }
1839
    Motor8 = MotorValue;
-
 
1840
#endif
-
 
1841
 
1639
                else Motor[i].SetPoint = 0;
1842
 
1640
        }