WebSVN - FlightCtrl - Diff - Rev 779 and 781 - /branches/V0.68d CRK HexaLotte/GPS.c

 #include <inttypes.h>
 #include <stdlib.h>
 #include "fc.h"
 #include "ubx.h"
 #include "mymath.h"
 #include "timer0.h"
 #include "uart.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 grater than 500 cm (20m) the D-Part of the PD-Controller is limited
 #define GPS_D_LIMIT                     50              // PD-Controntroller D-Limit.
 int16_t GPS_Pitch = 0;
 int16_t GPS_Roll = 0;
 int32_t GPS_P_Factor = 0, GPS_D_Factor = 0;
-int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
 typedef struct
+{
-        int32_t Northing;
+        int32_t Longitude;
-        int32_t Easting;
+        int32_t Latitude;
         uint8_t Status;
 } GPS_Pos_t;
 // GPS coordinates for hold position
 GPS_Pos_t HoldPosition  = {0,0, INVALID};
 // GPS coordinates for home position
 GPS_Pos_t HomePosition  = {0,0, INVALID};
 // ---------------------------------------------------------------------------------
-// set home position to current hold positon
+// set home position to current positon
 void GPS_SetHomePosition(void)
+{
+        if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D)
-{
+        {
-                HomePosition.Northing = HoldPosition.Northing;
+                HomePosition.Longitude = GPSInfo.longitude;
-                HomePosition.Easting = HoldPosition.Easting;
+                HomePosition.Latitude = GPSInfo.latitude;
-                HomePosition.Status = HoldPosition.Status;
+                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.Status = VALID;
+        }
+        else
+        {
+                HoldPosition.Status = INVALID;
                 if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set
+}
 // 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 sticks values from the deviation to target position
 void GPS_PDController(GPS_Pos_t *TargetPos)
+{
         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;
-        if( (TargetPos->Status == VALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D))
+        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
+                // recalculate the target latitude projection only if the target data are updated
+                // to save time
+                if (TargetPos->Status != PROCESSED)
+                {
+                        cos_target_latitude = (int32_t)c_cos_8192((int16_t)(TargetPos->Latitude/10000000L));
+                        TargetPos->Status = PROCESSED;
+                }
-                GPSPosDev_North = GPSInfo.utmnorth - TargetPos->Northing;
+                // calculate latitude projection
+                GPSPosDev_East  *= cos_target_latitude;
-                GPSPosDev_East  = GPSInfo.utmeast  - TargetPos->Easting;
+                GPSPosDev_East /= 8192;
                 DebugOut.Analog[12] = GPSPosDev_North;
                 DebugOut.Analog[13] = GPSPosDev_East;
                 //DebugOut.Analog[12] = GPSInfo.velnorth;
                 //DebugOut.Analog[13] = GPSInfo.veleast;
                 //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_East =  -(GPS_D_Factor * GPSInfo.veleast)/512;
                 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;
+                }
                 // PD-controller
                 PD_North = P_North + D_North;
                 PD_East  = P_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.
                 // rotation transformation to compass heading to match any copter orientation
                 if (CompassHeading < 0) // no valid compass data
                 { // disable GPS
                         GPS_Neutral();
+                }
                 else
+                {
                         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_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_Roll  < -GPS_STICK_LIMIT)  GPS_Roll = -GPS_STICK_LIMIT;
+        }
         else // invalid input data
+        {
                 BeepTime = 50;
                 GPS_Neutral();
+        }
+}
 void GPS_Main(uint8_t ctrl)
+{
         static uint8_t GPS_Task = TSK_IDLE;
         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
+        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 sufficient
+                // 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 point to current gps position
+                                        // update hold position to current gps position
-                                        HoldPosition.Northing = GPSInfo.utmnorth;
-                                        HoldPosition.Easting  = GPSInfo.utmeast;
+                                        GPS_SetHoldPosition(); // can get invalid if gps signal is bad
-                                        HoldPosition.Status = VALID;
                                         // disable gps control
                                         GPS_Neutral();
                                         break; // eof TSK_IDLE
                                 case TSK_HOLD:
-                                        if(HoldPosition.Status == VALID)
+                                        if(HoldPosition.Status != INVALID)
+                                        {
                                                 // if sticks are centered (no manual control)
                                                 if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
+                                                {
                                                         GPS_PDController(&HoldPosition);
+                                                }
                                                 else // MK controlled by user
+                                                {
                                                         // update hold point to current gps position
-                                                        HoldPosition.Northing = GPSInfo.utmnorth;
-                                                        HoldPosition.Easting  = GPSInfo.utmeast;
-                                                        HoldPosition.Status   = GPSInfo.status;
+                                                        GPS_SetHoldPosition();
                                                         // disable gps control
                                                         GPS_Neutral();
+                                                }
+                                        }
+                                        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 == VALID)
+                                        if(HomePosition.Status != INVALID)
+                                        {
                                                 // update hold point to current gps position
                                                 // to avoid a flight back if home comming is deactivated
-                                                HoldPosition.Northing = GPSInfo.utmnorth;
-                                                HoldPosition.Easting  = GPSInfo.utmeast;
-                                                HoldPosition.Status   = GPSInfo.status;
+                                                GPS_SetHoldPosition();
                                                 // if sticks are centered (no manual control)
                                                 if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
+                                                {
                                                         GPS_PDController(&HomePosition);
+                                                }
                                                 else // manual control
+                                                {
                                                         GPS_Neutral();
+                                                }
+                                        }
                                         else // bad home position
+                                        {
                                                 BeepTime = 50; // signal invalid home position
                                                 // try to hold at least the position as a fallback option
-                                                // if sticks are centered (no manual control)
+                                                if (HoldPosition.Status != INVALID)
-                                                if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID))
+                                                {
+                                                        // if sticks are centered (no manual control)
+                                                        if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
+                                                        {
-                                                        GPS_PDController(&HoldPosition);
+                                                                GPS_PDController(&HoldPosition);
+                                                        }
+                                                        else // manual control or no reference position
+                                                        {
+                                                                GPS_Neutral();
+                                                        }
+                                                }
+                                                else
-                                                else // manual control or no rteference position
+                                                { // 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 = 1800 - (int16_t)GPSInfo.satnum * 225; // is zero at 8 sats
                                 if (satbeep < 0) satbeep = 0;
                                 BeepTime = 50 + (uint16_t)satbeep; // max 1850 * 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 CRK HexaLotte/GPS.c @ 1104 - Rev 779 → 781