WebSVN - FlightCtrl - Diff - Rev 806 and 812 - /branches/V0.68d Code Redesign killagreg/GPS.c

 #include <inttypes.h>
 #include <stdlib.h>
 #include "fc.h"
 #include "ubx.h"
 #include "mymath.h"
 #include "timer0.h"
 #include "uart.h"
 #include "rc.h"
 #include "eeprom.h"
 #define TSK_IDLE                0
 #define TSK_HOLD                1
 #define TSK_HOME                2
 #define GPS_STICK_SENSE         20              // must be at least in a range where 90% of the trimming does not switch of the GPS function
 #define GPS_STICK_LIMIT         45              // limit of gps stick control to avoid critical flight attitudes
-#define GPS_D_LIMIT_DIST        500             // for position deviations greater than 500cm the D-Part of the PD-Controller is limited
-#define GPS_D_LIMIT                     50              // PD-Controntroller D-Limit.
+#define GPS_POSDEV_INTEGRAL_LIMIT  32000  // limit for the position error integral
-int16_t GPS_Pitch = 0;
-int16_t GPS_Roll = 0;
 int16_t GPS_Pitch = 0, GPS_Roll = 0;
-int32_t GPS_P_Factor = 0, GPS_D_Factor = 0;
+int32_t GPS_P_Factor = 0, GPS_I_Factor = 0, GPS_D_Factor = 0;
 typedef struct
+{
         int32_t Longitude;
         int32_t Latitude;
         int32_t Altitude;
         uint8_t Status;
 } GPS_Pos_t;
 // GPS coordinates for hold position
 GPS_Pos_t HoldPosition  = {0,0,0,INVALID};
 // GPS coordinates for home position
 GPS_Pos_t HomePosition  = {0,0,0,INVALID};
 // ---------------------------------------------------------------------------------
 // checks pitch and roll sticks for manual control
 uint8_t IsManualControlled(void)
+{
         if ( (abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) < GPS_STICK_SENSE) && (abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) < GPS_STICK_SENSE)) return 0;
         else return 1;
+}
 // set home position to current positon
 void GPS_SetHomePosition(void)
+{
         if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D)
+        {
                 HomePosition.Longitude = GPSInfo.longitude;
                 HomePosition.Latitude = GPSInfo.latitude;
                 HomePosition.Altitude = GPSInfo.altitude;
                 HomePosition.Status = VALID;
                 BeepTime = 1000; // signal if new home position was set
+        }
         else
+        {
                 HomePosition.Status = INVALID;
+        }
+}
 // set hold position to current positon
 void GPS_SetHoldPosition(void)
+{
         if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D)
+        {
                 HoldPosition.Longitude = GPSInfo.longitude;
                 HoldPosition.Latitude = GPSInfo.latitude;
                 HoldPosition.Altitude = GPSInfo.altitude;
                 HoldPosition.Status = VALID;
+        }
         else
+        {
                 HoldPosition.Status = INVALID;
+        }
+}
 // clear home position
 void GPS_ClearHomePosition(void)
+{
                 HomePosition.Status = INVALID;
+}
 // disable GPS control sticks
 void GPS_Neutral(void)
+{
         GPS_Pitch = 0;
         GPS_Roll = 0;
+}
 // calculates the GPS control stick values from the deviation to target position
 // if the pointer to the target positin is NULL or is the target position invalid
 // then the P part of the controller is deactivated.
-void GPS_PDController(GPS_Pos_t *pTargetPos)
+void GPS_PIDController(GPS_Pos_t *pTargetPos)
+{
         int32_t coscompass, sincompass;
         int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
-        int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
+        int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;
-        int32_t PD_North = 0, PD_East = 0;
+        int32_t PID_North = 0, PID_East = 0;
+        uint8_t GPS_Stick_Limited = 0;
         static int32_t cos_target_latitude = 1;
+        static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0;
+        static GPS_Pos_t *pLastTargetPos = 0;
         // if GPS data and Compass are ok
         if((GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D) && (CompassHeading >= 0) )
+        {
                 if(pTargetPos != NULL) // if there is a target position
+                {
                         if(pTargetPos->Status != INVALID) // and the position data are valid
-                        { // calculate position deviation from latitude and longitude differences
-                                GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally
-                                GPSPosDev_East  = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally
+                        {
-                                // recalculate the target latitude projection only if the target data are updated
+                                // if the target data are updated or the target pointer has changed
-                                // to save time
-                                if (pTargetPos->Status != PROCESSED)
+                                if ((pTargetPos->Status != PROCESSED) || (pTargetPos != pLastTargetPos) )
+                                {
+                                        // reset error integral
+                                        GPSPosDevIntegral_North = 0;
+                                        GPSPosDevIntegral_East = 0;
+                                        // recalculate latitude projection
                                         cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L));
+                                        // remember last target pointer
+                                        pLastTargetPos = pTargetPos;
+                                        // mark data as processed
                                         pTargetPos->Status = PROCESSED;
+                                }
+                                // calculate position deviation from latitude and longitude differences
+                                GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally
+                                GPSPosDev_East  = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally
                                 // calculate latitude projection
                                 GPSPosDev_East  *= cos_target_latitude;
                                 GPSPosDev_East /= 8192;
-                                DebugOut.Analog[12] = GPSPosDev_North;
+                        DebugOut.Analog[12] = GPSPosDev_North;
                                 DebugOut.Analog[13] = GPSPosDev_East;
                                 //DebugOut.Analog[12] = GPSInfo.velnorth;
                                 //DebugOut.Analog[13] = GPSInfo.veleast;
-                                //Calculate P-components of the controller (negative sign for compensation)
-                                P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
-                                P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
+                        }
-                        else // not valid target position available
+                        else // no valid target position available
+                        {
+                                // reset error
                                 GPSPosDev_North = 0;
                                 GPSPosDev_East = 0;
+                                // reset error integral
+                                GPSPosDevIntegral_North = 0;
+                                GPSPosDevIntegral_East = 0;
+                        }
+                }
-                else // not target position available
+                else // no target position available
+                {
+                        // reset error
                         GPSPosDev_North = 0;
                         GPSPosDev_East = 0;
+                        // reset error integral
+                        GPSPosDevIntegral_North = 0;
+                        GPSPosDevIntegral_East = 0;
+                }
-                //Calculate PD-components of the controller (negative sign for compensation)
+                //Calculate PID-components of the controller (negative sign for compensation)
                 P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
+                P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
+                I_North = -(GPS_I_Factor * GPSPosDevIntegral_North)/2048;
-                P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
+                I_East =  -(GPS_I_Factor * GPSPosDevIntegral_East)/2048;
                 D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
-                D_East =  -(GPS_D_Factor * GPSInfo.veleast)/512;
-                // limit D-Part if position deviation is significant of the target position
-                // this accelerates flying to the target position
-                if( (abs(GPSPosDev_North) > GPS_D_LIMIT_DIST) ||  (abs(GPSPosDev_East) > GPS_D_LIMIT_DIST) )
-                {
-                        if (D_North > GPS_D_LIMIT) D_North = GPS_D_LIMIT;
-                        else if (D_North < -GPS_D_LIMIT) D_North = -GPS_D_LIMIT;
-                        if (D_East > GPS_D_LIMIT)  D_East  = GPS_D_LIMIT;
-                        else if (D_East  < -GPS_D_LIMIT) D_East  = -GPS_D_LIMIT;
                 D_East =  -(GPS_D_Factor * GPSInfo.veleast)/512;
                 // PD-controller
-                PD_North = P_North + D_North;
+                PID_North = P_North + I_North + D_North;
-                PD_East  = P_East  + D_East;
+                PID_East  = P_East  + I_East  + D_East;
                 // GPS to pitch and roll settings
                 // A positive pitch angle moves head downwards (flying forward).
                 // A positive roll angle tilts left side downwards (flying left).
                 // If compass heading is 0 the head of the copter is in north direction.
                 // A positive pitch angle will fly to north and a positive roll angle will fly to west.
                 // In case of a positive north deviation/velocity the
                 // copter should fly to south (negative pitch).
                 // In case of a positive east position deviation and a positive east velocity the
                 // copter should fly to west (positive roll).
                 // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
                 // in the fc.c. Therefore a positive north deviation/velocity should result in a positive
                 // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
                 coscompass = (int32_t)c_cos_8192(CompassHeading);
                 sincompass = (int32_t)c_sin_8192(CompassHeading);
-                GPS_Roll  =    (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
+                GPS_Roll  =    (int16_t)((coscompass * PID_East - sincompass * PID_North) / 8192);
-                GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192);
+                GPS_Pitch = -1*(int16_t)((sincompass * PID_East + coscompass * PID_North) / 8192);
                 // limit GPS controls
+                if (GPS_Pitch > GPS_STICK_LIMIT)
+                {
+                        GPS_Pitch =  GPS_STICK_LIMIT;
+                        GPS_Stick_Limited = 1;
-                if (GPS_Pitch >  GPS_STICK_LIMIT) GPS_Pitch =  GPS_STICK_LIMIT;
+                }
+                else if (GPS_Pitch < -GPS_STICK_LIMIT)
+                {
+                        GPS_Pitch = -GPS_STICK_LIMIT;
+                        GPS_Stick_Limited = 1;
-                else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT;
+                }
+                if (GPS_Roll > GPS_STICK_LIMIT)
+                {
+                        GPS_Roll = GPS_STICK_LIMIT;
+                        GPS_Stick_Limited = 1;
-                if (GPS_Roll  >  GPS_STICK_LIMIT)  GPS_Roll =  GPS_STICK_LIMIT;
+                }
+                else if (GPS_Roll < -GPS_STICK_LIMIT)
+                {
+                        GPS_Roll = -GPS_STICK_LIMIT;
+                        GPS_Stick_Limited = 1;
+                }
+                if(!GPS_Stick_Limited) // prevent further growing if error integrals if control limit is reached
+                {
+                        // calculate position error integrals
+                        GPSPosDevIntegral_North += GPSPosDev_North/4;
+                        if( GPSPosDevIntegral_North > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = GPS_POSDEV_INTEGRAL_LIMIT;
+                        else if (GPSPosDevIntegral_North < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_North = -GPS_POSDEV_INTEGRAL_LIMIT;
+                        GPSPosDevIntegral_East += GPSPosDev_East/4;
+                        if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT;
+                        else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT;
+                }
                 else if (GPS_Roll  < -GPS_STICK_LIMIT)  GPS_Roll = -GPS_STICK_LIMIT;
+        }
         else // invalid GPS data
+        {
                 GPS_Neutral(); // do nothing
+                // reset error integral
+                GPSPosDevIntegral_North = 0;
+                GPSPosDevIntegral_East = 0;
+        }
+}
 void GPS_Main(uint8_t ctrl)
+{
         static uint8_t GPS_Task = TSK_IDLE;
         static uint8_t GPS_P_Delay = 0;
         int16_t satbeep;
         // ctrl enables the gps feature
         if(ctrl < 70) GPS_Task = TSK_IDLE;
         else if (ctrl < 160) GPS_Task = TSK_HOLD;
         else GPS_Task = TSK_HOME; // ctrl >= 160
         switch(GPSInfo.status)
+        {
         case INVALID:  // invalid gps data
                 GPS_Neutral();
                 if(GPS_Task != TSK_IDLE)
+                {
                         BeepTime = 100; // beep if signal is neccesary
+                }
                 break;
         case PROCESSED: // if gps data are already processed do nothing
                 // downcount timeout
                 if(GPSTimeout) GPSTimeout--;
                 // if no new data arrived within timeout set current data invalid
                 // and therefore disable GPS
                 else
+                {
                         GPS_Neutral();
                         GPSInfo.status = INVALID;
+                }
                 break;
         case VALID: // new valid data from gps device
                 // if the gps data quality is good
                 if (GPSInfo.satfix == SATFIX_3D)
+                {
                         switch(GPS_Task) // check what's to do
+                        {
                                 case TSK_IDLE:
                                         // update hold position to current gps position
                                         GPS_SetHoldPosition(); // can get invalid if gps signal is bad
                                         // disable gps control
                                         GPS_Neutral();
                                         break; // eof TSK_IDLE
                                 case TSK_HOLD:
                                         if(HoldPosition.Status != INVALID)
+                                        {
                                                 if( IsManualControlled() ) // MK controlled by user
+                                                {
                                                         // update hold point to current gps position
                                                         GPS_SetHoldPosition();
                                                         // disable gps control
                                                         GPS_Neutral();
                                                         GPS_P_Delay = 0;
+                                                }
                                                 else // GPS control active
+                                                {
                                                         if(GPS_P_Delay<7)
                                                         { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
                                                                 GPS_P_Delay++;
                                                                 GPS_SetHoldPosition();  // update hold point to current gps position
-                                                                GPS_PDController(NULL); // activates only the D-Part
+                                                                GPS_PIDController(NULL); // activates only the D-Part
+                                                        }
-                                                        else GPS_PDController(&HoldPosition);// activates the P&D-Part
+                                                        else GPS_PIDController(&HoldPosition);// activates the P&D-Part
+                                                }
+                                        }
                                         else // invalid Hold Position
                                         {  // try to catch a valid hold position from gps data input
                                                 GPS_SetHoldPosition();
                                                 GPS_Neutral();
+                                        }
                                         break; // eof TSK_HOLD
                                 case TSK_HOME:
                                         if(HomePosition.Status != INVALID)
+                                        {
                                                 // update hold point to current gps position
                                                 // to avoid a flight back if home comming is deactivated
                                                 GPS_SetHoldPosition();
                                                 if( IsManualControlled() ) // MK controlled by user
+                                                {
                                                         GPS_Neutral();
+                                                }
                                                 else // GPS control active
+                                                {
-                                                        GPS_PDController(&HomePosition);
+                                                        GPS_PIDController(&HomePosition);
+                                                }
+                                        }
                                         else // bad home position
+                                        {
                                                 BeepTime = 50; // signal invalid home position
                                                 // try to hold at least the position as a fallback option
                                                 if (HoldPosition.Status != INVALID)
+                                                {
                                                         if( IsManualControlled() ) // MK controlled by user
+                                                        {
                                                                 GPS_Neutral();
+                                                        }
                                                         else // GPS control active
+                                                        {
-                                                                GPS_PDController(&HoldPosition);
+                                                                GPS_PIDController(&HoldPosition);
+                                                        }
+                                                }
                                                 else
                                                 { // try to catch a valid hold position
                                                         GPS_SetHoldPosition();
                                                         GPS_Neutral();
+                                                }
+                                        }
                                         break; // eof TSK_HOME
                                 default: // unhandled task
                                         GPS_Neutral();
                                         break; // eof default
                         } // eof switch GPS_Task
                 } // eof 3D-FIX
                 else // no 3D-SATFIX
                 {       // disable gps control
                         GPS_Neutral();
                         if(GPS_Task != TSK_IDLE)
+                        {
                                 satbeep = 1600 - (int16_t)GPSInfo.satnum * 200; // is zero at 8 sats
                                 if (satbeep < 0) satbeep = 0;
                                 BeepTime = 50 + (uint16_t)satbeep; // max 1650 * 0.1 ms =
+                        }
+                }
                 // set current data as processed to avoid further calculations on the same gps data
                 GPSInfo.status = PROCESSED;
                 break;
         } // eof GPSInfo.status
         DebugOut.Analog[14] = GPS_Pitch;
         DebugOut.Analog[15] = GPS_Roll;
+}

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/GPS.c - Rev 806 → 812