WebSVN - FlightCtrl - Diff - Rev 901 and 903 - /branches/V0.69k CRK HexaLotte/fc.c

 volatile int16_t  DiffPitch, 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(MOTOR_OFF || !MotorsOn)
+    {
+#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];
+     }
-    DebugOut.Analog[12] = Motor_Front;
-    DebugOut.Analog[13] = Motor_Rear;
-    DebugOut.Analog[14] = Motor_Left;
     DebugOut.Analog[15] = Motor_Right;
 #endif
     //Start I2C Interrupt Mode
     twi_state = 0;
     motor = 0;
 /*  MotorControl                                                        */
 /************************************************************************/
 void MotorControl(void)
+{
         int16_t MotorValue, pd_result, h, tmp_int;
-        int16_t YawMixFraction, ThrustMixFraction;
+        int16_t YawMixFraction, ThrustMixFraction, PitchMixFraction, RollMixFraction;
         static int32_t SumPitch = 0, SumRoll = 0;
         static int32_t SetPointYaw = 0;
         static int32_t IntegralErrorPitch = 0;
         static int32_t IntegralErrorRoll = 0;
         static uint16_t RcLostTimer;
                 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 thrust stick is in the upper position
-                if((PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] > 80) && MotorsOn == 0)
+                if(MotorsOn == 0 && (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] > 80))
+                {
                         // and if the yaw stick is in the leftmost position
                         if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75)
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                         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;
                                         MotorsOn = 1;
                                         SetPointYaw = 0;
                                         Reading_IntegralGyroYaw = 0;
                                         Reading_IntegralGyroPitch = 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;
                                         MotorsOn = 0;
                                         #ifdef USE_KILLAGREG
                                         if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
+                                        {
                 DebugOut.Analog[2]  = Mean_AccPitch;
                 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;
     tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(ThrustMixFraction + 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 = ThrustMixFraction + pd_result + YawMixFraction;          // Mixer
-    MotorValue /= STICK_GAIN;
-        if ((MotorValue < 0)) MotorValue = 0;
-        else if(MotorValue > ParamSet.Trust_Max)            MotorValue = ParamSet.Trust_Max;
-        if (MotorValue < ParamSet.Trust_Min)            MotorValue = ParamSet.Trust_Min;
-        Motor_Front = MotorValue;
- // Motor Rear
-        MotorValue = ThrustMixFraction - pd_result + YawMixFraction;     // Mixer
-        MotorValue /= STICK_GAIN;
-        if ((MotorValue < 0)) MotorValue = 0;
         else if(MotorValue > ParamSet.Trust_Max)            MotorValue = ParamSet.Trust_Max;
-        if (MotorValue < ParamSet.Trust_Min)            MotorValue = ParamSet.Trust_Min;
+    PitchMixFraction = 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)(ThrustMixFraction + 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 =        ThrustMixFraction
+                                        + PitchMixFraction
+                                        + RollMixFraction/2
+                                        - YawMixFraction;         // Mixer
+    MotorValue /= STICK_GAIN;
+        if (MotorValue > ParamSet.Trust_Max)    MotorValue = ParamSet.Trust_Max;
+        else if (MotorValue < ParamSet.Trust_Min)       MotorValue = ParamSet.Trust_Min;
+        Motor_FrontLeft = MotorValue;
+        // Motor FrontRight
+    MotorValue =        ThrustMixFraction
+                                        + PitchMixFraction
+                                        - RollMixFraction/2
+                                        + YawMixFraction;         // Mixer
+    MotorValue /= STICK_GAIN;
+        if (MotorValue > ParamSet.Trust_Max)    MotorValue = ParamSet.Trust_Max;
+        else if (MotorValue < ParamSet.Trust_Min)       MotorValue = ParamSet.Trust_Min;
+        Motor_FrontRight = MotorValue;
+        // Motor RearLeft
+        MotorValue =    ThrustMixFraction
+                                        - PitchMixFraction
+                                        + RollMixFraction/2
+                                        - YawMixFraction;     // Mixer
+    MotorValue /= STICK_GAIN;
+        if (MotorValue > ParamSet.Trust_Max)    MotorValue = ParamSet.Trust_Max;
+        else if (MotorValue < ParamSet.Trust_Min)       MotorValue = ParamSet.Trust_Min;
+        Motor_RearLeft = MotorValue;
+        // Motor RearRight
+        MotorValue =    ThrustMixFraction
+                                        - PitchMixFraction
+                                        - RollMixFraction/2
+                                        + YawMixFraction;     // Mixer
+    MotorValue /= STICK_GAIN;
+        if (MotorValue > ParamSet.Trust_Max)    MotorValue = ParamSet.Trust_Max;
+        else if (MotorValue < ParamSet.Trust_Min)       MotorValue = ParamSet.Trust_Min;
+        Motor_RearRight= MotorValue;
+    // Motor Left
+    MotorValue =        ThrustMixFraction
+                                        + RollMixFraction
+                                        + YawMixFraction;  // Mixer
+    MotorValue /= STICK_GAIN;
+        if (MotorValue > ParamSet.Trust_Max)    MotorValue = ParamSet.Trust_Max;
+        else if (MotorValue < ParamSet.Trust_Min)       MotorValue = ParamSet.Trust_Min;
+    Motor_Left = MotorValue;
+        // Motor Right
+        MotorValue =    ThrustMixFraction
+                                        - RollMixFraction
+                                        - YawMixFraction;  // Mixer
+    MotorValue /= STICK_GAIN;
+        if (MotorValue > ParamSet.Trust_Max)    MotorValue = ParamSet.Trust_Max;
+        else if (MotorValue < ParamSet.Trust_Min)       MotorValue = ParamSet.Trust_Min;
+    Motor_Right = MotorValue;
+#else
+        // Motor Front
+    MotorValue = ThrustMixFraction + PitchMixFraction + YawMixFraction;   // Mixer
+    MotorValue /= STICK_GAIN;
+        if ((MotorValue < 0)) MotorValue = 0;
+        else if(MotorValue > ParamSet.Trust_Max)            MotorValue = ParamSet.Trust_Max;
+        if (MotorValue < ParamSet.Trust_Min)            MotorValue = ParamSet.Trust_Min;
+        Motor_Front = MotorValue;
+        // Motor Rear
+        MotorValue = ThrustMixFraction - PitchMixFraction + YawMixFraction;     // Mixer
+        MotorValue /= STICK_GAIN;
+        if ((MotorValue < 0)) MotorValue = 0;
+        else if(MotorValue > ParamSet.Trust_Max)            MotorValue = ParamSet.Trust_Max;
+        if (MotorValue < ParamSet.Trust_Min)            MotorValue = ParamSet.Trust_Min;
+        Motor_Rear = MotorValue;
     // Motor Left
-    MotorValue = ThrustMixFraction + pd_result - YawMixFraction;  // Mixer
+    MotorValue = ThrustMixFraction + RollMixFraction - YawMixFraction;  // Mixer
     MotorValue /= STICK_GAIN;
         if ((MotorValue < 0)) MotorValue = 0;
         else if(MotorValue > ParamSet.Trust_Max)                MotorValue = ParamSet.Trust_Max;
         if (MotorValue < ParamSet.Trust_Min)            MotorValue = ParamSet.Trust_Min;
     Motor_Left = MotorValue;
- // Motor Right
+        // Motor Right
-        MotorValue = ThrustMixFraction - pd_result - YawMixFraction;  // Mixer
+        MotorValue = ThrustMixFraction - RollMixFraction - YawMixFraction;  // Mixer
         MotorValue /= STICK_GAIN;
         if ((MotorValue < 0)) MotorValue = 0;
         else if(MotorValue > ParamSet.Trust_Max)                MotorValue = ParamSet.Trust_Max;
+        if (MotorValue < ParamSet.Trust_Min)            MotorValue = ParamSet.Trust_Min;
-        if (MotorValue < ParamSet.Trust_Min)            MotorValue = ParamSet.Trust_Min;
+    Motor_Right = MotorValue;

Subversion Repositories FlightCtrl

(root)/branches/V0.69k CRK HexaLotte/fc.c @ 916 - Rev 901 → 903