Subversion Repositories FlightCtrl

#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

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

}

// disable GPS control sticks

void GPS_Neutral(void)

{

        GPS_Pitch = 0;

        GPS_Roll = 0;

}

// calculates the GPS control sticks values from the position deviation

void GPS_PDController(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;

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

        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

        #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)

{

        static uint8_t GPS_Task = TSK_IDLE;

        int16_t satbeep;

        // poti2 enables the gps feature

        if(Poti2 < 70) GPS_Task = TSK_IDLE;

        else if (Poti2 < 160) GPS_Task = TSK_HOLD;

        else GPS_Task = TSK_HOME; // Poti2 >= 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))

                                        {

                                                // Calculate deviation from hold position

                                                GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing;

                                                GPSPosDev_East  = GPSInfo.utmeast  - HoldPosition.Easting;

                                                DebugOut.Analog[12] = GPSPosDev_North;

                                                DebugOut.Analog[13] = GPSPosDev_East;

                                                GPS_PDController();

                                        }

                                        else // MK controlled by user

                                        {

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

                                case TSK_HOME:

                                        if(HomePosition.Status == VALID)

                                        {

                                                // update hold point to current gps position

                                                HoldPosition.Northing = GPSInfo.utmnorth;

                                                HoldPosition.Easting  = GPSInfo.utmeast;

                                                HoldPosition.Status = VALID;

                                                // Calculate deviation from home position

                                                GPSPosDev_North = GPSInfo.utmnorth - HomePosition.Northing;

                                                GPSPosDev_East  = GPSInfo.utmeast  - HomePosition.Easting;

                                                DebugOut.Analog[12] = GPSPosDev_North;

                                                DebugOut.Analog[13] = GPSPosDev_East;

                                                GPS_PDController();

                                        }

                                        else // bad home position

                                        {

                                                GPS_Neutral();

                                                BeepTime = 50; // signal invalid home position

                                        }

                                        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;

}