WebSVN - NaviCtrl - Diff - Rev 245 and 247


/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 2008 Ingo Busker, Holger Buss
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
// + FOR NON COMMERCIAL USE ONLY
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// +     from this software without specific prior written permission.
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
// +     for non-commercial use (directly or indirectly)
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// +     with our written permission
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// +     clearly linked as origin
// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
//
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// +  POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#include "91x_lib.h"
#include "uart1.h"
#include "ubx.h"
#include "led.h"
#include "timer1.h"
 
// ------------------------------------------------------------------------------------------------
// defines
 
#define DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980 722819 // the year 0 does not exist!
#define DAYS_PER_YEAR           365
#define DAYS_PER_LEAPYEAR       366
#define DAYS_PER_4YEARS         1461    //((3 * DAYS_PER_YEAR) + DAYS_PER_LEAPYEAR) // years dividable by 4 are leap years
#define DAYS_PER_100YEARS       36524   //((25 * DAYS_PER_4YEARS) - 1) // years dividable by 100 are no leap years
#define DAYS_PER_400YEARS       146097  //((4 * DAYS_PER_100YEARS) + 1L) // but years dividable by 400 are leap years
#define SECONDS_PER_MINUTE      60
#define MINUTES_PER_HOUR        60
#define HOURS_PER_DAY           24
#define DAYS_PER_WEEK           7
#define SECONDS_PER_HOUR        3600    //(SECONDS_PER_MINUTE * MINUTES_PER_HOUR)
#define SECONDS_PER_DAY         86400   //(SECONDS_PER_HOUR * HOURS_PER_DAY)
#define SECONDS_PER_WEEK        604800  //(SECONDS_PER_DAY * DAYS_PER_WEEK)
 
// days per month in normal and leap years
const u32       Leap[ 13 ]      = { 0,  31,  60,  91, 121, 152, 182, 213, 244, 274, 305, 335, 366 };
const u32 Normal[ 13 ]  = { 0,  31,  59,  90, 120, 151, 181, 212, 243, 273, 304, 334, 365 };
 
#define LEAP_SECONDS_FROM_1980  15
 
// message sync bytes
#define UBX_SYNC1_CHAR  0xB5
#define UBX_SYNC2_CHAR  0x62
// protocoll identifiers
 
// navigation class
#define UBX_CLASS_NAV   0x01
#define UBX_ID_POSLLH   0x02
#define UBX_ID_SOL              0x06
#define UBX_ID_VELNED   0x12
 
 
 
 
 
// ------------------------------------------------------------------------------------------------
// typedefs
 
 
// ubx parser state
typedef enum
{
        UBXSTATE_IDLE,
        UBXSTATE_SYNC1,
        UBXSTATE_SYNC2,
        UBXSTATE_CLASS,
        UBXSTATE_LEN1,
        UBXSTATE_LEN2,
        UBXSTATE_DATA,
        UBXSTATE_CKA,
        UBXSTATE_CKB
} ubxState_t;
 
typedef struct
{
        u32                     itow;           // ms GPS Millisecond Time of Week
        s32                     frac;           // ns remainder of rounded ms above
        s16                     week;           // GPS week
        u8                      GPSfix;         // GPSfix Type, range 0..6
        u8                      Flags;          // Navigation Status Flags
        s32                     ECEF_X;         // cm ECEF X coordinate
        s32                     ECEF_Y;         // cm ECEF Y coordinate
        s32                     ECEF_Z;         // cm ECEF Z coordinate
        u32                     PAcc;           // cm 3D Position Accuracy Estimate
        s32                     ECEFVX;         // cm/s ECEF X velocity
        s32                     ECEFVY;         // cm/s ECEF Y velocity
        s32                     ECEFVZ;         // cm/s ECEF Z velocity
        u32                     SAcc;           // cm/s Speed Accuracy Estimate
        u16                     PDOP;           // 0.01 Position DOP
        u8                      res1;           // reserved
        u8                      numSV;          // Number of SVs used in navigation solution
        u32                     res2;           // reserved
        u8                      Status;     // invalid/newdata/processed
} __attribute__((packed)) ubx_nav_sol_t;
 
 
typedef struct
{
        u32                     itow;           // ms  GPS Millisecond Time of Week
        s32                     VEL_N;          // cm/s  NED north velocity
        s32                     VEL_E;          // cm/s  NED east velocity
        s32                     VEL_D;          // cm/s  NED down velocity
        u32                     Speed;          // cm/s  Speed (3-D)
        u32                     GSpeed;         // cm/s  Ground Speed (2-D)
        s32                     Heading;        // 1e-05 deg  Heading 2-D
        u32                     SAcc;           // cm/s  Speed Accuracy Estimate
        u32                     CAcc;           // deg  Course / Heading Accuracy Estimate
        u8                      Status;         // invalid/newdata/processed
} __attribute__((packed)) ubx_nav_velned_t;
 
typedef struct
{
        u32                     itow;           // ms GPS Millisecond Time of Week
        s32                     LON;            // 1e-07 deg Longitude
        s32                     LAT;            // 1e-07 deg Latitude
        s32                     HEIGHT;         // mm Height above Ellipsoid
        s32                     HMSL;           // mm Height above mean sea level
        u32                     Hacc;           // mm Horizontal Accuracy Estimate
        u32                     Vacc;           // mm Vertical Accuracy Estimate
        u8                      Status;         // invalid/newdata/processed
} __attribute__((packed)) ubx_nav_posllh_t;
 
//------------------------------------------------------------------------------------
// global variables
 
// local buffers for the incomming ubx messages
volatile ubx_nav_sol_t          UbxSol    = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
volatile ubx_nav_posllh_t       UbxPosLlh = {0,0,0,0,0,0,0, INVALID};
volatile ubx_nav_velned_t       UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
volatile ubxmsg_t                       UbxMsg;
 
// shared buffer
gps_data_t              GPSData = {200,{0,0,0,INVALID},0,0,0,0,0,0,0, INVALID};
DateTime_t                      GPSDateTime = {0,0,0,0,0,0,0, INVALID};
 
#define UBX_TIMEOUT 500 // 500 ms
u32 UBX_Timeout = 0;
 
//------------------------------------------------------------------------------------
// functions
 
u8 IsLeapYear(u16 year)
{
        if((year%400 == 0) || ( (year%4 == 0) && (year%100 != 0) ) ) return 1;
        else return 0;
}
/********************************************************/
/*  Calculates the UTC Time from the GPS week and tow   */
/********************************************************/
void SetGPSTime(DateTime_t * pTimeStruct)
{
        u32 Days, Seconds, Week;
        u16 YearPart;
        u32 * MonthDayTab = 0;
        u8 i;
 
 
        // if GPS data show valid time data
        if((UbxSol.Status != INVALID) && (UbxSol.Flags & FLAG_WKNSET) && (UbxSol.Flags & FLAG_TOWSET) )
        {
                Seconds = UbxSol.itow / 1000L;
                Week = (u32)UbxSol.week;
                // correct leap seconds since 1980
                if(Seconds < LEAP_SECONDS_FROM_1980)
                {
                        Week--;
                        Seconds = SECONDS_PER_WEEK - LEAP_SECONDS_FROM_1980 + Seconds;
                }
                else Seconds -= LEAP_SECONDS_FROM_1980;
 
                Days = DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980;
                Days += (Week * DAYS_PER_WEEK);
                Days += Seconds / SECONDS_PER_DAY; // seperate days from GPS seconds of week
 
                pTimeStruct->Year = 1;
                YearPart = (u16)(Days / DAYS_PER_400YEARS);
                pTimeStruct->Year += YearPart * 400;
                Days = Days % DAYS_PER_400YEARS;
                YearPart = (u16)(Days / DAYS_PER_100YEARS);
                pTimeStruct->Year += YearPart * 100;
                Days = Days % DAYS_PER_100YEARS;
                YearPart = (u16)(Days / DAYS_PER_4YEARS);
                pTimeStruct->Year += YearPart * 4;
                Days = Days % DAYS_PER_4YEARS;
                if(Days < (3* DAYS_PER_YEAR)) YearPart = (u16)(Days / DAYS_PER_YEAR);
                else YearPart = 3;
                pTimeStruct->Year += YearPart;
                // calculate remaining days of year
                Days -= (u32)(YearPart *  DAYS_PER_YEAR);
                Days += 1;
                // check if current year is a leap year
                if(IsLeapYear(pTimeStruct->Year)) MonthDayTab = (u32*)Leap;
                else MonthDayTab = (u32*)Normal;
            // seperate month and day from days of year
                for ( i = 0; i < 12; i++ )
                {
                        if ( (MonthDayTab[i]< Days) && (Days <= MonthDayTab[i+1]) )
                        {
                                pTimeStruct->Month = i+1;
                                pTimeStruct->Day = Days - MonthDayTab[i];
                                i = 12;
                        }
                }
                Seconds = Seconds % SECONDS_PER_DAY; // remaining seconds of current day
                pTimeStruct->Hour = (u8)(Seconds / SECONDS_PER_HOUR);
                Seconds = Seconds % SECONDS_PER_HOUR; // remaining seconds of current hour
                pTimeStruct->Min = (u8)(Seconds / SECONDS_PER_MINUTE);
                Seconds = Seconds % SECONDS_PER_MINUTE; // remaining seconds of current minute
                pTimeStruct->Sec = (u8)(Seconds);
                pTimeStruct->mSec  = (u16)(UbxSol.itow % 1000L);
                pTimeStruct->Valid = 1;
        }
        else
        {
                pTimeStruct->Valid = 0;
        }
}
 
 
 
/********************************************************/
/*                  Initialize UBX Parser               */
/********************************************************/
void UBX_Init(void)
{
        // mark msg buffers invalid
        UbxSol.Status = INVALID;
        UbxPosLlh.Status = INVALID;
        UbxVelNed.Status = INVALID;
        UbxMsg.Status = INVALID;
        GPSData.Status = INVALID;
 
        UBX_Timeout = SetDelay(2 * UBX_Timeout);
}
 
/********************************************************/
/*            Upate GPS data stcructure                 */
/********************************************************/
void Update_GPSData (void)
{
        static u32 Msg_Count_Timeout = 0;
        static u8 Msg_Count = 0;
        static u32 LastTimeStamp = 0;
        u32 TimeStamp;
 
        // the timeout is used to detect the delay between two message sets
        // and is used for synchronisation so that always a set is collected
        // that belongs together
        // _______NAVSOL|POSLLH|VELNED|___________________NAVSOL|POSLLH|VELNED|_____________
        //              |  8ms | 8ms  |         184 ms          |      |      |
        // msg_count:   0      1      2                         0      1      2
 
        if(CheckDelay(Msg_Count_Timeout))       Msg_Count = 0;
        else Msg_Count++;
        Msg_Count_Timeout = SetDelay(100); // reset ubx msg timeout
 
        // if a new set of ubx messages was collected
        if((Msg_Count >= 2))
        {       // if set is complete
                if((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA))
                {
                        UBX_Timeout = SetDelay(UBX_TIMEOUT);
                        DebugOut.Analog[9]++;
 
                        // update GPS data only if the status is INVALID or PROCESSED  and the last ubx message was received within less than 100 ms
                        if(GPSData.Status != NEWDATA) // if last data were processed
                        { // wait for new data at all neccesary ubx messages
                                GPSData.Status = INVALID;
                                // update message cycle time
                                TimeStamp = CountMilliseconds;
                                GPSData.MsgCycleTime = (u16)(TimeStamp - LastTimeStamp);
                                LastTimeStamp = TimeStamp;
                                DebugOut.Analog[16] = GPSData.MsgCycleTime;
                                // NAV SOL
                                GPSData.Flags =                                 UbxSol.Flags;
                                GPSData.NumOfSats =                     UbxSol.numSV;
                                GPSData.SatFix =                                UbxSol.GPSfix;
                                GPSData.Position_Accuracy =             UbxSol.PAcc;
                                GPSData.Speed_Accuracy =                UbxSol.SAcc;
                                SetGPSTime(&SystemTime); // update system time
                                // NAV POSLLH
                                GPSData.Position.Status =               INVALID;
                                GPSData.Position.Longitude =    UbxPosLlh.LON;
                                GPSData.Position.Latitude =     UbxPosLlh.LAT;
                                GPSData.Position.Altitude =     UbxPosLlh.HMSL;
                                GPSData.Position.Status =               NEWDATA;
                                // NAV VELNED
                                GPSData.Speed_East =                    UbxVelNed.VEL_E;
                                GPSData.Speed_North =                   UbxVelNed.VEL_N;
                                GPSData.Speed_Top       =                       -UbxVelNed.VEL_D;
                                GPSData.Speed_Ground =                  UbxVelNed.GSpeed;
                                GPSData.Heading =                               UbxVelNed.Heading;
 
                                GPSData.Status = NEWDATA; // new data available
                        } // EOF if(GPSData.Status != NEWDATA)
                } // EOF all ubx messages received
                // set state to collect new data
                UbxSol.Status =                                 PROCESSED;      // ready for new data
                UbxPosLlh.Status =                              PROCESSED;      // ready for new data
                UbxVelNed.Status =                              PROCESSED;      // ready for new data
        }
}
 
 
/********************************************************/
/*                   UBX Parser                         */
/********************************************************/
void UBX_RxParser(u8 c)
{
        static ubxState_t ubxState = UBXSTATE_IDLE;
        static u8 ubxclass;
        static u8 ubxid;
        static u16 msglen;
        static u8 cka, ckb;
        static u8 *ubxP, *ubxEp, *ubxSp; // pointers to data currently transfered
 
 
        //state machine
        switch (ubxState)       // ubx message parser
        {
                case UBXSTATE_IDLE: // check 1st sync byte
                        if (c == UBX_SYNC1_CHAR) ubxState = UBXSTATE_SYNC1;
                        else ubxState = UBXSTATE_IDLE; // out of synchronization
                        break;
 
                case UBXSTATE_SYNC1: // check 2nd sync byte
                        if (c == UBX_SYNC2_CHAR) ubxState = UBXSTATE_SYNC2;
                        else ubxState = UBXSTATE_IDLE; // out of synchronization
                        break;
 
                case UBXSTATE_SYNC2: // check msg class to be NAV
                        ubxclass = c;
                        ubxState = UBXSTATE_CLASS;
                        break;
 
                case UBXSTATE_CLASS: // check message identifier
                        ubxid = c;
                        ubxState = UBXSTATE_LEN1;
                        cka = ubxclass + ubxid;
                        ckb = ubxclass + cka;
                        break;
 
                case UBXSTATE_LEN1: // 1st message length byte
                        msglen = (u16)c; // lowbyte first
                        cka += c;
                        ckb += cka;
                        ubxState = UBXSTATE_LEN2;
                        break;
 
                case UBXSTATE_LEN2: // 2nd message length byte
                        msglen += ((u16)c)<<8; // high byte last
                        cka += c;
                        ckb += cka;
 
                        switch(ubxclass)
                        {
                                case UBX_CLASS_NAV:
                                        switch(ubxid)
                                        {
                                                case UBX_ID_POSLLH: // geodetic position
                                                        ubxP =  (u8 *)&UbxPosLlh; // data start pointer
                                                        ubxEp = (u8 *)(&UbxPosLlh + 1); // data end pointer
                                                        ubxSp = (u8 *)&UbxPosLlh.Status; // status pointer
                                                        break;
               
                                                case UBX_ID_SOL: // navigation solution
                                                        ubxP =  (u8 *)&UbxSol; // data start pointer
                                                        ubxEp = (u8 *)(&UbxSol + 1); // data end pointer
                                                        ubxSp = (u8 *)&UbxSol.Status; // status pointer
                                                        break;
               
                                                case UBX_ID_VELNED: // velocity vector in tangent plane
                                                        ubxP =  (u8 *)&UbxVelNed; // data start pointer
                                                        ubxEp = (u8 *)(&UbxVelNed + 1); // data end pointer
                                                        ubxSp = (u8 *)&UbxVelNed.Status; // status pointer
                                                        break;
               
                                                default:                        // unsupported identifier
                                                        ubxState = UBXSTATE_IDLE;
                                                        return;
                                        }
                                        break;
 
                                default: // other  classes
 
                                        if(UbxMsg.Status == NEWDATA) ubxState = UBXSTATE_IDLE;
                                        else if(((UbxMsg.Hdr.Class&UbxMsg.ClassMask) == (ubxclass&UbxMsg.ClassMask)) && ((UbxMsg.Hdr.Id&UbxMsg.IdMask) == (ubxid&UbxMsg.IdMask)))
                                        {       // buffer is free and message matches to filter criteria
                                                UbxMsg.Status = INVALID;
 
                                                UbxMsg.Hdr.Class = ubxclass;
                                                UbxMsg.Hdr.Id = ubxid;
                                                UbxMsg.Hdr.Length = msglen;
 
                                                ubxP =  (u8 *)&(UbxMsg.Data);   // data start pointer
                                                ubxEp = (u8 *)(&UbxMsg + 1);    // data end pointer
 
                                                ubxSp = (u8 *)&UbxMsg.Status;   // status pointer       
 
 
 
 
 
 
                                        }
                                        else ubxState = UBXSTATE_IDLE;
                                        break;
                        }
                        if(ubxState != UBXSTATE_IDLE)
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
                        {
                                // if the old data are not processed so far then break parsing now
                                // to avoid writing new data in ISR during reading by another function
                                if ( *ubxSp == NEWDATA )
                                {
                                        ubxState = UBXSTATE_IDLE;
                                        if(ubxclass == UBX_CLASS_NAV) Update_GPSData(); //update GPS info respectively
                                }
                                else // data invalid or allready processed
                                {
                                        *ubxSp = INVALID; // mark invalid during buffer filling
                                        ubxState = UBXSTATE_DATA;
                                }
                        }
                        break;
 
                case UBXSTATE_DATA: // collecting data
                        if (ubxP < ubxEp)
                        {
                                *ubxP++ = c; // copy curent data byte if any space is left
                                cka += c;
                                ckb += cka;
                                if (--msglen == 0)      ubxState = UBXSTATE_CKA; // switch to next state if all data was read
                        }
                        else // rx buffer overrun
                        {
                                ubxState = UBXSTATE_IDLE;
                        }
                        break;
 
                case UBXSTATE_CKA:
                        if (c == cka) ubxState = UBXSTATE_CKB;
                        else
                        {
                                *ubxSp = INVALID;
                                ubxState = UBXSTATE_IDLE;
                        }
                        break;
 
                case UBXSTATE_CKB:
                        if (c == ckb)
                        {
                                *ubxSp = NEWDATA; // new data are valid
                                if(ubxclass == UBX_CLASS_NAV) Update_GPSData(); //update GPS info respectively
                        }
                        else
                        {       // if checksum not match then set data invalid
                                *ubxSp = INVALID;
                        }
                        ubxState = UBXSTATE_IDLE; // ready to parse new data
                        break;
 
                default: // unknown ubx state
                        ubxState = UBXSTATE_IDLE;
                        break;
 
        }
}
 
u8 UBX_CreateMsg(Buffer_t* pBuff, u8* pData, u16 Len)
{
        u16 i;
        u8 cka = 0, ckb = 0;
        // check if buffer is available
        if(pBuff->Locked == TRUE) return(0);
        // check if buffer size is sufficient
        if(pBuff->Size < 8 + Len) return(0);
        // lock the buffer
        pBuff->Locked = TRUE;
        // start at begin 
        pBuff->Position = 0;
        pBuff->pData[pBuff->Position++] = UBX_SYNC1_CHAR;
        pBuff->pData[pBuff->Position++] = UBX_SYNC2_CHAR;
 
        for(i=0;i<Len;i++)
        {
                pBuff->pData[pBuff->Position++] = pData[i];
 
 
        }
        // calculate checksum
        for(i=2;i<pBuff->Position;i++)
        {
                cka += pBuff->pData[i];
                ckb += cka;
        }
        pBuff->pData[pBuff->Position++] = cka;
        pBuff->pData[pBuff->Position++] = ckb;
        pBuff->DataBytes = pBuff->Position;
        pBuff->Position = 0;  // reset buffer position for transmision
        return(1);
}
 

Rev 245	Rev 247
1	/#######################################################################################/	1	/#######################################################################################/
2	/* !!! THIS IS NOT FREE SOFTWARE !!! */	2	/* !!! THIS IS NOT FREE SOFTWARE !!! */
3	/#######################################################################################/	3	/#######################################################################################/
4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5	// + Copyright (c) 2008 Ingo Busker, Holger Buss	5	// + Copyright (c) 2008 Ingo Busker, Holger Buss
6	// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY	6	// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
7	// + FOR NON COMMERCIAL USE ONLY	7	// + FOR NON COMMERCIAL USE ONLY
8	// + www.MikroKopter.com	8	// + www.MikroKopter.com
9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),	10	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
11	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.	11	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
12	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt	12	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
13	// + bzgl. der Nutzungsbedingungen aufzunehmen.	13	// + bzgl. der Nutzungsbedingungen aufzunehmen.
14	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,	14	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
15	// + Verkauf von Luftbildaufnahmen, usw.	15	// + Verkauf von Luftbildaufnahmen, usw.
16	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	16	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,	17	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
18	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen	18	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts	20	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
21	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"	21	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
22	// + eindeutig als Ursprung verlinkt werden	22	// + eindeutig als Ursprung verlinkt werden
23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion	24	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
25	// + Benutzung auf eigene Gefahr	25	// + Benutzung auf eigene Gefahr
26	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden	26	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
27	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	27	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28	// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur	28	// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
29	// + mit unserer Zustimmung zulässig	29	// + mit unserer Zustimmung zulässig
30	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	30	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen	31	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
32	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	32	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
33	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,	33	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
34	// + this list of conditions and the following disclaimer.	34	// + this list of conditions and the following disclaimer.
35	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived	35	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
36	// + from this software without specific prior written permission.	36	// + from this software without specific prior written permission.
37	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted	37	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
38	// + for non-commercial use (directly or indirectly)	38	// + for non-commercial use (directly or indirectly)
39	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted	39	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
40	// + with our written permission	40	// + with our written permission
41	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be	41	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
42	// + clearly linked as origin	42	// + clearly linked as origin
43	// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed	43	// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
44	//	44	//
45	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"	45	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
46	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE	46	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
47	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE	47	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
48	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE	48	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
49	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR	49	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
50	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF	50	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
51	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS	51	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
52	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN	52	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
53	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)	53	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
54	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE	54	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
55	// + POSSIBILITY OF SUCH DAMAGE.	55	// + POSSIBILITY OF SUCH DAMAGE.
56	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	56	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
57	#include <string.h>	-
58	#include "91x_lib.h"	57	#include "91x_lib.h"
59	#include "uart1.h"	58	#include "uart1.h"
60	#include "ubx.h"	59	#include "ubx.h"
61	#include "led.h"	60	#include "led.h"
62	#include "timer1.h"	61	#include "timer1.h"
63		62
64	// ------------------------------------------------------------------------------------------------	63	// ------------------------------------------------------------------------------------------------
65	// defines	64	// defines
66		65
67	#define DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980 722819 // the year 0 does not exist!	66	#define DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980 722819 // the year 0 does not exist!
68	#define DAYS_PER_YEAR 365	67	#define DAYS_PER_YEAR 365
69	#define DAYS_PER_LEAPYEAR 366	68	#define DAYS_PER_LEAPYEAR 366
70	#define DAYS_PER_4YEARS 1461 //((3 * DAYS_PER_YEAR) + DAYS_PER_LEAPYEAR) // years dividable by 4 are leap years	69	#define DAYS_PER_4YEARS 1461 //((3 * DAYS_PER_YEAR) + DAYS_PER_LEAPYEAR) // years dividable by 4 are leap years
71	#define DAYS_PER_100YEARS 36524 //((25 * DAYS_PER_4YEARS) - 1) // years dividable by 100 are no leap years	70	#define DAYS_PER_100YEARS 36524 //((25 * DAYS_PER_4YEARS) - 1) // years dividable by 100 are no leap years
72	#define DAYS_PER_400YEARS 146097 //((4 * DAYS_PER_100YEARS) + 1L) // but years dividable by 400 are leap years	71	#define DAYS_PER_400YEARS 146097 //((4 * DAYS_PER_100YEARS) + 1L) // but years dividable by 400 are leap years
73	#define SECONDS_PER_MINUTE 60	72	#define SECONDS_PER_MINUTE 60
74	#define MINUTES_PER_HOUR 60	73	#define MINUTES_PER_HOUR 60
75	#define HOURS_PER_DAY 24	74	#define HOURS_PER_DAY 24
76	#define DAYS_PER_WEEK 7	75	#define DAYS_PER_WEEK 7
77	#define SECONDS_PER_HOUR 3600 //(SECONDS_PER_MINUTE * MINUTES_PER_HOUR)	76	#define SECONDS_PER_HOUR 3600 //(SECONDS_PER_MINUTE * MINUTES_PER_HOUR)
78	#define SECONDS_PER_DAY 86400 //(SECONDS_PER_HOUR * HOURS_PER_DAY)	77	#define SECONDS_PER_DAY 86400 //(SECONDS_PER_HOUR * HOURS_PER_DAY)
79	#define SECONDS_PER_WEEK 604800 //(SECONDS_PER_DAY * DAYS_PER_WEEK)	78	#define SECONDS_PER_WEEK 604800 //(SECONDS_PER_DAY * DAYS_PER_WEEK)
80		79
81	// days per month in normal and leap years	80	// days per month in normal and leap years
82	const u32 Leap[ 13 ] = { 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, 366 };	81	const u32 Leap[ 13 ] = { 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, 366 };
83	const u32 Normal[ 13 ] = { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, 365 };	82	const u32 Normal[ 13 ] = { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, 365 };
84		83
85	#define LEAP_SECONDS_FROM_1980 15	84	#define LEAP_SECONDS_FROM_1980 15
86		85
87	// message sync bytes	86	// message sync bytes
88	#define UBX_SYNC1_CHAR 0xB5	87	#define UBX_SYNC1_CHAR 0xB5
89	#define UBX_SYNC2_CHAR 0x62	88	#define UBX_SYNC2_CHAR 0x62
90	// protocoll identifiers	89	// protocoll identifiers
91	#define UBX_CLASS_UNDEF 0x00	-
92	// navigation class	90	// navigation class
93	#define UBX_CLASS_NAV 0x01	91	#define UBX_CLASS_NAV 0x01
94	#define UBX_ID_POSLLH 0x02	92	#define UBX_ID_POSLLH 0x02
95	#define UBX_ID_SOL 0x06	93	#define UBX_ID_SOL 0x06
96	#define UBX_ID_VELNED 0x12	94	#define UBX_ID_VELNED 0x12
97	// acknowledge class	-
98	#define UBX_CLASS_ACK 0x05	-
99	#define UBX_ID_ACK_NAK 0x00	-
100	#define UBX_ID_ACK_ACK 0x01	-
-		95
101	// ------------------------------------------------------------------------------------------------	96	// ------------------------------------------------------------------------------------------------
102	// typedefs	97	// typedefs
103		98
104		99
105	// ubx parser state	100	// ubx parser state
106	typedef enum	101	typedef enum
107	{	102	{
108	UBXSTATE_IDLE,	103	UBXSTATE_IDLE,
109	UBXSTATE_SYNC1,	104	UBXSTATE_SYNC1,
110	UBXSTATE_SYNC2,	105	UBXSTATE_SYNC2,
111	UBXSTATE_CLASS,	106	UBXSTATE_CLASS,
112	UBXSTATE_LEN1,	107	UBXSTATE_LEN1,
113	UBXSTATE_LEN2,	108	UBXSTATE_LEN2,
114	UBXSTATE_DATA,	109	UBXSTATE_DATA,
115	UBXSTATE_CKA,	110	UBXSTATE_CKA,
116	UBXSTATE_CKB	111	UBXSTATE_CKB
117	} ubxState_t;	112	} ubxState_t;
118		113
119	typedef struct	114	typedef struct
120	{	115	{
121	u32 itow; // ms GPS Millisecond Time of Week	116	u32 itow; // ms GPS Millisecond Time of Week
122	s32 frac; // ns remainder of rounded ms above	117	s32 frac; // ns remainder of rounded ms above
123	s16 week; // GPS week	118	s16 week; // GPS week
124	u8 GPSfix; // GPSfix Type, range 0..6	119	u8 GPSfix; // GPSfix Type, range 0..6
125	u8 Flags; // Navigation Status Flags	120	u8 Flags; // Navigation Status Flags
126	s32 ECEF_X; // cm ECEF X coordinate	121	s32 ECEF_X; // cm ECEF X coordinate
127	s32 ECEF_Y; // cm ECEF Y coordinate	122	s32 ECEF_Y; // cm ECEF Y coordinate
128	s32 ECEF_Z; // cm ECEF Z coordinate	123	s32 ECEF_Z; // cm ECEF Z coordinate
129	u32 PAcc; // cm 3D Position Accuracy Estimate	124	u32 PAcc; // cm 3D Position Accuracy Estimate
130	s32 ECEFVX; // cm/s ECEF X velocity	125	s32 ECEFVX; // cm/s ECEF X velocity
131	s32 ECEFVY; // cm/s ECEF Y velocity	126	s32 ECEFVY; // cm/s ECEF Y velocity
132	s32 ECEFVZ; // cm/s ECEF Z velocity	127	s32 ECEFVZ; // cm/s ECEF Z velocity
133	u32 SAcc; // cm/s Speed Accuracy Estimate	128	u32 SAcc; // cm/s Speed Accuracy Estimate
134	u16 PDOP; // 0.01 Position DOP	129	u16 PDOP; // 0.01 Position DOP
135	u8 res1; // reserved	130	u8 res1; // reserved
136	u8 numSV; // Number of SVs used in navigation solution	131	u8 numSV; // Number of SVs used in navigation solution
137	u32 res2; // reserved	132	u32 res2; // reserved
138	u8 Status; // invalid/newdata/processed	133	u8 Status; // invalid/newdata/processed
139	} __attribute__((packed)) ubx_nav_sol_t;	134	} __attribute__((packed)) ubx_nav_sol_t;
140		135
141		136
142	typedef struct	137	typedef struct
143	{	138	{
144	u32 itow; // ms GPS Millisecond Time of Week	139	u32 itow; // ms GPS Millisecond Time of Week
145	s32 VEL_N; // cm/s NED north velocity	140	s32 VEL_N; // cm/s NED north velocity
146	s32 VEL_E; // cm/s NED east velocity	141	s32 VEL_E; // cm/s NED east velocity
147	s32 VEL_D; // cm/s NED down velocity	142	s32 VEL_D; // cm/s NED down velocity
148	u32 Speed; // cm/s Speed (3-D)	143	u32 Speed; // cm/s Speed (3-D)
149	u32 GSpeed; // cm/s Ground Speed (2-D)	144	u32 GSpeed; // cm/s Ground Speed (2-D)
150	s32 Heading; // 1e-05 deg Heading 2-D	145	s32 Heading; // 1e-05 deg Heading 2-D
151	u32 SAcc; // cm/s Speed Accuracy Estimate	146	u32 SAcc; // cm/s Speed Accuracy Estimate
152	u32 CAcc; // deg Course / Heading Accuracy Estimate	147	u32 CAcc; // deg Course / Heading Accuracy Estimate
153	u8 Status; // invalid/newdata/processed	148	u8 Status; // invalid/newdata/processed
154	} __attribute__((packed)) ubx_nav_velned_t;	149	} __attribute__((packed)) ubx_nav_velned_t;
155		150
156	typedef struct	151	typedef struct
157	{	152	{
158	u32 itow; // ms GPS Millisecond Time of Week	153	u32 itow; // ms GPS Millisecond Time of Week
159	s32 LON; // 1e-07 deg Longitude	154	s32 LON; // 1e-07 deg Longitude
160	s32 LAT; // 1e-07 deg Latitude	155	s32 LAT; // 1e-07 deg Latitude
161	s32 HEIGHT; // mm Height above Ellipsoid	156	s32 HEIGHT; // mm Height above Ellipsoid
162	s32 HMSL; // mm Height above mean sea level	157	s32 HMSL; // mm Height above mean sea level
163	u32 Hacc; // mm Horizontal Accuracy Estimate	158	u32 Hacc; // mm Horizontal Accuracy Estimate
164	u32 Vacc; // mm Vertical Accuracy Estimate	159	u32 Vacc; // mm Vertical Accuracy Estimate
165	u8 Status; // invalid/newdata/processed	160	u8 Status; // invalid/newdata/processed
166	} __attribute__((packed)) ubx_nav_posllh_t;	161	} __attribute__((packed)) ubx_nav_posllh_t;
167		162
168	//------------------------------------------------------------------------------------	163	//------------------------------------------------------------------------------------
169	// global variables	164	// global variables
170		165
171	// local buffers for the incomming ubx messages	166	// local buffers for the incomming ubx messages
172	volatile ubx_nav_sol_t UbxSol = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};	167	volatile ubx_nav_sol_t UbxSol = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
173	volatile ubx_nav_posllh_t UbxPosLlh = {0,0,0,0,0,0,0, INVALID};	168	volatile ubx_nav_posllh_t UbxPosLlh = {0,0,0,0,0,0,0, INVALID};
174	volatile ubx_nav_velned_t UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID};	169	volatile ubx_nav_velned_t UbxVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
175	volatile ubx_ack_t UbxAck = {0,0,0,INVALID};	170	volatile ubxmsg_t UbxMsg;
176		171
177	// shared buffer	172	// shared buffer
178	gps_data_t GPSData = {200,{0,0,0,INVALID},0,0,0,0,0,0,0, INVALID};	173	gps_data_t GPSData = {200,{0,0,0,INVALID},0,0,0,0,0,0,0, INVALID};
179	DateTime_t GPSDateTime = {0,0,0,0,0,0,0, INVALID};	174	DateTime_t GPSDateTime = {0,0,0,0,0,0,0, INVALID};
180		175
181	#define UBX_TIMEOUT 500 // 500 ms	176	#define UBX_TIMEOUT 500 // 500 ms
182	u32 UBX_Timeout = 0;	177	u32 UBX_Timeout = 0;
183		178
184	//------------------------------------------------------------------------------------	179	//------------------------------------------------------------------------------------
185	// functions	180	// functions
186		181
187	u8 IsLeapYear(u16 year)	182	u8 IsLeapYear(u16 year)
188	{	183	{
189	if((year%400 == 0) \|\| ( (year%4 == 0) && (year%100 != 0) ) ) return 1;	184	if((year%400 == 0) \|\| ( (year%4 == 0) && (year%100 != 0) ) ) return 1;
190	else return 0;	185	else return 0;
191	}	186	}
192	/********************************************************/	187	/********************************************************/
193	/* Calculates the UTC Time from the GPS week and tow */	188	/* Calculates the UTC Time from the GPS week and tow */
194	/********************************************************/	189	/********************************************************/
195	void SetGPSTime(DateTime_t * pTimeStruct)	190	void SetGPSTime(DateTime_t * pTimeStruct)
196	{	191	{
197	u32 Days, Seconds, Week;	192	u32 Days, Seconds, Week;
198	u16 YearPart;	193	u16 YearPart;
199	u32 * MonthDayTab = 0;	194	u32 * MonthDayTab = 0;
200	u8 i;	195	u8 i;
201		196
202		197
203	// if GPS data show valid time data	198	// if GPS data show valid time data
204	if((UbxSol.Status != INVALID) && (UbxSol.Flags & FLAG_WKNSET) && (UbxSol.Flags & FLAG_TOWSET) )	199	if((UbxSol.Status != INVALID) && (UbxSol.Flags & FLAG_WKNSET) && (UbxSol.Flags & FLAG_TOWSET) )
205	{	200	{
206	Seconds = UbxSol.itow / 1000L;	201	Seconds = UbxSol.itow / 1000L;
207	Week = (u32)UbxSol.week;	202	Week = (u32)UbxSol.week;
208	// correct leap seconds since 1980	203	// correct leap seconds since 1980
209	if(Seconds < LEAP_SECONDS_FROM_1980)	204	if(Seconds < LEAP_SECONDS_FROM_1980)
210	{	205	{
211	Week--;	206	Week--;
212	Seconds = SECONDS_PER_WEEK - LEAP_SECONDS_FROM_1980 + Seconds;	207	Seconds = SECONDS_PER_WEEK - LEAP_SECONDS_FROM_1980 + Seconds;
213	}	208	}
214	else Seconds -= LEAP_SECONDS_FROM_1980;	209	else Seconds -= LEAP_SECONDS_FROM_1980;
215		210
216	Days = DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980;	211	Days = DAYS_FROM_JAN01YEAR0001_TO_JAN6_1980;
217	Days += (Week * DAYS_PER_WEEK);	212	Days += (Week * DAYS_PER_WEEK);
218	Days += Seconds / SECONDS_PER_DAY; // seperate days from GPS seconds of week	213	Days += Seconds / SECONDS_PER_DAY; // seperate days from GPS seconds of week
219		214
220	pTimeStruct->Year = 1;	215	pTimeStruct->Year = 1;
221	YearPart = (u16)(Days / DAYS_PER_400YEARS);	216	YearPart = (u16)(Days / DAYS_PER_400YEARS);
222	pTimeStruct->Year += YearPart * 400;	217	pTimeStruct->Year += YearPart * 400;
223	Days = Days % DAYS_PER_400YEARS;	218	Days = Days % DAYS_PER_400YEARS;
224	YearPart = (u16)(Days / DAYS_PER_100YEARS);	219	YearPart = (u16)(Days / DAYS_PER_100YEARS);
225	pTimeStruct->Year += YearPart * 100;	220	pTimeStruct->Year += YearPart * 100;
226	Days = Days % DAYS_PER_100YEARS;	221	Days = Days % DAYS_PER_100YEARS;
227	YearPart = (u16)(Days / DAYS_PER_4YEARS);	222	YearPart = (u16)(Days / DAYS_PER_4YEARS);
228	pTimeStruct->Year += YearPart * 4;	223	pTimeStruct->Year += YearPart * 4;
229	Days = Days % DAYS_PER_4YEARS;	224	Days = Days % DAYS_PER_4YEARS;
230	if(Days < (3* DAYS_PER_YEAR)) YearPart = (u16)(Days / DAYS_PER_YEAR);	225	if(Days < (3* DAYS_PER_YEAR)) YearPart = (u16)(Days / DAYS_PER_YEAR);
231	else YearPart = 3;	226	else YearPart = 3;
232	pTimeStruct->Year += YearPart;	227	pTimeStruct->Year += YearPart;
233	// calculate remaining days of year	228	// calculate remaining days of year
234	Days -= (u32)(YearPart * DAYS_PER_YEAR);	229	Days -= (u32)(YearPart * DAYS_PER_YEAR);
235	Days += 1;	230	Days += 1;
236	// check if current year is a leap year	231	// check if current year is a leap year
237	if(IsLeapYear(pTimeStruct->Year)) MonthDayTab = (u32*)Leap;	232	if(IsLeapYear(pTimeStruct->Year)) MonthDayTab = (u32*)Leap;
238	else MonthDayTab = (u32*)Normal;	233	else MonthDayTab = (u32*)Normal;
239	// seperate month and day from days of year	234	// seperate month and day from days of year
240	for ( i = 0; i < 12; i++ )	235	for ( i = 0; i < 12; i++ )
241	{	236	{
242	if ( (MonthDayTab[i]< Days) && (Days <= MonthDayTab[i+1]) )	237	if ( (MonthDayTab[i]< Days) && (Days <= MonthDayTab[i+1]) )
243	{	238	{
244	pTimeStruct->Month = i+1;	239	pTimeStruct->Month = i+1;
245	pTimeStruct->Day = Days - MonthDayTab[i];	240	pTimeStruct->Day = Days - MonthDayTab[i];
246	i = 12;	241	i = 12;
247	}	242	}
248	}	243	}
249	Seconds = Seconds % SECONDS_PER_DAY; // remaining seconds of current day	244	Seconds = Seconds % SECONDS_PER_DAY; // remaining seconds of current day
250	pTimeStruct->Hour = (u8)(Seconds / SECONDS_PER_HOUR);	245	pTimeStruct->Hour = (u8)(Seconds / SECONDS_PER_HOUR);
251	Seconds = Seconds % SECONDS_PER_HOUR; // remaining seconds of current hour	246	Seconds = Seconds % SECONDS_PER_HOUR; // remaining seconds of current hour
252	pTimeStruct->Min = (u8)(Seconds / SECONDS_PER_MINUTE);	247	pTimeStruct->Min = (u8)(Seconds / SECONDS_PER_MINUTE);
253	Seconds = Seconds % SECONDS_PER_MINUTE; // remaining seconds of current minute	248	Seconds = Seconds % SECONDS_PER_MINUTE; // remaining seconds of current minute
254	pTimeStruct->Sec = (u8)(Seconds);	249	pTimeStruct->Sec = (u8)(Seconds);
255	pTimeStruct->mSec = (u16)(UbxSol.itow % 1000L);	250	pTimeStruct->mSec = (u16)(UbxSol.itow % 1000L);
256	pTimeStruct->Valid = 1;	251	pTimeStruct->Valid = 1;
257	}	252	}
258	else	253	else
259	{	254	{
260	pTimeStruct->Valid = 0;	255	pTimeStruct->Valid = 0;
261	}	256	}
262	}	257	}
263		258
264		259
265		260
266	/********************************************************/	261	/********************************************************/
267	/* Initialize UBX Parser */	262	/* Initialize UBX Parser */
268	/********************************************************/	263	/********************************************************/
269	void UBX_Init(void)	264	void UBX_Init(void)
270	{	265	{
271	// mark msg buffers invalid	266	// mark msg buffers invalid
272	UbxSol.Status = INVALID;	267	UbxSol.Status = INVALID;
273	UbxPosLlh.Status = INVALID;	268	UbxPosLlh.Status = INVALID;
274	UbxVelNed.Status = INVALID;	269	UbxVelNed.Status = INVALID;
275	UbxAck.Status = INVALID;	270	UbxMsg.Status = INVALID;
276	GPSData.Status = INVALID;	271	GPSData.Status = INVALID;
277		272
278	UBX_Timeout = SetDelay(2 * UBX_Timeout);	273	UBX_Timeout = SetDelay(2 * UBX_Timeout);
279	}	274	}
280		275
281	/********************************************************/	276	/********************************************************/
282	/* Upate GPS data stcructure */	277	/* Upate GPS data stcructure */
283	/********************************************************/	278	/********************************************************/
284	void Update_GPSData (void)	279	void Update_GPSData (void)
285	{	280	{
286	static u32 Msg_Count_Timeout = 0;	281	static u32 Msg_Count_Timeout = 0;
287	static u8 Msg_Count = 0;	282	static u8 Msg_Count = 0;
288	static u32 LastTimeStamp = 0;	283	static u32 LastTimeStamp = 0;
289	u32 TimeStamp;	284	u32 TimeStamp;
290		285
291	// the timeout is used to detect the delay between two message sets	286	// the timeout is used to detect the delay between two message sets
292	// and is used for synchronisation so that always a set is collected	287	// and is used for synchronisation so that always a set is collected
293	// that belongs together	288	// that belongs together
294	// _______NAVSOL\|POSLLH\|VELNED\|___________________NAVSOL\|POSLLH\|VELNED\|_____________	289	// _______NAVSOL\|POSLLH\|VELNED\|___________________NAVSOL\|POSLLH\|VELNED\|_____________
295	// \| 8ms \| 8ms \| 184 ms \| \| \|	290	// \| 8ms \| 8ms \| 184 ms \| \| \|
296	// msg_count: 0 1 2 0 1 2	291	// msg_count: 0 1 2 0 1 2
297		292
298	if(CheckDelay(Msg_Count_Timeout)) Msg_Count = 0;	293	if(CheckDelay(Msg_Count_Timeout)) Msg_Count = 0;
299	else Msg_Count++;	294	else Msg_Count++;
300	Msg_Count_Timeout = SetDelay(100); // reset ubx msg timeout	295	Msg_Count_Timeout = SetDelay(100); // reset ubx msg timeout
301		296
302	// if a new set of ubx messages was collected	297	// if a new set of ubx messages was collected
303	if((Msg_Count >= 2))	298	if((Msg_Count >= 2))
304	{ // if set is complete	299	{ // if set is complete
305	if((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA))	300	if((UbxSol.Status == NEWDATA) && (UbxPosLlh.Status == NEWDATA) && (UbxVelNed.Status == NEWDATA))
306	{	301	{
307	UBX_Timeout = SetDelay(UBX_TIMEOUT);	302	UBX_Timeout = SetDelay(UBX_TIMEOUT);
308	DebugOut.Analog[9]++;	303	DebugOut.Analog[9]++;
309		304
310	// update GPS data only if the status is INVALID or PROCESSED and the last ubx message was received within less than 100 ms	305	// update GPS data only if the status is INVALID or PROCESSED and the last ubx message was received within less than 100 ms
311	if(GPSData.Status != NEWDATA) // if last data were processed	306	if(GPSData.Status != NEWDATA) // if last data were processed
312	{ // wait for new data at all neccesary ubx messages	307	{ // wait for new data at all neccesary ubx messages
313	GPSData.Status = INVALID;	308	GPSData.Status = INVALID;
314	// update message cycle time	309	// update message cycle time
315	TimeStamp = CountMilliseconds;	310	TimeStamp = CountMilliseconds;
316	GPSData.MsgCycleTime = (u16)(TimeStamp - LastTimeStamp);	311	GPSData.MsgCycleTime = (u16)(TimeStamp - LastTimeStamp);
317	LastTimeStamp = TimeStamp;	312	LastTimeStamp = TimeStamp;
318	DebugOut.Analog[16] = GPSData.MsgCycleTime;	313	DebugOut.Analog[16] = GPSData.MsgCycleTime;
319	// NAV SOL	314	// NAV SOL
320	GPSData.Flags = UbxSol.Flags;	315	GPSData.Flags = UbxSol.Flags;
321	GPSData.NumOfSats = UbxSol.numSV;	316	GPSData.NumOfSats = UbxSol.numSV;
322	GPSData.SatFix = UbxSol.GPSfix;	317	GPSData.SatFix = UbxSol.GPSfix;
323	GPSData.Position_Accuracy = UbxSol.PAcc;	318	GPSData.Position_Accuracy = UbxSol.PAcc;
324	GPSData.Speed_Accuracy = UbxSol.SAcc;	319	GPSData.Speed_Accuracy = UbxSol.SAcc;
325	SetGPSTime(&SystemTime); // update system time	320	SetGPSTime(&SystemTime); // update system time
326	// NAV POSLLH	321	// NAV POSLLH
327	GPSData.Position.Status = INVALID;	322	GPSData.Position.Status = INVALID;
328	GPSData.Position.Longitude = UbxPosLlh.LON;	323	GPSData.Position.Longitude = UbxPosLlh.LON;
329	GPSData.Position.Latitude = UbxPosLlh.LAT;	324	GPSData.Position.Latitude = UbxPosLlh.LAT;
330	GPSData.Position.Altitude = UbxPosLlh.HMSL;	325	GPSData.Position.Altitude = UbxPosLlh.HMSL;
331	GPSData.Position.Status = NEWDATA;	326	GPSData.Position.Status = NEWDATA;
332	// NAV VELNED	327	// NAV VELNED
333	GPSData.Speed_East = UbxVelNed.VEL_E;	328	GPSData.Speed_East = UbxVelNed.VEL_E;
334	GPSData.Speed_North = UbxVelNed.VEL_N;	329	GPSData.Speed_North = UbxVelNed.VEL_N;
335	GPSData.Speed_Top = -UbxVelNed.VEL_D;	330	GPSData.Speed_Top = -UbxVelNed.VEL_D;
336	GPSData.Speed_Ground = UbxVelNed.GSpeed;	331	GPSData.Speed_Ground = UbxVelNed.GSpeed;
337	GPSData.Heading = UbxVelNed.Heading;	332	GPSData.Heading = UbxVelNed.Heading;
338		333
339	GPSData.Status = NEWDATA; // new data available	334	GPSData.Status = NEWDATA; // new data available
340	} // EOF if(GPSData.Status != NEWDATA)	335	} // EOF if(GPSData.Status != NEWDATA)
341	} // EOF all ubx messages received	336	} // EOF all ubx messages received
342	// set state to collect new data	337	// set state to collect new data
343	UbxSol.Status = PROCESSED; // ready for new data	338	UbxSol.Status = PROCESSED; // ready for new data
344	UbxPosLlh.Status = PROCESSED; // ready for new data	339	UbxPosLlh.Status = PROCESSED; // ready for new data
345	UbxVelNed.Status = PROCESSED; // ready for new data	340	UbxVelNed.Status = PROCESSED; // ready for new data
346	}	341	}
347	}	342	}
348		343
349		344
350	/********************************************************/	345	/********************************************************/
351	/* UBX Parser */	346	/* UBX Parser */
352	/********************************************************/	347	/********************************************************/
353	void UBX_RxParser(u8 c)	348	void UBX_RxParser(u8 c)
354	{	349	{
355	static ubxState_t ubxState = UBXSTATE_IDLE;	350	static ubxState_t ubxState = UBXSTATE_IDLE;
356	static u8 ubxclass = UBX_CLASS_UNDEF;	351	static u8 ubxclass;
-		352	static u8 ubxid;
357	static u16 msglen;	353	static u16 msglen;
358	static u8 cka, ckb;	354	static u8 cka, ckb;
359	static u8 ubxP, ubxEp, *ubxSp; // pointers to data currently transfered	355	static u8 ubxP, ubxEp, *ubxSp; // pointers to data currently transfered
360		356
361		357
362	//state machine	358	//state machine
363	switch (ubxState) // ubx message parser	359	switch (ubxState) // ubx message parser
364	{	360	{
365	case UBXSTATE_IDLE: // check 1st sync byte	361	case UBXSTATE_IDLE: // check 1st sync byte
366	if (c == UBX_SYNC1_CHAR) ubxState = UBXSTATE_SYNC1;	362	if (c == UBX_SYNC1_CHAR) ubxState = UBXSTATE_SYNC1;
367	else ubxState = UBXSTATE_IDLE; // out of synchronization	363	else ubxState = UBXSTATE_IDLE; // out of synchronization
368	break;	364	break;
369		365
370	case UBXSTATE_SYNC1: // check 2nd sync byte	366	case UBXSTATE_SYNC1: // check 2nd sync byte
371	if (c == UBX_SYNC2_CHAR) ubxState = UBXSTATE_SYNC2;	367	if (c == UBX_SYNC2_CHAR) ubxState = UBXSTATE_SYNC2;
372	else ubxState = UBXSTATE_IDLE; // out of synchronization	368	else ubxState = UBXSTATE_IDLE; // out of synchronization
373	break;	369	break;
374		370
375	case UBXSTATE_SYNC2: // check msg class to be NAV	371	case UBXSTATE_SYNC2: // check msg class to be NAV
376	ubxclass = c;	372	ubxclass = c;
377	ubxState = UBXSTATE_CLASS;	373	ubxState = UBXSTATE_CLASS;
378	break;	374	break;
379		375
380	case UBXSTATE_CLASS: // check message identifier	376	case UBXSTATE_CLASS: // check message identifier
-		377	ubxid = c;
-		378	ubxState = UBXSTATE_LEN1;
-		379	cka = ubxclass + ubxid;
-		380	ckb = ubxclass + cka;
-		381	break;
-		382
-		383	case UBXSTATE_LEN1: // 1st message length byte
-		384	msglen = (u16)c; // lowbyte first
-		385	cka += c;
-		386	ckb += cka;
-		387	ubxState = UBXSTATE_LEN2;
-		388	break;
-		389
-		390	case UBXSTATE_LEN2: // 2nd message length byte
-		391	msglen += ((u16)c)<<8; // high byte last
-		392	cka += c;
-		393	ckb += cka;
-		394
381	switch(ubxclass)	395	switch(ubxclass)
382	{	396	{
383	case UBX_CLASS_NAV:	397	case UBX_CLASS_NAV:
384	switch(c)	398	switch(ubxid)
385	{	399	{
386	case UBX_ID_POSLLH: // geodetic position	400	case UBX_ID_POSLLH: // geodetic position
387	ubxP = (u8 *)&UbxPosLlh; // data start pointer	401	ubxP = (u8 *)&UbxPosLlh; // data start pointer
388	ubxEp = (u8 *)(&UbxPosLlh + 1); // data end pointer	402	ubxEp = (u8 *)(&UbxPosLlh + 1); // data end pointer
389	ubxSp = (u8 *)&UbxPosLlh.Status; // status pointer	403	ubxSp = (u8 *)&UbxPosLlh.Status; // status pointer
390	break;	404	break;
391		405
392	case UBX_ID_SOL: // navigation solution	406	case UBX_ID_SOL: // navigation solution
393	ubxP = (u8 *)&UbxSol; // data start pointer	407	ubxP = (u8 *)&UbxSol; // data start pointer
394	ubxEp = (u8 *)(&UbxSol + 1); // data end pointer	408	ubxEp = (u8 *)(&UbxSol + 1); // data end pointer
395	ubxSp = (u8 *)&UbxSol.Status; // status pointer	409	ubxSp = (u8 *)&UbxSol.Status; // status pointer
396	break;	410	break;
397		411
398	case UBX_ID_VELNED: // velocity vector in tangent plane	412	case UBX_ID_VELNED: // velocity vector in tangent plane
399	ubxP = (u8 *)&UbxVelNed; // data start pointer	413	ubxP = (u8 *)&UbxVelNed; // data start pointer
400	ubxEp = (u8 *)(&UbxVelNed + 1); // data end pointer	414	ubxEp = (u8 *)(&UbxVelNed + 1); // data end pointer
401	ubxSp = (u8 *)&UbxVelNed.Status; // status pointer	415	ubxSp = (u8 *)&UbxVelNed.Status; // status pointer
402	break;	416	break;
403		417
404	default: // unsupported identifier	418	default: // unsupported identifier
405	ubxState = UBXSTATE_IDLE;	419	ubxState = UBXSTATE_IDLE;
406	return;	420	return;
407	}	421	}
408	break;	422	break;
409		423
410	case UBX_CLASS_ACK:	424	default: // other classes
411	switch(c)	-
-		425	if(UbxMsg.Status == NEWDATA) ubxState = UBXSTATE_IDLE;
412	{	426	else if(((UbxMsg.Hdr.Class&UbxMsg.ClassMask) == (ubxclass&UbxMsg.ClassMask)) && ((UbxMsg.Hdr.Id&UbxMsg.IdMask) == (ubxid&UbxMsg.IdMask)))
413	case UBX_ID_ACK_ACK:	427	{ // buffer is free and message matches to filter criteria
414	UbxAck.Ack = 1;	428	UbxMsg.Status = INVALID;
415	break;	-
416		429	UbxMsg.Hdr.Class = ubxclass;
417	case UBX_ID_ACK_NAK:	430	UbxMsg.Hdr.Id = ubxid;
418	UbxAck.Ack = 0;	431	UbxMsg.Hdr.Length = msglen;
419	break;	-
420		432	ubxP = (u8 *)&(UbxMsg.Data); // data start pointer
421	default:	433	ubxEp = (u8 *)(&UbxMsg + 1); // data end pointer
422	ubxState = UBXSTATE_IDLE;	-
423	return;	434	ubxSp = (u8 *)&UbxMsg.Status; // status pointer
424	}	-
425	ubxP = (u8 *)&(UbxAck.clsID); // data start pointer	-
426	ubxEp = (u8 *)(&UbxAck + 1); // data end pointer	-
427	ubxSp = (u8 *)&UbxAck.Status; // status pointer	-
428	break;	-
429		-
430	default: // unsupported class	435	}
431	ubxState = UBXSTATE_IDLE;	436	else ubxState = UBXSTATE_IDLE;
432	break;	437	break;
433	}	438	}
434	if (ubxState != UBXSTATE_IDLE)	439	if(ubxState != UBXSTATE_IDLE)
435	{	-
436	ubxState = UBXSTATE_LEN1;	-
437	cka = UBX_CLASS_NAV + c;	-
438	ckb = UBX_CLASS_NAV + cka;	-
439	}	-
440	break;	-
441		-
442	case UBXSTATE_LEN1: // 1st message length byte	-
443	msglen = (u16)c; // lowbyte first	-
444	cka += c;	-
445	ckb += cka;	-
446	ubxState = UBXSTATE_LEN2;	-
447	break;	-
448		-
449	case UBXSTATE_LEN2: // 2nd message length byte	-
450	msglen += ((u16)c)<<8; // high byte last	-
451	cka += c;	-
452	ckb += cka;	440	{
453	// if the old data are not processed so far then break parsing now	441	// if the old data are not processed so far then break parsing now
454	// to avoid writing new data in ISR during reading by another function	442	// to avoid writing new data in ISR during reading by another function
455	if ( *ubxSp == NEWDATA )	443	if ( *ubxSp == NEWDATA )
456	{	444	{
457	ubxState = UBXSTATE_IDLE;	445	ubxState = UBXSTATE_IDLE;
458	if(ubxclass == UBX_CLASS_NAV) Update_GPSData(); //update GPS info respectively	446	if(ubxclass == UBX_CLASS_NAV) Update_GPSData(); //update GPS info respectively
459	}	447	}
460	else // data invalid or allready processd	448	else // data invalid or allready processed
461	{	449	{
462	*ubxSp = INVALID; // mark invalid during buffer filling	450	*ubxSp = INVALID; // mark invalid during buffer filling
-		451	ubxState = UBXSTATE_DATA;
463	ubxState = UBXSTATE_DATA;	452	}
464	}	453	}
465	break;	454	break;
466		455
467	case UBXSTATE_DATA: // collecting data	456	case UBXSTATE_DATA: // collecting data
468	if (ubxP < ubxEp)	457	if (ubxP < ubxEp)
469	{	458	{
470	*ubxP++ = c; // copy curent data byte if any space is left	459	*ubxP++ = c; // copy curent data byte if any space is left
471	cka += c;	460	cka += c;
472	ckb += cka;	461	ckb += cka;
473	if (--msglen == 0) ubxState = UBXSTATE_CKA; // switch to next state if all data was read	462	if (--msglen == 0) ubxState = UBXSTATE_CKA; // switch to next state if all data was read
474	}	463	}
475	else // rx buffer overrun	464	else // rx buffer overrun
476	{	465	{
477	ubxState = UBXSTATE_IDLE;	466	ubxState = UBXSTATE_IDLE;
478	}	467	}

Subversion Repositories NaviCtrl

(root)/tags/V2.14e/ubx.c @ 789 - Rev 245 → 247