WebSVN - FlightCtrl - Diff - Rev 1221 and 1222 - /branches/V0.73d Code Redesign killagreg/fc.c

 int16_t NaviAccNick = 0, NaviAccRoll = 0, NaviCntAcc = 0;
 // MK flags
 uint16_t ModelIsFlying = 0;
-uint8_t  MKFlags = 0;
+uint8_t  volatile MKFlags = 0;
+int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L;
-int32_t TurnOver180Nick = 250000L, TurnOver180Roll = 250000L;
+uint8_t GyroPFactor, GyroIFactor;       // the PD factors for the attitude control
-uint8_t GyroPFactor, GyroIFactor; // the PD factors for the attitude control
+uint8_t GyroYawPFactor, GyroYawIFactor; // the PD factors for the yae control
 int16_t Ki = 10300 / 33;
 int16_t Ki = 10300 / 33;
 int16_t  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
 uint8_t LoopingNick = 0, LoopingRoll = 0;
 uint8_t LoopingLeft = 0, LoopingRight = 0, LoopingDown = 0, LoopingTop = 0;
 /************************************************************************/
 /*  Transmit Motor Data via I2C                                         */
 /************************************************************************/
 void SendMotorData(void)
+{
+        uint8_t i;
     if(!(MKFlags & MKFLAG_MOTOR_RUN))
+    {
-                #ifdef USE_QUADRO
-                Motor1 = 0;
-                Motor2 = 0;
-                Motor3 = 0;
-                Motor4 = 0;
-                if(MotorTest[0]) Motor1 = MotorTest[0];
-                if(MotorTest[1]) Motor2 = MotorTest[1];
-                if(MotorTest[2]) Motor3 = MotorTest[2];
-                if(MotorTest[3]) Motor4 = MotorTest[3];
-                #else
-                Motor1 = 0;
-                Motor2 = 0;
-                Motor3 = 0;
-                Motor4 = 0;
-                Motor5 = 0;
-                Motor6 = 0;
-                Motor7 = 0;
-                Motor8 = 0;
-                if(MotorTest[0]) {Motor1 = MotorTest[0]; Motor2 = MotorTest[0];}
-                if(MotorTest[3]) {Motor3 = MotorTest[3]; Motor4 = MotorTest[3];}
-                if(MotorTest[1]) {Motor5 = MotorTest[1]; Motor6 = MotorTest[1];}
-                if(MotorTest[2]) {Motor7 = MotorTest[2]; Motor8 = MotorTest[2];}
-                #endif
                 MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off
+                for(i = 0; i < MAX_MOTORS; i++)
+                {
+                        if(!MotorTest_Active) Motor[i].SetPoint = 0;
+                        else                  Motor[i].SetPoint = MotorTest[i];
+                }
+                if(MotorTest_Active) MotorTest_Active--;
+        }
-        #ifdef USE_QUADRO
-        DebugOut.Analog[12] = Motor1; // Front
+        DebugOut.Analog[12] = Motor[0].SetPoint; // Front
-        DebugOut.Analog[13] = Motor2; // Rear
+        DebugOut.Analog[13] = Motor[1].SetPoint; // Rear
-        DebugOut.Analog[14] = Motor4; // Left
+        DebugOut.Analog[14] = Motor[3].SetPoint; // Left
-        DebugOut.Analog[15] = Motor3; // Right
-        #else // OCTO Motor addresses are counted clockwise starting at the head
-        DebugOut.Analog[12] = (Motor1 + Motor2) / 2;
-        DebugOut.Analog[13] = (Motor5 + Motor6) / 2;
-        DebugOut.Analog[14] = (Motor7 + Motor8) / 2;
-        DebugOut.Analog[15] = (Motor3 + Motor4) / 2;
-        #endif
+        DebugOut.Analog[15] = Motor[2].SetPoint; // Right
     //Start I2C Interrupt Mode
-    twi_state = TWI_STATE_MOTOR_TX;
-    I2C_Start();
+    I2C_Start(TWI_STATE_MOTOR_TX);
-}
+}
 /************************************************************************/
 /*  Map the parameter to poti values                                    */
 /************************************************************************/
 /*  MotorControl                                                        */
 /************************************************************************/
 void MotorControl(void)
+{
-        int16_t MotorValue, h, tmp_int;
+        int16_t h, tmp_int;
         // Mixer Fractions that are combined for Motor Control
         int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction;
         static uint8_t HeightControlActive = 0;
         static int16_t HeightControlGas = 0;
         static int8_t  TimerDebugOut = 0;
         static uint16_t UpdateCompassCourse = 0;
         // high resolution motor values for smoothing of PID motor outputs
-        static int16_t MotorValue1 = 0, MotorValue2 = 0, MotorValue3 = 0, MotorValue4 = 0;
-        #ifndef USE_QUADRO
-        static int16_t MotorValue5 = 0, MotorValue6 = 0, MotorValue7 = 0, MotorValue8 = 0;
+        static int16_t MotorValue[MAX_MOTORS];
-        #endif
+        uint8_t i;
         Mean();
         GRN_ON;
                 StickGas  = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;// shift to positive numbers
                 // update gyro control loop factors
                 GyroPFactor = FCParam.GyroP + 10;
+                GyroIFactor = FCParam.GyroI;
+                GyroYawPFactor = FCParam.GyroP + 10;
-                GyroIFactor = FCParam.GyroI;
+                GyroYawIFactor = FCParam.GyroI;
+        {
                 StickYaw = 0;
                 StickNick = 0;
                 StickRoll = 0;
                 GyroPFactor  = 90;
-                GyroIFactor = 120;
+                GyroIFactor  = 120;
+                GyroYawPFactor = 90;
+                GyroYawIFactor = 120;
                 LoopingRoll = 0;
                 LoopingNick = 0;
                 MaxStickNick = 0;
                 MaxStickRoll = 0;
+        }
                 DebugOut.Analog[19] = CompassCalState;
         //      DebugOut.Analog[24] = GyroNick/2;
         //      DebugOut.Analog[25] = GyroRoll/2;
                 DebugOut.Analog[27] = (int16_t)FCParam.KalmanMaxDrift;
         //      DebugOut.Analog[28] = (int16_t)FCParam.KalmanMaxFusion;
-        //      DebugOut.Analog[29] = (int16_t)FCParam.KalmanK;
                 DebugOut.Analog[30] = GPSStickNick;
                 DebugOut.Analog[31] = GPSStickRoll;
+        }
+        {
                 PPartRoll = 0;
+        }
         PDPartRoll = PPartRoll + (int32_t)((int32_t)GyroRoll * GyroPFactor +  (int32_t)TrimRoll * 128L) / (256L / STICK_GAIN); // +D-Part
-        // octo has a double yaw momentum because of the doubled motor number
-        // therefore double D-Part and halfen P-Part for the same result
-        #ifdef USE_OCTO
-        PDPartYaw =  (int32_t)(GyroYaw * 4 * (int32_t)GyroPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroIFactor) / (4 * (44000 / STICK_GAIN));
-        #else
-        PDPartYaw =  (int32_t)(GyroYaw * 2 * (int32_t)GyroPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroIFactor) / (2 * (44000 / STICK_GAIN));
-        #endif
+        PDPartYaw =  (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / STICK_GAIN));
         //DebugOut.Analog[21] = PDPartNick;
         CHECK_MIN_MAX(PDPartNick, -SENSOR_LIMIT, SENSOR_LIMIT);
         CHECK_MIN_MAX(PDPartRoll, -SENSOR_LIMIT, SENSOR_LIMIT);
         CHECK_MIN_MAX(PDPartYaw,  -SENSOR_LIMIT, SENSOR_LIMIT);
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// all BL-Ctrl connected?
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+        if(MissingMotor)
+        {
+                // if we are in the lift off condition
+                if( (ModelIsFlying > 1) && (ModelIsFlying < 50) && (GasMixFraction > 0) )
+                ModelIsFlying = 1; // keep within lift off condition
+                GasMixFraction = ParamSet.GasMin; // reduce gas to min to avoid lift of
+        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Height Control
 // The height control algorithm reduces the gas but does not increase the gas.
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         tmp_int = (int32_t)((int32_t)FCParam.DynamicStability * (int32_t)(GasMixFraction + abs(YawMixFraction) / 2)) / 64;
         CHECK_MIN_MAX(NickMixFraction, -tmp_int, tmp_int);
         CHECK_MIN_MAX(RollMixFraction, -tmp_int, tmp_int);
-#ifdef USE_QUADRO
-        // QuadroKopter Mixer
-        // Motor Front (++)
-    MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;
-    MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
-    MotorValue = MotorValue1 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-        Motor1 = MotorValue;
-        // Motor Rear (-+)
-        MotorValue = GasMixFraction - NickMixFraction + YawMixFraction;
-        MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
-        MotorValue = MotorValue2 / STICK_GAIN;
-        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-        Motor2 = MotorValue;
-        // Motor Right (--)
-        MotorValue = GasMixFraction - RollMixFraction - YawMixFraction;
-        MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
-        MotorValue = MotorValue3 / STICK_GAIN;
-        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-    Motor3 = MotorValue;
-        // Motor Left (+-)
-    MotorValue = GasMixFraction + RollMixFraction - YawMixFraction;
-    MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
-    MotorValue = MotorValue4 / STICK_GAIN;
-        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-    Motor4 = MotorValue;
-#endif
-#ifdef USE_OCTO
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        // OctoKopter Mixer
-        // Motor 1 (+++)
+// Universal Mixer
-    MotorValue = GasMixFraction + NickMixFraction + RollMixFraction + YawMixFraction;
-    MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
-    MotorValue = MotorValue1 / STICK_GAIN;
-        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-        Motor1= MotorValue;
-        // Motor 2 (+--)
-        MotorValue = GasMixFraction + NickMixFraction - RollMixFraction - YawMixFraction;
-        MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
-    MotorValue = MotorValue2 / STICK_GAIN;
-        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-        Motor2 = MotorValue;
-        // Motor 3 (+-+)
-        MotorValue = GasMixFraction + NickMixFraction - RollMixFraction + YawMixFraction;
-        MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
-    MotorValue = MotorValue3 / STICK_GAIN;
-        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-        Motor3 = MotorValue;
-        // Motor 4 (---)
-        MotorValue = GasMixFraction - NickMixFraction - RollMixFraction - YawMixFraction;
-        MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
-    MotorValue = MotorValue4 / STICK_GAIN;
-        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-        Motor4 = MotorValue;
-        // Motor 5 (--+)
-        MotorValue = GasMixFraction - NickMixFraction - RollMixFraction + YawMixFraction;
-        MotorValue5 = MotorSmoothing(MotorValue, MotorValue5);
-    MotorValue = MotorValue5 / STICK_GAIN;
-        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    Motor5 = MotorValue;
+        for(i = 0; i < MAX_MOTORS; i++)
+        {
-        // Motor 6 (-+-)
-        MotorValue = GasMixFraction - NickMixFraction + RollMixFraction - YawMixFraction;
-        MotorValue6 = MotorSmoothing(MotorValue, MotorValue6);
-    MotorValue = MotorValue6 / STICK_GAIN;
-        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-    Motor6 = MotorValue;
-        // Motor7 (-++)
-        MotorValue = GasMixFraction - NickMixFraction + RollMixFraction + YawMixFraction;
-        MotorValue7 = MotorSmoothing(MotorValue, MotorValue7);
-    MotorValue = MotorValue7 / STICK_GAIN;
-        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-    Motor7 = MotorValue;
-    // Motor8 (++-)
-        MotorValue = GasMixFraction + NickMixFraction + RollMixFraction - YawMixFraction;
-        MotorValue8 = MotorSmoothing(MotorValue, MotorValue8);
-    MotorValue = MotorValue8 / STICK_GAIN;
+                int16_t tmp;
-        CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-    Motor8 = MotorValue;
-#endif
-#ifdef USE_OCTO2
-        // Octokopter Mixer alternativ setup
+                if(Mixer.Motor[i][MIX_GAS] > 0) // if gas then mixer
+                {
-    MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;
-    MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
-    MotorValue = MotorValue1 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-        Motor1 = MotorValue;
-    MotorValue = GasMixFraction + NickMixFraction - RollMixFraction - YawMixFraction;
-    MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
-    MotorValue = MotorValue2 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-        Motor2 = MotorValue;
-        MotorValue = GasMixFraction - RollMixFraction + YawMixFraction;
-    MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
-    MotorValue = MotorValue3 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-    Motor3 = MotorValue;
-        MotorValue = GasMixFraction - NickMixFraction - RollMixFraction - YawMixFraction;
-    MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
-    MotorValue = MotorValue4 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-    Motor4 = MotorValue;
-        MotorValue = GasMixFraction - RollMixFraction + YawMixFraction;
-    MotorValue5 = MotorSmoothing(MotorValue, MotorValue5);
-    MotorValue = MotorValue5 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-        Motor5 = MotorValue;
+                        tmp =  ((int32_t)GasMixFraction  * Mixer.Motor[i][MIX_GAS] ) / 64L;
-        MotorValue = GasMixFraction - NickMixFraction + RollMixFraction - YawMixFraction;
-    MotorValue6 = MotorSmoothing(MotorValue, MotorValue6);
-    MotorValue = MotorValue6 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-        Motor6 = MotorValue;
-    MotorValue = GasMixFraction + RollMixFraction + YawMixFraction;
-    MotorValue7 = MotorSmoothing(MotorValue, MotorValue7);
-    MotorValue = MotorValue7 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-    Motor7 = MotorValue;
-    MotorValue = GasMixFraction + NickMixFraction + RollMixFraction - YawMixFraction;
-    MotorValue8 = MotorSmoothing(MotorValue, MotorValue8);
-    MotorValue = MotorValue8 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-    Motor8 = MotorValue;
-#endif
-#ifdef USE_OCTO3
-        // Octokopter Mixer alternativ setup
+                        tmp += ((int32_t)NickMixFraction * Mixer.Motor[i][MIX_NICK]) / 64L;
-    MotorValue = GasMixFraction + NickMixFraction + YawMixFraction;
-    MotorValue1 = MotorSmoothing(MotorValue, MotorValue1);
-    MotorValue = MotorValue1 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-        Motor1 = MotorValue;
+                        tmp += ((int32_t)RollMixFraction * Mixer.Motor[i][MIX_ROLL]) / 64L;
-    MotorValue = GasMixFraction + NickMixFraction - YawMixFraction;
-    MotorValue2 = MotorSmoothing(MotorValue, MotorValue2);
-    MotorValue = MotorValue2 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-        Motor2 = MotorValue;
-        MotorValue = GasMixFraction - RollMixFraction + YawMixFraction;
-    MotorValue3 = MotorSmoothing(MotorValue, MotorValue3);
-    MotorValue = MotorValue3 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-    Motor3 = MotorValue;
-        MotorValue = GasMixFraction - RollMixFraction - YawMixFraction;
-    MotorValue4 = MotorSmoothing(MotorValue, MotorValue4);
-    MotorValue = MotorValue4 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-    Motor4 = MotorValue;
-        MotorValue = GasMixFraction - NickMixFraction + YawMixFraction;
-    MotorValue5 = MotorSmoothing(MotorValue, MotorValue5);
-    MotorValue = MotorValue5 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-        Motor5 = MotorValue;
-        MotorValue = GasMixFraction - NickMixFraction - YawMixFraction;
+                        tmp += ((int32_t)YawMixFraction  * Mixer.Motor[i][MIX_YAW] ) / 64L;
-    MotorValue6 = MotorSmoothing(MotorValue, MotorValue6);
+                        MotorValue[i] = MotorSmoothing(tmp, MotorValue[i]);  // Spike Filter
-    MotorValue = MotorValue6 / STICK_GAIN;
+                        tmp = MotorValue[i] / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+                        CHECK_MIN_MAX(tmp, ParamSet.GasMin, ParamSet.GasMax);
-        Motor6 = MotorValue;
-    MotorValue = GasMixFraction + RollMixFraction + YawMixFraction;
-    MotorValue7 = MotorSmoothing(MotorValue, MotorValue7);
-    MotorValue = MotorValue7 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
-    Motor7 = MotorValue;
+                        Motor[i].SetPoint = tmp;
-    MotorValue = GasMixFraction + RollMixFraction - YawMixFraction;
-    MotorValue8 = MotorSmoothing(MotorValue, MotorValue8);
-    MotorValue = MotorValue8 / STICK_GAIN;
-    CHECK_MIN_MAX(MotorValue, ParamSet.GasMin, ParamSet.GasMax);
+                }
-    Motor8 = MotorValue;
-#endif
+                else Motor[i].SetPoint = 0;
+        }

Subversion Repositories FlightCtrl

(root)/branches/V0.73d Code Redesign killagreg/fc.c - Rev 1221 → 1222