Subversion Repositories FlightCtrl

#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        2000    // 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 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 deviation to target position
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;

                //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
                // 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(HoldPosition.Status == VALID)
                                        {
                                                // 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;
                                                        // disable gps control
                                                        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   = GPSInfo.status;
                                                // 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((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID))
                                                {
                                                        GPS_PDController(&HoldPosition);
                                                }
                                                else // manual control or no rteference position
                                                {
                                                        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;
}