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

+{
         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
-// calculates the GPS control sticks values from the deviation to target position
+// then the P part of the controller is deactivated.
-void GPS_PDController(GPS_Pos_t *TargetPos)
+void GPS_PDController(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 PD_North = 0, PD_East = 0;
-        static int32_t cos_target_latitude = 1;
+        static int32_t cos_target_latitude = 1;
         // if GPS data and Compass are ok
-        if( (TargetPos->Status != INVALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D))
-        {
-                // calculate position deviation from latitude and longitude differences
-                GPSPosDev_North = (GPSInfo.latitude - TargetPos->Latitude); // to calculate real cm we would need *111/100 additionally
-                GPSPosDev_East  = (GPSInfo.longitude - TargetPos->Longitude); // to calculate real cm we would need *111/100 additionally
+        if((GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D) && (CompassHeading >= 0) )
-                // recalculate the target latitude projection only if the target data are updated
+        {
                 // to save time
+                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
+                                // to save time
-                if (TargetPos->Status != PROCESSED)
+                                if (pTargetPos->Status != PROCESSED)
-                {
+                                {
+                                        cos_target_latitude = (int32_t)c_cos_8192((int16_t)(pTargetPos->Latitude/10000000L));
+                                        pTargetPos->Status = PROCESSED;
+                                }
+                                // calculate latitude projection
+                                GPSPosDev_East  *= cos_target_latitude;
+                                GPSPosDev_East /= 8192;
+                                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
+                        {
+                                GPSPosDev_North = 0;
+                                GPSPosDev_East = 0;
+                        }
+                }
+                else // not target position available
+                {
-                        cos_target_latitude = (int32_t)c_cos_8192((int16_t)(TargetPos->Latitude/10000000L));
+                        GPSPosDev_North = 0;
-                        TargetPos->Status = PROCESSED;
-                }
-                // calculate latitude projection
-                GPSPosDev_East  *= cos_target_latitude;
-                GPSPosDev_East /= 8192;
-                DebugOut.Analog[12] = GPSPosDev_North;
-                DebugOut.Analog[13] = GPSPosDev_East;
-                //DebugOut.Analog[12] = GPSInfo.velnorth;
+                        GPSPosDev_East = 0;
                 //DebugOut.Analog[13] = GPSInfo.veleast;
                 //Calculate PD-components of the controller (negative sign for compensation)
+                //Calculate PD-components of the controller (negative sign for compensation)
                 P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
-                D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
+                P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
+                D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
-                P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
+                D_East =  -(GPS_D_Factor * GPSInfo.veleast)/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;
+                        if (D_East > GPS_D_LIMIT)  D_East  = GPS_D_LIMIT;
                         else if (D_East  < -GPS_D_LIMIT) D_East  = -GPS_D_LIMIT;
+                }
                 // PD-controller
                 PD_North = P_North + D_North;
                 PD_East  = P_East  + D_East;
                 // GPS to pitch and roll settings
                 // 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).
+                // 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
+                // A positive pitch angle will fly to north and a positive roll angle will fly to west.
-                // copter should fly to south (negative pitch).
+                // In case of a positive north deviation/velocity the
-                // In case of a positive east position deviation and a positive east velocity the
+                // copter should fly to south (negative pitch).
-                // 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.
-                // rotation transformation to compass heading to match any copter orientation
-                if (CompassHeading < 0) // no valid compass data
+                // In case of a positive east position deviation and a positive east velocity the
-                { // disable GPS
+                // copter should fly to west (positive roll).
-                        GPS_Neutral();
+                // 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
-                else
+                // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
-                        coscompass = (int32_t)c_cos_8192(CompassHeading);
+                coscompass = (int32_t)c_cos_8192(CompassHeading);
-                        sincompass = (int32_t)c_sin_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 * PD_East - sincompass * PD_North) / 8192);
-                        GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192);
+                GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192);
                 // limit GPS controls
                 if (GPS_Pitch >  GPS_STICK_LIMIT) GPS_Pitch =  GPS_STICK_LIMIT;
-                else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT;
-                if (GPS_Roll  >  GPS_STICK_LIMIT)  GPS_Roll =  GPS_STICK_LIMIT;
+                else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT;
-                else if (GPS_Roll  < -GPS_STICK_LIMIT)  GPS_Roll = -GPS_STICK_LIMIT;
+                if (GPS_Roll  >  GPS_STICK_LIMIT)  GPS_Roll =  GPS_STICK_LIMIT;
                 else if (GPS_Roll  < -GPS_STICK_LIMIT)  GPS_Roll = -GPS_STICK_LIMIT;
         else // invalid input data
         else // invalid GPS data
                 BeepTime = 50;
+                GPS_Neutral(); // do nothing
                 GPS_Neutral();
+}
                                         if(HoldPosition.Status != INVALID)
+                                        {
                                                 // if sticks are centered (no manual control)
                                                 if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
+                                                {
+                                                        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_PDController(&HoldPosition);
+                                                        else GPS_PDController(&HoldPosition);// activates the P&D-Part
+                                                }
                                                 else // MK controlled by user
+                                                {
                                                         // update hold point to current gps position
                                                         GPS_SetHoldPosition();
                                                         // disable gps control
                                                         GPS_Neutral();
+                                                        GPS_P_Delay = 0;
+                                                }
+                                        }
                                         else // invalid Hold Position
                                         {  // try to catch a valid hold position from gps data input
                                                 GPS_SetHoldPosition();

Subversion Repositories FlightCtrl

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