Subversion Repositories FlightCtrl

#include <inttypes.h>

#include "fc.h"
#include "ubx.h"
#include "mymath.h"

int16_t GPS_Pitch = 0;
int16_t GPS_Roll = 0;

uint8_t GPS_P_Factor = 0;
uint8_t GPS_D_Factor = 0;

typedef struct
{
        int32_t Northing;
        int32_t Easting;
        uint8_t Status;

} GPS_Pos_t;

// GPS coordinates for hold position
GPS_Pos_t GPSHoldPoint  = {0,0, INVALID};
// GPS coordinates for flying home
GPS_Pos_t GPSHomePoint = {0,0, INVALID};;



// ---------------------------------------------------------------------------------


void GPS_Main(void)
{
        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;
        int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;

        // poti2 enables the gps feature
        if(Poti2 > 70) // run GPS function only if Poti 2 is larger than 70 (switch on)
        {
                switch(GPSInfo.status)
                {
                case INVALID:  // invalid gps data
                        GPS_Pitch = 0;
                        GPS_Roll = 0;
                        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_Pitch = 0;
                                GPS_Roll = 0;
                                GPSInfo.status = INVALID;
                        }
                        break;
                case VALID: // new valid data from gps device
                        // if the gps data quality is sufficient
                        if (GPSInfo.satfix == SATFIX_3D)
                        {
                                // if sticks are centered  and hold position is valid enable position hold
                                if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (GPSHoldPoint.Status == VALID))
                                {
                                        coscompass = (int32_t)c_cos_8192(CompassHeading);
                                        sincompass = (int32_t)c_sin_8192(CompassHeading);

                                        // Calculate deviation from hold position
                                        GPSPosDev_North = GPSInfo.utmnorth - GPSHoldPoint.Northing;
                                        GPSPosDev_East  = GPSInfo.utmeast  - GPSHoldPoint.Easting;

                                        //Calculate PD-components of the controller
                                        P_North = (GPS_P_Factor * GPSPosDev_North)/2048;
                                        D_North = (GPS_D_Factor * GPSInfo.velnorth)/255;
                                        P_East =  (GPS_P_Factor * GPSPosDev_East)/2048;
                                        D_East =  (GPS_D_Factor * GPSInfo.veleast)/255;

                                        // PD-controller
                                        PD_North = (-P_North + D_North);
                                        PD_East = (P_East - D_East);

                                        // GPS to pitch and roll
                                        GPS_Pitch = (int16_t)((coscompass * PD_North - sincompass * PD_East) / 8192);
                                        GPS_Roll  = (int16_t)((sincompass * PD_North + coscompass * PD_East) / 8192);

                                        // limit GPS controls
                                        if (GPS_Pitch >  35) GPS_Pitch =  35;
                                        if (GPS_Pitch < -35) GPS_Pitch = -35;
                                        if (GPS_Roll >  35) GPS_Roll =  35;
                                        if (GPS_Roll < -35) GPS_Roll = -35;
                                }
                                else // MK controlled by user
                                {
                                        // update hold point to current gps position
                                        GPSHoldPoint.Northing = GPSInfo.utmnorth;
                                        GPSHoldPoint.Easting  = GPSInfo.utmeast;
                                        GPSHoldPoint.Status = VALID;
                                        // disable gps control
                                        GPS_Pitch = 0;
                                        GPS_Roll = 0;
                                }
                        } // eof 3D-FIX
                        else // no 3D-SATFIX
                        {       // diable gps control
                                GPS_Pitch = 0;
                                GPS_Roll = 0;
                        }
                        // set current data as processed to avoid further calculations on the same gps data
                        GPSInfo.status = PROCESSED;
                        break;
                } // eof GPSInfo.status
        }
        return;
}