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

 #include "mm3.h"
 #endif
 #ifdef USE_CMPS03
 #include "cmps03.h"
 #endif
+#include "led.h"
 volatile uint16_t I2CTimeout = 100;
 // gyro readings
 volatile int16_t Reading_GyroPitch, Reading_GyroRoll, Reading_GyroYaw;
 float Gyro_P_Factor;
 float Gyro_I_Factor;
 volatile int16_t  DiffPitch, DiffRoll;
-int16_t  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
+int16_t  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
     //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;
 /************************************************************************/
 /*  Maps the parameter to poti values                                   */
 /************************************************************************/
 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
+        {
-        #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;}
+                #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.MaxHeight,ParamSet.MaxHeight,0,255);
-        CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100);
+                CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100);
-        CHK_POTI(FCParam.Height_P,ParamSet.Height_P,0,100);
+                CHK_POTI(FCParam.Height_P,ParamSet.Height_P,0,100);
-        CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect,0,255);
+                CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect,0,255);
-        CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255);
+                CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255);
-        CHK_POTI(FCParam.LoopThrustLimit,ParamSet.LoopThrustLimit,0,255);
+                CHK_POTI(FCParam.LoopThrustLimit,ParamSet.LoopThrustLimit,0,255);
-        CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255);
+                CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255);
-        CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255);
+                CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255);
-        CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255);
+                CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255);
-        Ki = (float) FCParam.I_Factor * FACTOR_I;
+                Ki = (float) FCParam.I_Factor * FACTOR_I;
+        }
+}
 /************************************************************************/
-     static int8_t TimerDebugOut = 0;
+        static int8_t TimerDebugOut = 0;
-     static int8_t StoreNewCompassCourse = 0;
+        static int8_t StoreNewCompassCourse = 0;
-     static int32_t CorrectionPitch, CorrectionRoll;
+        static int32_t CorrectionPitch, CorrectionRoll;
-        Mean();
         Mean();
+        GRN_ON;
     GRN_ON;
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // determine thrust value
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         ThrustMixFraction = StickThrust;
                         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
-        }
+        } // eof SenderOkay < 100
         else
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // RC-signal is good
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
         if(SenderOkay > 140)
                                 if(++delay_stopmotors > 200)  // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
+                                {
                                         delay_stopmotors = 200; // do not repeat if once executed
                                         Modell_Is_Flying = 0;
                                         MotorsOn = 0;
+                                        GPS_ClearHomePosition();
+                                }
+                        }
                         else delay_stopmotors = 0; // reset delay timer if sticks are not in this position
+                }
+                        // remapping of paameters only if the signal rc-sigbnal conditions are good
-                }
+        } // eof SenderOkay > 140
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // new values from RC
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-                if(!NewPpmData-- || EmergencyLanding) // NewData = 0 means new data from RC
+        if(!NewPpmData-- || EmergencyLanding) // NewData = 0 means new data from RC
+        {
-                        int tmp_int;
+                int tmp_int;
-                        ParameterMapping(); // remapping params (online poti replacement)
+                ParameterMapping(); // remapping params (online poti replacement)
-                        // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
+                // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
-                        StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4;
+                StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4;
-                        StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D;
+                StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D;
-                        StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4;
+                StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4;
-                        StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D;
+                StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D;
-                if(Looping_Roll || Looping_Pitch)
+        if(Looping_Roll || Looping_Pitch)
+        {
                 if(ThrustMixFraction > ParamSet.LoopThrustLimit) ThrustMixFraction = ParamSet.LoopThrustLimit;
+        }
+//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+//+ LED Control on J16/J17
+//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+        LED_OffTime = FCParam.UserParam7;
+        LED_OnTime = FCParam.UserParam8;
+        LED_Update();
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // in case of emergency landing
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-                // set all inputs to save values
+        // set all inputs to save values
+                }
+        }
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //  GPS
 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-                if((ParamSet.GlobalConfig & CFG_GPS_ACTIVE) && !EmergencyLanding)
+        if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE)
+        {
-                        GPS_P_Factor = FCParam.UserParam5;
+                GPS_P_Factor = FCParam.UserParam5;
-                        GPS_D_Factor = FCParam.UserParam6;
+                GPS_D_Factor = FCParam.UserParam6;
-                        GPS_Main(); // updates GPS_Pitch and GPS_Roll on new GPS data
+                if(EmergencyLanding) GPS_Main(230); // enables Comming Home
+                else GPS_Main(Poti3);
+        }
-                else
+        else
+        {
-                        GPS_Neutral();
+                GPS_Neutral();
+        }

Subversion Repositories FlightCtrl

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