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

 #include <inttypes.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 12
 #define GPS_STICK_LIMIT 45
 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 Easting;
         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
 void GPS_SetHomePosition(void)
+{
                 HomePosition.Northing = HoldPosition.Northing;
                 HomePosition.Easting = HoldPosition.Easting;
                 HomePosition.Status = HoldPosition.Status;
                 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 position deviation
+// calculates the GPS control sticks values from the deviation to target position
-void GPS_PDController(void)
+void GPS_PDController(GPS_Pos_t *TargetPos)
+{
         int32_t coscompass, sincompass;
         int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
         int32_t PD_North = 0, PD_East = 0;
+        if( (TargetPos->Status == VALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D))
+        {
+                GPSPosDev_North = GPSInfo.utmnorth - TargetPos->Northing;
+                GPSPosDev_East  = GPSInfo.utmeast  - TargetPos->Easting;
+                DebugOut.Analog[12] = GPSPosDev_North;
+                DebugOut.Analog[13] = GPSPosDev_East;
-        //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;
+                P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
-        D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
+                D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
-        P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
+                P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
-        D_East =  -(GPS_D_Factor * GPSInfo.veleast)/512;
+                D_East =  -(GPS_D_Factor * GPSInfo.veleast)/512;
-        // PD-controller
+                // PD-controller
-        PD_North = P_North + D_North;
+                PD_North = P_North + D_North;
-        PD_East  = P_East  + D_East;
+                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 pitch angle moves head downwards (flying forward).
-        //A positive roll angle tilts left side downwards (flying left).
+                //A positive roll angle tilts left side downwards (flying left).
-        // If compass heading is 0 the head of the copter is in north direction.
+                // 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.
+                // 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
+                // In case of a positive north deviation/velocity the
-        // copter should fly to south (negative pitch).
+                // copter should fly to south (negative pitch).
-        // In case of a positive east position deviation and a positive east velocity the
+                // In case of a positive east position deviation and a positive east velocity the
-        // copter should fly to west (positive roll).
+                // copter should fly to west (positive roll).
-        // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
+                // 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
+                // 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.
+                // 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
+                // rotation transformation to compass heading to match any copter orientation
-        if (CompassHeading < 0) // no valid compass data
+                if (CompassHeading < 0) // no valid compass data
-        { // disable GPS
+                { // disable GPS
-                GPS_Neutral();
+                        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;
+                if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT;
+                if (GPS_Roll  >  GPS_STICK_LIMIT)  GPS_Roll =  GPS_STICK_LIMIT;
+                if (GPS_Roll  < -GPS_STICK_LIMIT)  GPS_Roll = -GPS_STICK_LIMIT;
+        }
-        else
+        else // invalid input data
+        {
-                coscompass = (int32_t)c_cos_8192(CompassHeading);
+                BeepTime = 50;
-                sincompass = (int32_t)c_sin_8192(CompassHeading);
+                GPS_Neutral();
-                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
-        #define GPS_CTRL_LIMIT 35
-        if (GPS_Pitch >  GPS_CTRL_LIMIT) GPS_Pitch =  GPS_CTRL_LIMIT;
-        if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT;
-        if (GPS_Roll  >  GPS_CTRL_LIMIT)  GPS_Roll =  GPS_CTRL_LIMIT;
-        if (GPS_Roll  < -GPS_CTRL_LIMIT)  GPS_Roll = -GPS_CTRL_LIMIT;
+}
-void GPS_Main(void)
+void GPS_Main(uint8_t ctrl)
+{
         static uint8_t GPS_Task = TSK_IDLE;
         int16_t satbeep;
-        // poti2 enables the gps feature
+        // ctrl enables the gps feature
-        if(Poti2 < 70) GPS_Task = TSK_IDLE;
+        if(ctrl < 70) GPS_Task = TSK_IDLE;
-        else if (Poti2 < 160) GPS_Task = TSK_HOLD;
+        else if (ctrl < 160) GPS_Task = TSK_HOLD;
-        else GPS_Task = TSK_HOME; // Poti2 >= 160
+        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
                 // 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 (GPSInfo.satfix == SATFIX_3D)
+                {
                         switch(GPS_Task) // check what's to do
+                        {
                                 case TSK_IDLE:
                                         // update hold point to current gps position
                                         HoldPosition.Northing = GPSInfo.utmnorth;
                                         HoldPosition.Easting  = GPSInfo.utmeast;
                                         HoldPosition.Status = VALID;
                                         // disable gps control
                                         GPS_Neutral();
                                         break; // eof TSK_IDLE
                                 case TSK_HOLD:
-                                        // if sticks are centered and hold position is valid enable position hold control
-                                        if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (HoldPosition.Status == VALID))
+                                        if(HoldPosition.Status == VALID)
+                                        {
-                                                // Calculate deviation from hold position
+                                                // if sticks are centered (no manual control)
-                                                GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing;
+                                                if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
-                                                GPSPosDev_East  = GPSInfo.utmeast  - HoldPosition.Easting;
-                                                DebugOut.Analog[12] = GPSPosDev_North;
-                                                DebugOut.Analog[13] = GPSPosDev_East;
+                                                {
-                                                GPS_PDController();
+                                                        GPS_PDController(&HoldPosition);
-                                        }
+                                                }
-                                        else // MK controlled by user
+                                                else // MK controlled by user
-                                        {
+                                                {
-                                                // update hold point to current gps position
+                                                        // update hold point to current gps position
-                                                HoldPosition.Northing = GPSInfo.utmnorth;
+                                                        HoldPosition.Northing = GPSInfo.utmnorth;
-                                                HoldPosition.Easting  = GPSInfo.utmeast;
+                                                        HoldPosition.Easting  = GPSInfo.utmeast;
-                                                HoldPosition.Status = VALID;
+                                                        HoldPosition.Status   = GPSInfo.status;
-                                                // disable gps control
+                                                        // disable gps control
-                                                GPS_Neutral();
+                                                        GPS_Neutral();
+                                                }
+                                        }
                                         break; // eof TSK_HOLD
                                 case TSK_HOME:
                                         if(HomePosition.Status == VALID)
+                                        {
                                                 // 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 = VALID;
+                                                HoldPosition.Status   = GPSInfo.status;
-                                                // Calculate deviation from home position
+                                                // if sticks are centered (no manual control)
-                                                GPSPosDev_North = GPSInfo.utmnorth - HomePosition.Northing;
+                                                if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
-                                                GPSPosDev_East  = GPSInfo.utmeast  - HomePosition.Easting;
-                                                DebugOut.Analog[12] = GPSPosDev_North;
+                                                {
-                                                DebugOut.Analog[13] = GPSPosDev_East;
+                                                        GPS_PDController(&HomePosition);
-                                                GPS_PDController();
+                                                }
+                                        }
                                         else // bad home position
+                                        {
-                                                GPS_Neutral();
                                                 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((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID))
+                                                {
+                                                        GPS_PDController(&HoldPosition);
+                                                }
+                                        }
                                         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 = 2048 - (int16_t)GPSInfo.satnum * 256; // is zero at 8 sats
                                 if (satbeep < 0) satbeep = 0;
                                 BeepTime = 50 + (uint16_t)satbeep;
+                        }
+                }
                 // 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 @ 1796 - Rev 752 → 762