savedata.c

GPS导航定位程序

#include "includes.h"

/****************************************************************************    
* Function: void SaveData(void)
*
* Saves fix data to an ASCII file in a comma delimited format.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void SaveData(void)
{
    char buffer[960];
    
    unsigned coasting;

    unsigned long cTIC;

    int year,month,day,hour,minute,second,gwk,chan,sv,activity;
    int corlk,carfrlk,bitlk,framelk,used,visible;
    int nav_mode,dgps_mode,length;

    static time_t time_of_last_log=0;
    time_t time_now;
    
    double dtemp,gsec;

    navstatestruc N;

    /* First check if it's time to save the next fix */

    time(&time_now);

    TimeToNextLog = DataLogInterval - (long)(time_now - time_of_last_log);

    if(TimeToNextLog>0) /* Not time yet */
        return;

    time_of_last_log = time_now;

    PROTECT++;       
    N = CurNavState; /* Get a local copy of the current navigation state */
    PROTECT--;

    /* Check if the fix restriction criterion has been exceeded. */

    if(N.hgt>HEIGHT_LIMIT || N.speed>SPEED_LIMIT)
    {
        N.lat = 99.9/R2D;
        N.lon = 999.9/R2D;
        N.hgt = 99999.0;
        N.speed = 999.9;
        N.hdg = 999.9;
    }

    length = sprintf(&buffer[0],"%.1d,%10.5lf,%9.5lf,%5.0lf,",
                     ReceiverID,N.lon*R2D,N.lat*R2D,N.hgt);

    /* Do the following if you want the records to contain the time of
       the last nav solution in UTC */

//  TICToGpsTime(N.toa,&gwk,&gsec);        /* Get time of nav solution */
//  GpsTimeToUTCDate(gwk,gsec,&year,&month,&day,&hour,&minute,&dtemp); /* UTC time */

    /* Do the following if you want the records stamped with GPS
       time regardless of the time of the last nav solution */

//  TICToGpsTime(cTIC,&gwk,&gsec);                         /* Get current time */
//  gpsgrg(gwk,gsec,&year,&month,&day,&hour,&minute,&dtemp);  /* GPS time */

    /* Do the following if you want the records stamped with UTC
       time regardless of the time of the last nav solution */

    CurrentTIC(&cTIC);
    TICToGpsTime(cTIC,&gwk,&gsec);           /* Get current time */
    GpsTimeToUTCDate(gwk,gsec,&year,&month,&day,&hour,&minute,&dtemp);  /* UTC time */
    second = (int)dtemp;

    length += sprintf(&buffer[length],"%.2d,%.2d,%.2d,%.2d,%.2d,%.2d,%.1lf,0,0,",
                    hour,minute,second,day,month,year-1900,N.pdop);

    visible = 0;

    for(chan=0; chan<MAXCHANNELS; chan++)
    {
        activity = 0;
        sv = CH[chan].SV;
        activity += sv*10;

        if(ielvd[sv-1]>=ElvMask)
            visible++;

        if(sv!=0)
        {
            disable(); 
            corlk = CH[chan].corlk;
            carfrlk = CH[chan].carfrlk;
            bitlk = CH[chan].bitlk;
            framelk = CH[chan].FrameSync;
            coasting = CH[chan].coasting;
            enable();

            if(N.svnavmat&(1UL<<(sv-1)))
                used = TRUE;
            else
                used = FALSE;

            if(corlk)
            {
                activity++;                       /* 1 */
                if(carfrlk)
                {
                    activity++;                   /* 2 */
                    if(bitlk)
                    {
                        activity++;               /* 3 */
                        if(framelk)
                        {
                            activity++;           /* 4 */
                            if(used)
                            {
                                activity += 2;    /* 6 */
                                if(!coasting)
                                    activity++;   /* 7 */
                            }
                            else if(!coasting)
                                activity++;       /* 5 */
                        }
                    }
                }
            }
        }
        length += sprintf(&buffer[length],"%.3d,",activity);
    }

    if(N.navmode==FIX_NO)
        nav_mode = 0;
    else if(N.navmode==FIX_2D)              
        nav_mode = 2;
    else if(N.navmode==FIX_3D)
        nav_mode = 3;

    if(TrackMode==COLD_START)
        dgps_mode = 0;
    else if(N.diff==FALSE)              
        dgps_mode = 1;
    else if(N.diff==TRUE)
        dgps_mode = 2;

    length += sprintf(&buffer[length],"%5.1lf,%5.1lf,%.1d,%.1d,%.2d,%.2d,%s\n",
    N.speed,N.hdg,nav_mode,dgps_mode,visible,N.nsats,DataLogComment);

    if(fpDataLog!=NULL)
    {
        fprintf(fpDataLog,"%s",buffer);
        DataRecordsLogged++;
    }

    if(DataLogSerial==TRUE)
    {
        length = 0;
        while(buffer[length]!='\0') 
            _bios_serialcom(_COM_SEND,0,buffer[length++]);
    }
}