Subversion Repositories FlightCtrl

// +

// + GPS read out:

// + modified Version of the Pitschu Brushless Ufo - (c) Peter Schulten, Mülheim, Germany

// + only for non-profit use 

};

char newData_navPosUtm = 0;  // Flag, wenn neue PosUtm-Daten vorliegen (211007Kr)

static uint8_t  gpsState;

#define                 GPS_EMPTY       0

#define                 GPS_SYNC1       1

#define                 GPS_SYNC2       2

#define                 GPS_CLASS       3

#define                 GPS_LEN1        4

#define                 GPS_LEN2        5

#define                 GPS_FILLING     6

#define                 GPS_CKA         7

#define                 GPS_CKB         8

gpsInfo_t               actualPos;                      // measured position (last gps record)

#define SYNC_CHAR1              0xb5

#define SYNC_CHAR2              0x62

#define CLASS_NAV               0x01

#define MSGID_STATUS    0x03

//#define       MSGID_POSLLH    0x02    //(231107Kr)

#define MSGID_POSUTM    0x08

#define MSGID_VELNED    0x12

typedef struct {

        unsigned long   ITOW;           // time of week

        uint8_t                 GPSfix;         // GPSfix Type, range 0..6

        uint8_t                 Flags;          // Navigation Status Flags

        uint8_t                 DiffS;          // Differential Status 

        uint8_t                 res;            // reserved

        unsigned long   TTFF;           // Time to first fix (millisecond time tag) 

        unsigned long   MSSS;           // Milliseconds since Startup / Reset 

        uint8_t                 packetStatus;

} NAV_STATUS_t;

/*

typedef struct {                                                                        //(231107Kr)

        unsigned long   ITOW;           // time of week

        long                    LON;            // longitude in 1e-07 deg

        long                    LAT;            // lattitude

        long                    HEIGHT;         // height in mm

        long                    HMSL;           // height above mean sea level im mm

        unsigned long   Hacc;           // horizontal accuracy in mm

        unsigned long   Vacc;           // vertical accuracy in mm

        uint8_t                 packetStatus;

} NAV_POSLLH_t;

*/

typedef struct {

        unsigned long   ITOW;           // time of week

        long                    EAST;           // cm  UTM Easting  

        long                    NORTH;          // cm  UTM Nording  

        long                    ALT;            // cm  altitude

        uint8_t                 ZONE;           // UTM zone number

        uint8_t                 HEM;            // Hemisphere Indicator (0=North, 1=South)  

        uint8_t                 packetStatus;

} NAV_POSUTM_t;

typedef struct {

        unsigned long   ITOW;           // ms  GPS Millisecond Time of Week  

        long                    VEL_N;          // cm/s  NED north velocity  

        long                    VEL_E;          // cm/s  NED east velocity  

        long                    VEL_D;          // cm/s  NED down velocity  

        unsigned long   Speed;          // cm/s  Speed (3-D)  

        unsigned long   GSpeed;         // cm/s  Ground Speed (2-D)  

        long                    Heading;        // deg (1e-05)  Heading 2-D  

        unsigned long   SAcc;           // cm/s  Speed Accuracy Estimate  

        unsigned long   CAcc;           // deg  Course / Heading Accuracy Estimate  

        uint8_t                 packetStatus;

} NAV_VELNED_t;

NAV_STATUS_t    navStatus;

//NAV_POSLLH_t  navPosLlh;      //(231107Kr)

NAV_POSUTM_t    navPosUtm;

NAV_VELNED_t    navVelNed;

volatile char                   *ubxP, *ubxEp, *ubxSp;  // pointers to packet currently transfered

volatile uint8_t                CK_A, CK_B;             // UBX checksum bytes

volatile unsigned short msgLen;

volatile uint8_t                msgID;

volatile uint8_t                ignorePacket;           // true when previous packet was not processed

#ifdef GPS_DEBUG                        // if set then the GPS data is transfered to display

extern volatile uint8_t v24state;

char buf[200];

void GPSscanData (void)

{

        if (navStatus.packetStatus == 1)                        // valid packet

        {

                actualPos.GPSFix = navStatus.GPSfix;

                actualPos.newData = navStatus.packetStatus;

                navStatus.packetStatus = 0;

        }

/*     

        if (navPosLlh.packetStatus == 1)                        // valid packet

        {

                actualPos.longi = navPosLlh.LON;     //(231107Kr)

                actualPos.lati = navPosLlh.LAT;      //(231107Kr)

                actualPos.height = navPosLlh.HEIGHT; //(231107Kr)

                navPosLlh.packetStatus = 0;

        }

*/

        if (navPosUtm.packetStatus == 1)                        // valid packet

        {

                actualPos.northing = navPosUtm.NORTH;  

                actualPos.easting = navPosUtm.EAST;  

                actualPos.altitude = navPosUtm.ALT;

                navPosUtm.packetStatus = 0;

                newData_navPosUtm = 1;  // (211007Kr)

                ROT_ON;         // Rot blinkt in der Frequenz mit der neue UTM-Daten des GPS Empfänger ankommen und brauchbar sind //(211107Kr)

        }

        if (navVelNed.packetStatus == 1)                        // valid packet

        {

                actualPos.velNorth = navVelNed.VEL_N;

                actualPos.velEast = navVelNed.VEL_E;

                actualPos.velDown = navVelNed.VEL_D;

                actualPos.GSpeed = navVelNed.GSpeed;  //Geschwindigkeit [cm/s] über Grund (151007Kr)

                navVelNed.packetStatus = 0;

        }

}

 SioTmp = UDR;

    uint8_t c;

        uint8_t re;

        re = (UCSR0A & (_B1(FE0) | _B1(DOR0)));         // any error occured ?

        c = SioTmp;  

#ifdef GPS_DEBUG

        *bp++ = c;

        if (bp >= (buf+200)) bp = buf;

        if (v24state == 0)

        {

                v24state = 1;

                UDR0 = *ep++;

                if (ep >= buf+200)

                        ep = buf;

                UCSR0B |= _B1(UDRIE0);          //enable further irqs

        }

#endif

        if (re == 0)

        {

                switch (gpsState)

                {

                        case GPS_EMPTY:

                                if (c == SYNC_CHAR1)

                                        gpsState = GPS_SYNC1;

                                break;

                        case GPS_SYNC1:

                                if (c == SYNC_CHAR2)

                                        gpsState = GPS_SYNC2;

                                else if (c != SYNC_CHAR1)

                                        gpsState = GPS_EMPTY;

                                break;

                        case GPS_SYNC2:

                                if (c == CLASS_NAV)

                                        gpsState = GPS_CLASS;

                                else

                                        gpsState = GPS_EMPTY;

                                break;

                        case GPS_CLASS:                                 // msg ID seen: init packed receive

                                msgID = c;

                                CK_A = CLASS_NAV + c;

                                CK_B = CLASS_NAV + CK_A;

                                gpsState = GPS_LEN1;

                                switch (msgID)

                                {

                                        case MSGID_STATUS:

                                                ubxP = (char*)&navStatus;

                                                ubxEp = (char*)(&navStatus + sizeof(NAV_STATUS_t));

                                                ubxSp = (char*)&navStatus.packetStatus;

                                                ignorePacket = navStatus.packetStatus;

                                                break;

                                        /*

                                        case MSGID_POSLLH:                              //(231107Kr)

                                                ubxP = (char*)&navPosLlh;

                                                ubxEp = (char*)(&navPosLlh + sizeof(NAV_POSLLH_t));

                                                ubxSp = (char*)&navPosLlh.packetStatus;

                                                ignorePacket = navPosLlh.packetStatus;

                                                break;

                                        */

                                        case MSGID_POSUTM:

                                                ubxP = (char*)&navPosUtm;

                                                ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t));

                                                ubxSp = (char*)&navPosUtm.packetStatus;

                                                ignorePacket = navPosUtm.packetStatus;

                                                break;

                                        case MSGID_VELNED:

                                                ubxP = (char*)&navVelNed;

                                                ubxEp = (char*)(&navVelNed + sizeof(NAV_VELNED_t));

                                                ubxSp = (char*)&navVelNed.packetStatus;

                                                ignorePacket = navVelNed.packetStatus;

                                                break;

                                        default:

                                                ignorePacket = 1;

                                                ubxSp = (char*)0;

                                }

                                break;

                        case GPS_LEN1:                                  // first len byte

                                msgLen = c;

                                CK_A += c;

                                CK_B += CK_A;

                                gpsState = GPS_LEN2;

                                break;

                        case GPS_LEN2:                                  // second len byte

                                msgLen = msgLen + (c * 256);

                                CK_A += c;

                                CK_B += CK_A;

                                gpsState = GPS_FILLING;         // next data will be stored in packet struct

                                break;

                        case GPS_FILLING:

                                CK_A += c;

                                CK_B += CK_A;

                                if ( !ignorePacket && ubxP < ubxEp)

                                                *ubxP++ = c;

                                if (--msgLen == 0)

                                        gpsState = GPS_CKA;

                                break;

                        case GPS_CKA:

                                if (c == CK_A)

                                        gpsState = GPS_CKB;

                                else

                                        gpsState = GPS_EMPTY;

                                break;

                        case GPS_CKB:

                                if (c == CK_B && ubxSp) // No error -> packet received successfully

                                        *ubxSp = 1;                     // set packetStatus in struct

                                gpsState = GPS_EMPTY;           // ready for next packet

                                break;

                        default:

                                gpsState = GPS_EMPTY;           // ready for next packet

                }

        }

        else            // discard any data if error occured

        {

                gpsState = GPS_EMPTY;

        }

        GPSscanData ();

  gpsState = GPS_EMPTY;