WebSVN - FlightCtrl - Diff - Rev 844 and 898 - /branches/V0.68d 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
-volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left;
+volatile uint8_t Motor_FrontLeft, Motor_FrontRight, Motor_RearLeft, Motor_RearRight, Motor_Right, Motor_Left;
 // stick values derived by rc channels readings
 /************************************************************************/
 void SendMotorData(void)
+{
     if(MOTOR_OFF || !MotorsOn)
+    {
-        Motor_Rear = 0;
+        Motor_RearLeft = 0;
+                Motor_RearRight = 0;
-        Motor_Front = 0;
+        Motor_FrontLeft = 0;
+                Motor_FrontRight = 0;
         Motor_Right = 0;
         Motor_Left = 0;
-        if(MotorTest[0]) Motor_Front = MotorTest[0];
+        if(MotorTest[0]) Motor_FrontLeft = Motor_FrontRight = MotorTest[0];
-        if(MotorTest[1]) Motor_Rear  = MotorTest[1];
+        if(MotorTest[1]) Motor_RearLeft  = Motor_RearRight  = MotorTest[1];
         if(MotorTest[2]) Motor_Left  = MotorTest[2];
         if(MotorTest[3]) Motor_Right = MotorTest[3];
+     }
-    //DebugOut.Analog[12] = Motor_Front;
+    DebugOut.Analog[12] = Motor_FrontLeft;
+    DebugOut.Analog[13] = Motor_RearRight;
+    DebugOut.Analog[14] = Motor_FrontRight;
-    //DebugOut.Analog[13] = Motor_Rear;
+    DebugOut.Analog[15] = Motor_RearLeft;
-    //DebugOut.Analog[14] = Motor_Left;
+    DebugOut.Analog[16] = Motor_Left;
-    //DebugOut.Analog[15] = Motor_Right;
+    DebugOut.Analog[17] = Motor_Right;
     //Start I2C Interrupt Mode
     twi_state = 0;
     motor = 0;
     I2C_Start();
 /*  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;
                         // deviation of gyro pitch integral (IntegralPitch is corrected by averaged acc sensor)
                         IntegralErrorRoll = IntegralRoll2 - IntegralRoll;
                         Reading_IntegralGyroRoll2 -= IntegralErrorRoll;
-                        DebugOut.Analog[17] = IntegralAccPitch / 26;
+                        // DebugOut.Analog[17] = IntegralAccPitch / 26;
                         DebugOut.Analog[18] = IntegralAccRoll / 26;
                         DebugOut.Analog[19] = IntegralErrorPitch;// / 26;
                         DebugOut.Analog[20] = IntegralErrorRoll;// / 26;
                 DebugOut.Analog[6]  = (Reading_Integral_Top / 512);
                 DebugOut.Analog[8]  = CompassHeading;
                 DebugOut.Analog[9]  = UBat;
                 DebugOut.Analog[10] = RC_Quality;
                 //DebugOut.Analog[11] = RC_Quality;
-                DebugOut.Analog[16] = Mean_AccTop;
+                //DebugOut.Analog[16] = Mean_AccTop;
                 /*    DebugOut.Analog[16] = motor_rx[0];
                 DebugOut.Analog[17] = motor_rx[1];
                 DebugOut.Analog[18] = motor_rx[2];
         // limit thrust to parameter setting
         if(ThrustMixFraction > ParamSet.Trust_Max - 20) ThrustMixFraction = ParamSet.Trust_Max - 20;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Mixer and PI-Controller
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         DebugOut.Analog[7] = ThrustMixFraction;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Yaw-Fraction
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     YawMixFraction = Reading_GyroYaw - SetPointYaw;     // yaw controller
     if(YawMixFraction > (ThrustMixFraction / 2)) YawMixFraction = ThrustMixFraction / 2;
     if(YawMixFraction < -(ThrustMixFraction / 2)) YawMixFraction = -(ThrustMixFraction / 2);
     if(YawMixFraction > ((ParamSet.Trust_Max - ThrustMixFraction))) YawMixFraction = ((ParamSet.Trust_Max - ThrustMixFraction));
     if(YawMixFraction < -((ParamSet.Trust_Max - ThrustMixFraction))) YawMixFraction = -((ParamSet.Trust_Max - ThrustMixFraction));
     if(ThrustMixFraction < 20) YawMixFraction = 0;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Pitch-Axis
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     DiffPitch = Reading_GyroPitch - (StickPitch - GPS_Pitch);   // get difference
     if(Gyro_I_Factor) SumPitch += IntegralPitch * Gyro_I_Factor - (StickPitch - GPS_Pitch); // I-part for attitude control
     pd_result = DiffPitch + Ki * SumPitch; // PI-controller for pitch
     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
-    MotorValue = ThrustMixFraction + pd_result + YawMixFraction;          // Mixer
-        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;
+        PitchMixFraction = pd_result;
- // Motor Rear
-        MotorValue = ThrustMixFraction - pd_result + YawMixFraction;     // Mixer
-        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;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Roll-Axis
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         DiffRoll = Reading_GyroRoll - (StickRoll  - GPS_Roll);  // get difference
     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
+        // Motor FrontLeft
+    MotorValue =        ThrustMixFraction
+                                        + PitchMixFraction
+                                        + RollMixFraction/2
+                                        - YawMixFraction;         // Mixer
+        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_FrontLeft = MotorValue;
+        // Motor FrontRight
+    MotorValue =        ThrustMixFraction
+                                        + PitchMixFraction
+                                        - RollMixFraction/2
+                                        + YawMixFraction;         // Mixer
+        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_FrontRight = MotorValue;
+        // Motor RearLeft
+        MotorValue =    ThrustMixFraction
+                                        - PitchMixFraction
+                                        + RollMixFraction/2
+                                        - YawMixFraction;     // Mixer
+        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_RearLeft = MotorValue;
+        // Motor RearRight
+        MotorValue =    ThrustMixFraction
+                                        - PitchMixFraction
+                                        - RollMixFraction/2
+                                        + YawMixFraction;     // Mixer
+        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_RearRight= MotorValue;
     // Motor Left
+    MotorValue =        ThrustMixFraction
+                                        + RollMixFraction
-    MotorValue = ThrustMixFraction + pd_result - YawMixFraction;  // Mixer
+                                        + YawMixFraction;  // Mixer
         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
+        MotorValue =    ThrustMixFraction
- // Motor Right
+                                        - RollMixFraction
-        MotorValue = ThrustMixFraction - pd_result - YawMixFraction;  // Mixer
+                                        - YawMixFraction;  // Mixer
         if ((MotorValue < 0)) MotorValue = 0;
         else if(MotorValue > ParamSet.Trust_Max)                MotorValue = ParamSet.Trust_Max;
         if (MotorValue < ParamSet.Trust_Min)            MotorValue = ParamSet.Trust_Min;

Subversion Repositories FlightCtrl

(root)/branches/V0.68d CRK HexaLotte/fc.c - Rev 844 → 898