WebSVN - FlightCtrl - Diff - Rev 790 and 791 - /branches/V0.68d Code Redesign killagreg/fc.c

 // setpoints for motors
 volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left;
 // stick values derived by rc channels readings
 int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickThrust = 0;
-int16_t MaxStickPitch = 0, MaxStickRoll = 0, MaxStickYaw = 0;
+int16_t MaxStickPitch = 0, MaxStickRoll = 0;
 // stick values derived by uart inputs
                 if(Reading_GyroPitch > 200)       Reading_GyroPitch += 4 * (Reading_GyroPitch - 200);
                 else if(Reading_GyroPitch < -200) Reading_GyroPitch += 4 * (Reading_GyroPitch + 200);
                 if(Reading_GyroRoll > 200)        Reading_GyroRoll  += 4 * (Reading_GyroRoll - 200);
                 else if(Reading_GyroRoll < -200)  Reading_GyroRoll  += 4 * (Reading_GyroRoll + 200);
+        }
-        //update poti values by rc-signals
-    if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
-    if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
-    if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
-    if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
-        //limit poti values
-    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
-    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
-    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
-    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
+}
 /************************************************************************/
 /*  Averaging Measurement Readings  for Calibration                     */
         Mean_AccRoll  = ACC_AMPLIFY * (int32_t)AdValueAccRoll;
         Mean_AccTop   = (int32_t)AdValueAccTop;
     // start ADC (enables internal trigger so that the ISR in analog.c
     // updates the readings once)
     ADC_Enable();
-    //update poti values by rc-signals
-    if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
-    if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
-    if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
-    if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
-        //PPM24-Extension
-        if(Poti5 < PPM_in[9] + 110)  Poti5++; else if(Poti5 >  PPM_in[9] + 110 && Poti5) Poti5--;
-        if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--;
-        if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--;
-        if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--;
-        //limit poti values
-    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
-    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
-    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
-    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
-    //PPM24-Extension
-        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;
         TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
         TurnOver180Roll =  (int32_t) ParamSet.AngleTurnOverRoll  * 2500L;
 void ParameterMapping(void)
+{
         if(SenderOkay > 140) // do the mapping of RC-Potis only if the rc-signal is ok
         // else the last updated values are used
+        {
+                 //update poti values by rc-signals
                 #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
                 CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight,0,255);
                 CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100);
                 CHK_POTI(FCParam.Height_P,ParamSet.Height_P,0,100);
                 CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect,0,255);
                         ThrustMixFraction = ParamSet.EmergencyThrust; // set emergency thrust
                         EmergencyLanding = 1; // enable emergency landing
                         // set neutral rc inputs
                         PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] = 0;
                         PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
+                        PPM_diff[ParamSet.ChannelAssignment[CH_YAW]] = 0;
                         PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0;
                         PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
                         PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0;
+                }
                 else MotorsOn = 0; // switch of all motors
                         SumPitch = 0;
                         SumRoll = 0;
                         Reading_IntegralGyroYaw = 0;
                         Reading_IntegralGyroYaw2 = 0;
+                }
+                if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
+                if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
+                if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
+                if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
+                //PPM24-Extension
+                if(Poti5 < PPM_in[9] + 110)  Poti5++; else if(Poti5 >  PPM_in[9] + 110 && Poti5) Poti5--;
+                if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--;
+                if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--;
+                if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--;
+                //limit poti values
+                if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
+                if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
+                if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
+                if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
+                //PPM24-Extension
+                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 thrust stick is in the upper position
                 if((PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] > 80) && MotorsOn == 0)
+                {
                         // and if the yaw stick is in the leftmost position
                         if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75)
                 // direct mapping of yaw and thrust
                 StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
                 StickThrust  = PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] + 120;// shift to positive numbers
-                // update max stick positions for pitch, roll and yaw
+                // update max stick positions for pitch and roll
                 if(abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) > MaxStickPitch)
                         MaxStickPitch = abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]);
                 else MaxStickPitch--;
                 if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll)
-                        MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]);
-                else MaxStickRoll--;
-                if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > MaxStickYaw)
-                        MaxStickYaw = abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]);
+                        MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]);
-                else MaxStickYaw--;
+                else MaxStickRoll--;
                 Gyro_I_Factor = 0.003;
                 Looping_Roll = 0;
                 Looping_Pitch = 0;
                 MaxStickPitch = 0;
                 MaxStickRoll = 0;
-                MaxStickYaw = 0;
+        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Trim Gyro-Integrals to ACC-Signals
                 if((MaxStickPitch > 15) || (MaxStickRoll > 15)) // reduce effect during stick commands
+                {
                         tmp_long  /= 3;
                         tmp_long2 /= 3;
+                }
-                if(MaxStickYaw > 25) // reduce further is yaw stick is active
+                if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further is yaw stick is active
+                {
                         tmp_long  /= 3;
                         tmp_long2 /= 3;
+                }
                         // Calculate deviation of the averaged gyro integral and the averaged acceleration integral
                         IntegralErrorRoll = (int32_t)(MeanIntegralRoll - (int32_t)IntegralAccRoll);
                         CorrectionRoll  = IntegralErrorRoll / ParamSet.GyroAccTrim;
                         AttitudeCorrectionRoll  = CorrectionRoll  / BALANCE_NUMBER;
-                        if((MaxStickPitch > 15) || (MaxStickRoll > 15) || (MaxStickYaw > 25))
+                        if((MaxStickPitch > 15) || (MaxStickRoll > 15) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25))
+                        {
                                 AttitudeCorrectionPitch /= 2;
                                 AttitudeCorrectionRoll /= 2;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  Yawing
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        if(MaxStickYaw > 20) // yaw stick is activated
+        if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 20) // yaw stick is activated
         {   // if not fixed compass course is set update compass course
                 if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) StoreNewCompassCourse = 1;
+        }
         if(Reading_GyroYaw   >  MAX_SENSOR) Reading_GyroYaw   =  MAX_SENSOR;
         if(Reading_GyroYaw   < -MAX_SENSOR) Reading_GyroYaw   = -MAX_SENSOR;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // Height Control
-// The higtht control algorithm reduces the thrust but does not increase the thrust.
+// The height control algorithm reduces the thrust but does not increase the thrust.
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        // If hight control is activated and no emergency landing is active
+        // If height control is activated and no emergency landing is active
         if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) )
+        {
                 int tmp_int;
-                // if hight control is activated by an rc channel
+                // if height control is activated by an rc channel
                 if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH)
                 {       // check if parameter is less than activation threshold
                         if(FCParam.MaxHeight < 50)
+                        {
                                 SetPointHeight = ReadingHeight - 20;  // update SetPoint with current reading
-                                HeightControlActive = 0; // disable hight control
+                                HeightControlActive = 0; // disable height control
                         }
-                        else HeightControlActive = 1; // enable hight control
+                        else HeightControlActive = 1; // enable height control
                 }
-                else // no switchable hight control
+                else // no switchable height control
+                {
                         SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20;
                         HeightControlActive = 1;
                 }
-                // get current hight
+                // get current height
                 h = ReadingHeight;
-                // if current hight is above the setpoint reduce thrust
+                // if current height is above the setpoint reduce thrust
                 if((h > SetPointHeight) && HeightControlActive)
+                {
-                {
+                        // ThrustMixFraction - HightDeviation * P  - HeightChange * D - ACCTop * DACC
-                        // hight difference -> P control part
+                        // height difference -> P control part
                         h = ((h - SetPointHeight) * (int16_t) FCParam.Height_P) / 16;
                         h = ThrustMixFraction - h; // reduce gas
-                        // higth gradient --> D control part
+                        // height gradient --> D control part
                         h -= (HeightD * FCParam.Height_D) / 8;  // D control part
                         // acceleration sensor effect
                         tmp_int = ((Reading_Integral_Top / 512) * (int32_t) FCParam.Height_ACC_Effect) / 32;
                         if(tmp_int > 50) tmp_int = 50;
                         if(tmp_int < -50) tmp_int = -50;
                         h -= tmp_int;
-                        // update hight control thrust
+                        // update height control thrust
                         HeightControlThrust = (HeightControlThrust*15 + h) / 16;
                         // limit thrust reduction
                         if(HeightControlThrust < ParamSet.Height_MinThrust)
+                        {
                                 if(ThrustMixFraction >= ParamSet.Height_MinThrust) HeightControlThrust = ParamSet.Height_MinThrust;
-                                // allows landing also if thrust stick is reduced below min thrust on hight control
+                                // allows landing also if thrust stick is reduced below min thrust on height control
                                 if(ThrustMixFraction < ParamSet.Height_MinThrust) HeightControlThrust = ThrustMixFraction;
+                        }
                         // limit thrust to stick setting
                         if(HeightControlThrust > ThrustMixFraction) HeightControlThrust = ThrustMixFraction;

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/fc.c - Rev 790 → 791