WebSVN - FlightCtrl - Diff - Rev 942 and 943 - /branches/V0.70d CRK HexaLotte/fc.c

 int16_t  DiffNick, DiffRoll;
 int16_t  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
+// setpoints for motors
+#ifdef HEXAKOPTER
+volatile uint8_t Motor_FrontLeft, Motor_FrontRight, Motor_RearLeft, Motor_RearRight, Motor_Right, Motor_Left;
+#else
-// setpoints for motors
+volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; //used by twimaster isr
-volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left;
+#endif
 /************************************************************************/
 void SendMotorData(void)
+{
     if(!(MKFlags & MKFLAG_MOTOR_RUN))
+    {
+#ifdef HEXAKOPTER
+        Motor_RearLeft = 0;
+        Motor_FrontLeft = 0;
+        Motor_RearRight = 0;
+        Motor_FrontRight = 0;
+        Motor_Right = 0;
+        Motor_Left = 0;
+        if(MotorTest[0]) Motor_FrontLeft = Motor_FrontRight = MotorTest[0];
+        if(MotorTest[1]) Motor_RearLeft = Motor_RearRight  = MotorTest[1];
+        if(MotorTest[2]) Motor_Left  = MotorTest[2];
+        if(MotorTest[3]) Motor_Right = MotorTest[3];
+#else
         Motor_Rear = 0;
         Motor_Front = 0;
         Motor_Right = 0;
         Motor_Left = 0;
         if(MotorTest[0]) Motor_Front = MotorTest[0];
         if(MotorTest[1]) Motor_Rear  = MotorTest[1];
         if(MotorTest[2]) Motor_Left  = MotorTest[2];
         if(MotorTest[3]) Motor_Right = MotorTest[3];
+#endif
         MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off
+    }
-    DebugOut.Analog[12] = Motor_Front;
-    DebugOut.Analog[13] = Motor_Rear;
-    DebugOut.Analog[14] = Motor_Left;
-    DebugOut.Analog[15] = Motor_Right;
     //Start I2C Interrupt Mode
     twi_state = TWI_STATE_MOTOR_TX;
     I2C_Start();
+}
 /*  MotorControl                                                        */
 /************************************************************************/
 void MotorControl(void)
+{
         int16_t MotorValue, pd_result, h, tmp_int;
-        int16_t YawMixFraction, GasMixFraction;
+        int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction;
         static int32_t SumNick = 0, SumRoll = 0;
         static int32_t SetPointYaw = 0;
         static int32_t IntegralErrorNick = 0;
         static int32_t IntegralErrorRoll = 0;
         static uint16_t RcLostTimer;
                 if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255;
                 if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255;
                 if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255;
                 if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255;
                 // if motors are off and the gas stick is in the upper position
                 if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && !(MKFlags & MKFLAG_MOTOR_RUN) )
+                {
                         // and if the yaw stick is in the leftmost position
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                         if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75)
+                        {
                                 if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
+                                {
-                                        delay_startmotors = 200; // do not repeat if once executed
+                                        delay_startmotors = 0; // do not repeat if once executed
                                         Model_Is_Flying = 1;
                                         MKFlags |= (MKFLAG_MOTOR_RUN|MKFLAG_START); // set flag RUN and START
                                         SetPointYaw = 0;
                                         Reading_IntegralGyroYaw = 0;
                                         Reading_IntegralGyroNick = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                         if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75)
+                                {
                                 if(++delay_stopmotors > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
+                                {
-                                        delay_stopmotors = 200; // do not repeat if once executed
+                                        delay_stopmotors = 0; // do not repeat if once executed
                                         Model_Is_Flying = 0;
                                         MKFlags &= ~(MKFLAG_MOTOR_RUN);
+                                }
+                        }
                         else delay_stopmotors = 0; // reset delay timer if sticks are not in this position
                 DebugOut.Analog[2]  = Mean_AccNick;
                 DebugOut.Analog[3]  = Mean_AccRoll;
                 DebugOut.Analog[4]  = Reading_GyroYaw;
                 DebugOut.Analog[5]  = ReadingHeight;
                 DebugOut.Analog[6]  = (Reading_Integral_Top / 512);
-                DebugOut.Analog[8]  = CompassHeading;
+//              DebugOut.Analog[8]  = CompassHeading;
+                DebugOut.Analog[8]  = RC_Quality;
                 DebugOut.Analog[9]  = UBat;
+#ifdef HEXAKOPTER
+                DebugOut.Analog[10] = Motor_FrontLeft;
+                DebugOut.Analog[11] = Motor_FrontRight;
+                DebugOut.Analog[12] = Motor_RearLeft;
+                DebugOut.Analog[13] = Motor_RearRight;
+                DebugOut.Analog[14] = Motor_Left;
-                DebugOut.Analog[10] = RC_Quality;
+                DebugOut.Analog[15] = Motor_Right;
+#else
                 DebugOut.Analog[11] = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
+                DebugOut.Analog[12] = Motor_Front;
+                DebugOut.Analog[13] = Motor_Rear;
+                DebugOut.Analog[14] = Motor_Left;
+                DebugOut.Analog[15] = Motor_Right;
+#endif
-                //DebugOut.Analog[16] = Mean_AccTop;
+                DebugOut.Analog[16] = Mean_AccTop;
     tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction)/2)) / 64;
     if(pd_result >  tmp_int) pd_result =  tmp_int;
-    if(pd_result < -tmp_int) pd_result = -tmp_int;
-        // Motor Front
+    if(pd_result < -tmp_int) pd_result = -tmp_int;
-    MotorValue = GasMixFraction + pd_result + YawMixFraction;     // Mixer
-    MotorValue /= STICK_GAIN;
-        if ((MotorValue < 0)) MotorValue = 0;
-        else if(MotorValue > ParamSet.Gas_Max)            MotorValue = ParamSet.Gas_Max;
-        if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
-        Motor_Front = MotorValue;
- // Motor Rear
-        MotorValue = GasMixFraction - pd_result + YawMixFraction;     // Mixer
-        MotorValue /= STICK_GAIN;
-        if ((MotorValue < 0)) MotorValue = 0;
         else if(MotorValue > ParamSet.Gas_Max)      MotorValue = ParamSet.Gas_Max;
-        if (MotorValue < ParamSet.Gas_Min)          MotorValue = ParamSet.Gas_Min;
+    NickMixFraction = pd_result;
         Motor_Rear = MotorValue;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Axis
     pd_result = DiffRoll + Ki * SumRoll;         // PI-controller for roll
     tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction)/2)) / 64;
     if(pd_result >  tmp_int) pd_result =  tmp_int;
     if(pd_result < -tmp_int) pd_result = -tmp_int;
+    RollMixFraction = pd_result;
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// Calculate Motor Mixes
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+#ifdef HEXAKOPTER
+        // Motor FrontLeft
+    MotorValue =        GasMixFraction
+                                        + NickMixFraction
+                                        + RollMixFraction/2
+                                        - YawMixFraction;         // Mixer
+    MotorValue /= STICK_GAIN;
+        if(MotorValue > ParamSet.Gas_Max)           MotorValue = ParamSet.Gas_Max;
+        else if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
+        Motor_FrontLeft = MotorValue;
+        // Motor FrontRight
+    MotorValue =        GasMixFraction
+                                        + NickMixFraction
+                                        - RollMixFraction/2
+                                        + YawMixFraction;         // Mixer
+    MotorValue /= STICK_GAIN;
+        if(MotorValue > ParamSet.Gas_Max)           MotorValue = ParamSet.Gas_Max;
+        else if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
+        Motor_FrontRight = MotorValue;
+        // Motor RearLeft
+        MotorValue =    GasMixFraction
+                                        - NickMixFraction
+                                        + RollMixFraction/2
+                                        - YawMixFraction;     // Mixer
+    MotorValue /= STICK_GAIN;
+        if(MotorValue > ParamSet.Gas_Max)           MotorValue = ParamSet.Gas_Max;
+        else if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
+        Motor_RearLeft = MotorValue;
+        // Motor RearRight
+        MotorValue =    GasMixFraction
+                                        - NickMixFraction
+                                        - RollMixFraction/2
+                                        + YawMixFraction;     // Mixer
+    MotorValue /= STICK_GAIN;
+        if(MotorValue > ParamSet.Gas_Max)           MotorValue = ParamSet.Gas_Max;
+        else if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
+        Motor_RearRight= MotorValue;
+    // Motor Left
+    MotorValue =        GasMixFraction
+                                        + RollMixFraction
+                                        + YawMixFraction;  // Mixer
+    MotorValue /= STICK_GAIN;
+        if(MotorValue > ParamSet.Gas_Max)           MotorValue = ParamSet.Gas_Max;
+        else if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
+    Motor_Left = MotorValue;
+        // Motor Right
+        MotorValue =    GasMixFraction
+                                        - RollMixFraction
+                                        - YawMixFraction;  // Mixer
+    MotorValue /= STICK_GAIN;
+        if(MotorValue > ParamSet.Gas_Max)           MotorValue = ParamSet.Gas_Max;
+        else if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
+    Motor_Right = MotorValue;
+#else
+        // Motor Front
+    MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;       // Mixer
+    MotorValue /= STICK_GAIN;
+        if(MotorValue > ParamSet.Gas_Max)           MotorValue = ParamSet.Gas_Max;
+        else if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
+        Motor_Front = MotorValue;
+        // Motor Rear
+        MotorValue = GasMixFraction - NickMixFraction + YawMixFraction;     // Mixer
+        MotorValue /= STICK_GAIN;
+        if(MotorValue > ParamSet.Gas_Max)           MotorValue = ParamSet.Gas_Max;
+        else if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
+        Motor_Rear = MotorValue;
     // Motor Left
-    MotorValue = GasMixFraction + pd_result - YawMixFraction;  // Mixer
+    MotorValue = GasMixFraction + RollMixFraction - YawMixFraction;  // Mixer
-    MotorValue /= STICK_GAIN;
-        if ((MotorValue < 0)) MotorValue = 0;
+    MotorValue /= STICK_GAIN;
-        else if(MotorValue > ParamSet.Gas_Max)          MotorValue = ParamSet.Gas_Max;
+        if(MotorValue > ParamSet.Gas_Max)           MotorValue = ParamSet.Gas_Max;
-        if (MotorValue < ParamSet.Gas_Min)          MotorValue = ParamSet.Gas_Min;
+        else if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
+    Motor_Left = MotorValue;
-    Motor_Left = MotorValue;
- // Motor Right
+        // Motor Right
-        MotorValue = GasMixFraction - pd_result - YawMixFraction;  // Mixer
-        MotorValue /= STICK_GAIN;
+        MotorValue = GasMixFraction - RollMixFraction - YawMixFraction;  // Mixer
-        if ((MotorValue < 0)) MotorValue = 0;
+        MotorValue /= STICK_GAIN;
-        else if(MotorValue > ParamSet.Gas_Max)          MotorValue = ParamSet.Gas_Max;
+        if(MotorValue > ParamSet.Gas_Max)           MotorValue = ParamSet.Gas_Max;
+        else if (MotorValue < ParamSet.Gas_Min)            MotorValue = ParamSet.Gas_Min;
-        if (MotorValue < ParamSet.Gas_Min)          MotorValue = ParamSet.Gas_Min;
+    Motor_Right = MotorValue;

Subversion Repositories FlightCtrl

(root)/branches/V0.70d CRK HexaLotte/fc.c - Rev 942 → 943