Subversion Repositories FlightCtrl

#include <inttypes.h>

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

int16_t GPS_Pitch = 0;
int16_t GPS_Roll = 0;

int32_t GPS_P_Factor = 0;
int32_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};



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


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) && (!EmergencyLanding)) // 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;
                        BeepTime = 50;
                        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 control
                                if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (GPSHoldPoint.Status == VALID))
                                {
                                        // Calculate deviation from hold position
                                        GPSPosDev_North = GPSInfo.utmnorth - GPSHoldPoint.Northing;
                                        GPSPosDev_East  = GPSInfo.utmeast  - GPSHoldPoint.Easting;

                                        //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;

                                        // 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
                                        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
                                        #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;
                                }
                                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;
                                BeepTime = 50;
                        }
                        // set current data as processed to avoid further calculations on the same gps data
                        GPSInfo.status = PROCESSED;
                        break;
                } // eof GPSInfo.status
        }
        return;
}