T536_DTU/app_public/gps/gps_uart.c

/******************************************************************************
版权所有：
文件名称：	gprs.c
文件版本：	01.01
创建作者：
创建日期：	2012-8-17
功能说明：	GRS模块
其它说明：
修改记录：
*/

/*------------------------------- 头文件 --------------------------------------
 */
#include "head.h"
#include "gps_uart.h"
#include "my_rtc.h"
#include "gps.h"
#include "bspconfig.h"

#if defined(ADD_ATGM366_GPS_FUN) || defined(DEV_GPS_ATGM366) || defined(DEV_GPS_ATGM332D)

#define BYTE2LONG(p) ((long)(((p)[0] << 24) + ((p)[1] << 16) + ((p)[2] << 8) + (p)[3]))
/*------------------------------ 全局变量 -------------------------------------
 */

GpsNmeaRmcDataType GpsNmeaRmcData;
u8 SendBuf[GPS_LINE_LEN / 2];

const GpsNmeaDataType GpsNmeaDataTbl[] =
    {
        {FALSE, "GGA"},
        {FALSE, "GLL"},
        {FALSE, "GSA"},
        {FALSE, "GSV"},
        {TRUE, "RMC"},
        {FALSE, "VTG"},
        {FALSE, "ZDA"},
        {FALSE, "TXT"},
        {FALSE, "ANT"},
        {FALSE, "DHV"},
        {FALSE, "GST"},

};

const char GpsNmeaCmdData[][50] =
    {
        {"$PCAS01,0*1C\r\n"},
        {"$PCAS01,1*1D\r\n"},
        {"$PCAS01,2*1E\r\n"},
        {"$PCAS01,3*1F\r\n"},
        {"$PCAS01,4*18\r\n"}, // 57600
        {"$PCAS01,5*19\r\n"},
        //    {"$PCAS03,0,0,0,0,1,0,0,0,0,0,,,0,0*03\r\n"},   //配置只上发RMC指令
        {"$PCAS03,0,0,0,4,1,0,0,0*07\r\n"}, // 配置发RMC、GSV指令

        {"$PCAS04,3*1A\r\n"},    // 配置选择北斗和GPS导航系统
        {"$PCAS02,1000*2E\r\n"}, // 更新频率为1HZ
        {"$PCAS10,3*1F\r\n"},    // 恢复出厂设置
        {"$PCAS04,2*1B\r\n"},    // 单独选择北斗
        {"$PCAS04,1*18\r\n"},    // 单独选择GPS
};

const int GpsCommBuadrateTbl[] =
    {
        4800, 9600, 19200, 38400, 57600, 115200};

/*------------------------------ 外部函数 -------------------------------------
外部函数供其它实体文件引用，必须仔细检查传入参数的合法性.
*/
extern int rtc_to_timespec(struct rtc_time_t *p_rtc, struct timespec *p_ts);

GPS_SYNC_DELAY gps_sync;
extern unsigned int g_keep_alive_gps;

/******************************************************************************
函数名称：
函数版本：  01.01
创建作者：
创建日期：
函数说明：
参数说明：
返回值：
    0：     成功
    其它：  失败
修改记录：
*/
extern struct timespec g_sys_time;

char gps_crc(const char *p, u8 length)
{
    char crc = p[0];
    u8 i;

    for (i = 1; i < length; i++)
    {
        crc ^= p[i];
    }

    return crc;
}

void GPSCommInit(GPS_COMM *p, u8 chnl) // 初始化
{
    memset(p, 0, sizeof(GPS_COMM));
    p->type = IEC_MODBUS_M;
    p->chnl = chnl;
    p->bData = false;

    // p->arrSendBuf=&g_tRsComm[chnl].sendbuf[0];

    ResetUartGPSLink(p);
    // rt_printf("gps com init\r\n");   //TODO ygl -
}

void ResetUartGPSLink(GPS_COMM *p) // 重置
{
    p->cTypeCounter = GPS_FRAME_HEAD;
    p->cRecvCnt = 0;
}

void GPS_Recv(GPS_COMM *p, BYTE byRevData)
{
    static unsigned char RecvCounter;

    if (p->bData)
    {
        p->cTypeCounter = GPS_FRAME_HEAD;
        return;
    }
    p->arrRecvBuf[p->cRecvCnt++] = byRevData; // 接收数据
    switch (p->cTypeCounter)
    {
    case GPS_FRAME_HEAD:      //
        if (byRevData == '$') // 开始符号
        {
            //             rt_printf("GPS_FRAME_HEAD\r\n");
            p->cTypeCounter++;
        }
        else
        {
            ResetUartGPSLink(p);
        }
        break;
    case GPS_FRAME_DATA:
        if (byRevData == '*') // 数据结束段
        {
            //             rt_printf("GPS_FRAME_DATA\r\n");
            RecvCounter = 0;
            p->cTypeCounter = GPS_FRAME_TAIL;
            // p->nTypeCounter=GPS_FRAME_TAIL;
        }
        else if (p->cRecvCnt > GPS_UART_MAX_LEN) // 数据异常保护后续需要修改
                                                 // else if(p->nRecvCnt>GPS_UART_MAX_LEN)  //数据异常保护后续需要修改
        {
            ResetUartGPSLink(p);
        }
        break;
    case GPS_FRAME_TAIL:
        if (byRevData == '\n') // 回车符
        {
            p->cTypeCounter = GPS_FRAME_HEAD;
            // p->nTypeCounter=GPS_FRAME_HEAD;
            p->bData = true;
            //	rt_printf("GPS_FRAME_TAIL\r\n");
            //	rt_printf("data: %s\r\n", p->arrRecvBuf);
        }
        else if (++RecvCounter >= 4)
        {
            ResetUartGPSLink(p);
        }
        break;
    default:
        ResetUartGPSLink(p);
        break;
    }
}

void GPS_Timer(void)
{
    u8 i;

    // TODO 待处理秒脉冲 by ewen
    // if(g_clock_mode != CLOCK_MODE_GPS_SECOND)
    // {
    // 	if(soe_check(EV_GPS_ADJUST_TIME)==true)
    // 	{
    // 		soe_record_ev(EV_GPS_ADJUST_TIME,0, 0,0,0 );
    // 	}
    return;
    // }

    if (ustimer_delay_origin2(&GpsNmeaRmcData.data_vaild_time, 60 * tRunPara.GPS_switch_delay * USTIMER_SEC))
    {
        GpsNmeaRmcData.data_vaild = 0;
        // TODO by ewen
        // if(soe_check(EV_GPS_ADJUST_TIME)==true)
        // {
        // 	soe_record_ev(EV_GPS_ADJUST_TIME,0, 0,0,0 );
        // }
    }
    if (ustimer_delay_origin2(&GpsNmeaRmcData.gsv_time, 30 * USTIMER_SEC))
    {
        GpsNmeaRmcData.bd_sv_num = 0;
        GpsNmeaRmcData.gp_sv_num = 0;
    }

    for (i = 0; i < GPS_NUM_MAX; i++)
    {
        if (ustimer_delay_origin2(&GpsNmeaRmcData.bd_online_time[i], 180 * USTIMER_SEC))
        {
            GpsNmeaRmcData.bd_gsv[i].valid = 0;
        }
        if (ustimer_delay_origin2(&GpsNmeaRmcData.gp_online_time[i], 180 * USTIMER_SEC))
        {
            GpsNmeaRmcData.gp_gsv[i].valid = 0;
        }
    }

    if (GpsNmeaRmcData.data_vaild == 0)
    {
#ifdef GPS_ALIVE_SOE
        if (soe_check(EV_GPS_ALIVE) == true)
        {
            soe_record_ev(EV_GPS_ALIVE, 0, 0, 0, 0);
        }
#endif
        GpsNmeaRmcData.bd_sv_num = 0;
        GpsNmeaRmcData.gp_sv_num = 0;
    }
#ifdef GPS_NUM_SHOW
    g_sw_pub.ac_in[PUB_AC_IN_GPS_NUM] = (qs16)((GpsNmeaRmcData.bd_sv_num + GpsNmeaRmcData.gp_sv_num) * Q16_BASE);
#elif defined BD_GPS_NUM
    g_sw_pub.ac_in[PUB_AC_IN_BD_NUM] = (qs16)(GpsNmeaRmcData.bd_sv_num * Q16_BASE);
    g_sw_pub.ac_in[PUB_AC_IN_GPS_NUM] = (qs16)(GpsNmeaRmcData.gp_sv_num * Q16_BASE);
#endif
}

void GPS_Comm_App(GPS_COMM *p)
{

#ifdef ADD_PLUSE_OUTPUT
    GenerateSecdPluse();
#endif

    GPS_Timer();
    //    if(!g_clock_mode_gps)
    //        return;

    GpsModuleFunInit(p);
    if (GpsNmeaRmcData.DataValidFlag == 'S') // soeê??t
    {
        struct rtc_time_t rtc;
        // TODO by ewen
        // 此处写RTC，但是在设置系统时间时已交由系统写入此处不用再写
        // soe_record_opt(EV_GPS_ADJUST_TIME,GpsNmeaRmcData.MsOffset);

        // if(soe_check(EV_GPS_ADJUST_TIME)==false)
        // {
        // 	soe_record_ev(EV_GPS_ADJUST_TIME,1, 0,0,0 );
        // }

        GpsNmeaRmcData.gps_soe_time = ustimer_get_origin();

        timespec_to_rtc(GpsNmeaRmcData.ts, &rtc, 1);
        if (rtc_time_write(&rtc) != 0) //
        {
            rt_printf("gps_fun:rtc_time_write err.\r\n");
            return;
        }
        GpsNmeaRmcData.DataValidFlag = '0';
        //   rt_printf_time("gps adjust,time=%d\r\n",GpsNmeaRmcData.MsOffset);
    }

    if (p->bData) // TODO: add vaild data control
    {
        GpsRecvDataFrameHandler(p);
        p->bData = false;
        p->cRecvCnt = 0;
    }
}

int _pow10(unsigned char num)
{
    int val = 1;
    while (num--)
        val *= 10;

    return val;
}

unsigned char *GetParamDataValue(unsigned char *pos, float *data) //, ,
{
    unsigned char count, IntNum = 0, DecNum = 0, len, num;
    unsigned char *p = pos, *p1;
    int value;

    p++; // jump the first signed
    // len=(unsigned char*)strstr(p,",")-p;
    len = (unsigned char *)strchr(p, ',') - p;
    if ((len <= 0) || len > 15)
    {
        *data = 0; // valid data
        return pos + len + 1;
    }

    p1 = p;
    for (count = 0; count < len; count++, p1++)
    {
        if (*p1 == '.') // 取到小数点位置
        {
            DecNum = len - IntNum - 1;
            break;
        }
        IntNum++;
    }

    // rt_printf("len=%d,InterNum=%d,DecNum=%d,x=%d\r\n",len,IntNum,DecNum,pow10(IntNum));

    value = 0;
    while (IntNum) // get the part of Int
    {
        num = *p++ - 0x30; // 必须为0x30-0x39
        value += num * _pow10(IntNum - 1);
        IntNum--;
    };
    *data = (float)value;
    if (DecNum)
    {
        p++; // point
        count = DecNum;
        value = 0;
        while (count) // get the part of dec
        {
            num = *p++ - 0x30; // 必须为0x30-0x39
            value += num * _pow10(count - 1);
            count--;
        };
        *data += (float)value / _pow10(DecNum);
    }

    return pos + len + 1;
}

// 取字符
unsigned char *GetParamDataChar(unsigned char *pos, unsigned char *data, int max_len)
{
    unsigned char len;
    unsigned char *p = pos;
    // int value;

    p++; // jump the first signed
    // len=(unsigned char*)strstr(p,",")-p;
    len = (unsigned char *)strchr(p, ',') - p;
    // rt_printf("len=%d\r\n",len);
    if ((len <= 0) || len > max_len)
    {
        *data = 0; // valid data
        return pos + len + 1;
    }
    memcpy(data, p, len);
    return pos + len + 1;
}

char *gps_char_data_get(char *pos, char end, int *data)
{
    unsigned char len;

    len = strchr(pos, end) - pos;
    if ((len <= 0) || len > 15)
    {
        *data = -1; // valid data
        return pos + len + 1;
    }
    *data = atoi(pos);

    return pos + len + 1;
}

int CheckDataFrameVaild(unsigned char *pos)
{
    unsigned char *p = pos, tmp;
    volatile unsigned char AddValue = 0;

    // if(strstr(p,"*")==NULL)
    if (strchr(p, '*') == NULL)
        return -1;
    if (*p++ != '$')
        return -1;

    while (*p != '*')
        AddValue ^= *p++;

    p++; //*
    tmp = (*p++ - 0x30) << 4;
    tmp |= (*p - 0x30) << 0;

    rt_printf("tmp=0x%02X,AddValue=0x%02X\r\n", tmp, AddValue);

    if (tmp != AddValue)
    {
        rt_printf("tmp=%s\r\n", pos);
        return 0;
    }

    return 1;
}

void UtcTimeToSysTime(struct rtc_time_t *rtc, GpsNmeaRmcDataType *utc, u8 opt)
{

    char *p = &utc->Date[0];

    rtc->day = (*p - 0x30) * 10 + *(p + 1) - 0x30;
    p += 2;
    rtc->month = (*p - 0x30) * 10 + *(p + 1) - 0x30;
    p += 2;
    rtc->year = (*p - 0x30) * 10 + *(p + 1) - 0x30;
    p += 2;

    p = &utc->UtcTime[0];
    rtc->hour = (*p - 0x30) * 10 + *(p + 1) - 0x30;
    p += 2;
    rtc->min = (*p - 0x30) * 10 + *(p + 1) - 0x30;
    p += 2;

    if (opt == 1)
        rtc->ms = ((*p - 0x30) * 10 + *(p + 1) - 0x30) * 1000 + (*(p + 3) - 0x30) * 100 + (*(p + 4) - 0x30) * 10 + *(p + 5) - 0x30;
    else // 取S
        rtc->ms = ((*p - 0x30) * 10 + *(p + 1) - 0x30) * 1000;
    // rt_printf("year=%d,month=%d,day=%d,hour=%d,min=%d,ms=%d\r\n",rtc->year,rtc->month,rtc->day,rtc->hour,rtc->min,rtc->ms);
}

void gps_pluse_clear(void)
{
    GpsNmeaRmcData.pluse_count = 0;
}

int GpsAdjustTimeFun(struct rtc_time_t *psrtc) // 本函数只能被捕获中断服务函数调用
{
    //   static u8 counter=0;
    struct timespec ts;
    int res = FALSE;
    u64 MsOffset;
    static u8 power_on = 1;

#ifdef CN_AREA_GUOWANG_FBC
    GpsNmeaRmcData.data_vaild_time = ustimer_get_origin();
    GpsNmeaRmcData.data_vaild = 1;
    if (GpsNmeaRmcData.slave_vaild == 0)
        return res;
#endif

    if (++GpsNmeaRmcData.pluse_count <= tRunPara.GPS_sync_time) // 根据对时间隔判断修正
    {
        //        GpsNmeaRmcData.DataValidFlag='0';
        return res;
    }
    GpsNmeaRmcData.wait_sync = 0;

//    counter=0;
#ifdef CN_AREA_GUOWANG_FBC
    GpsNmeaRmcData.slave_vaild = 0;
#else
    if (GpsNmeaRmcData.DataValidFlag != 'Y' || !tRunPara.bGPS_sync_enable)
        return res;
#endif

    GpsNmeaRmcData.DataValidFlag = '0';

    // rtc_to_timespec(psrtc,&ts);
#ifdef CN_AREA_GUOWANG_FBC
    GpsNmeaRmcData.ts.tv_sec += tRunPara.GPS_sync_time + 1;
    GpsNmeaRmcData.ts.tv_nsec = 20 * 1000 + 1000 * 1000; // 补偿20us + 1ms
#else
    GpsNmeaRmcData.ts.tv_sec += 1;         // tRunPara.GPS_sync_time + 1; //TODO EWEN
    GpsNmeaRmcData.ts.tv_nsec = 20 * 1000; // 补偿20us
#ifdef KP_GPS_TEST
    GpsNmeaRmcData.ts.tv_sec += 4;
#endif
#endif

    gps_get_time(&ts); // 当前系统的精准时间带ms的

    if (GpsNmeaRmcData.ts.tv_sec <= ts.tv_sec) // 系统时间超前
        MsOffset = (ts.tv_sec - GpsNmeaRmcData.ts.tv_sec) * 1000 + ts.tv_nsec / 1000000;
    else // 系统时间滞后
        MsOffset = (GpsNmeaRmcData.ts.tv_sec - ts.tv_sec) * 1000 - ts.tv_nsec / 1000000;

    if (MsOffset >= 5 || power_on) // 5ms
    {
        timespec_to_rtc(GpsNmeaRmcData.ts, psrtc, 1); // D￡ê±
        sys_time_set(psrtc);
        if (MsOffset >= 1000 || power_on) // 1Sò?é??ó2?????SOE
        {
            power_on = 0;
            GpsNmeaRmcData.DataValidFlag = 'S'; // SOE±ê??
            GpsNmeaRmcData.MsOffset = MsOffset;
        }
        else
            GpsNmeaRmcData.DataValidFlag = '0';
        //  rt_printf("set time=%d\r\n",MsOffset);   //校准
        res = TRUE;
    }
    else
        GpsNmeaRmcData.DataValidFlag = '0';

    return res;
}

float gps_angle_calc(float value)
{
    float tmp_d, tmp_m, tmp_dot;
    u32 tmp;

    tmp = (u32)value;
    tmp_d = value;
    tmp_m = (float)(tmp_d - tmp);
    tmp_dot = tmp_m * 100.0f / 60.0f;
    tmp = value;
    return (float)(tmp + tmp_dot);
}

void gps_dms_calc(float value, u32 *d, u32 *m, float *s)
{
    u32 tmp1, tmp2;

    tmp1 = (u32)value;
    tmp2 = (u32)value / 100;

    *d = tmp2;
    *m = (u32)((value / 100.0f - tmp2) * 100.0f);
    *s = (float)(value - tmp1) * 60.0f;
}

// GPS接收数据解析
int GpsRecvDataFrameHandler(GPS_COMM *data)
{
    static unsigned char LocalUtcTime[11];
    unsigned char *p = &data->arrRecvBuf[0];
    unsigned char *p1 = &data->arrRecvBuf[0];
    u8 msg_no, valid_flag;

    if ((ustimer_delay_origin2(&data->recv_wait_time, 3 * USTIMER_SEC) <= 0) && !data->recv_handle)
    {
        return FALSE;
    }
    data->recv_handle = 1;

    if (data->GpsModuleRunStep == GPS_MODULE_INIT_STEP) //???????????????????????????
        data->GpsModuleRunStep = GPS_MODULE_NORMAL_RUN_STEP;
    else if (data->GpsModuleRunStep == GPS_MODULE_BAUD_CHECK_STEP) // 查找到合适的波特率
        data->GpsModuleRunStep = GPS_MODULE_BAUD_SET_STEP;

    // if(!CheckDataFrameVaild(p))
    //     return;

    // $GNRMC,051033.000,A,2222.12622,N,11329.24936,E,0.00,0.00,050825,,,A*79
    if (strstr(p1, "RMC") != NULL)
    {

        p = strstr(p1, "RMC");
        memcpy(&GpsNmeaRmcData.SingleType, p1 + 1, 2); // 记录是BD还是GN,即北斗还是GPS
        GpsNmeaRmcData.SingleType[p - p1] = 0;

        p += 3;
        p = GetParamDataChar(p, &GpsNmeaRmcData.UtcTime[0], sizeof(GpsNmeaRmcData.UtcTime));
        p = GetParamDataChar(p, &valid_flag, sizeof(valid_flag)); // 数据有效标志
        if (valid_flag != 'A')
        {
            GpsNmeaRmcData.bd_sv_num = 0;
            GpsNmeaRmcData.gp_sv_num = 0;
            g_sw_pub.ac_in[PUB_AC_GPS_NUM_SV] = 0;
#ifdef GPS_NUM_SHOW
            g_sw_pub.ac_in[PUB_AC_IN_GPS_NUM] = (qs16)((GpsNmeaRmcData.bd_sv_num + GpsNmeaRmcData.gp_sv_num) * Q16_BASE);
#elif defined BD_GPS_NUM
            g_sw_pub.ac_in[PUB_AC_IN_BD_NUM] = (qs16)(GpsNmeaRmcData.bd_sv_num * Q16_BASE);
            g_sw_pub.ac_in[PUB_AC_IN_GPS_NUM] = (qs16)(GpsNmeaRmcData.gp_sv_num * Q16_BASE);
#endif

            return FALSE;
        }

        GpsNmeaRmcData.data_vaild_time = ustimer_get_origin();
        GpsNmeaRmcData.data_vaild = 1;
#ifdef GPS_ALIVE_SOE
        if (soe_check(EV_GPS_ALIVE) == false)
        {
            soe_record_ev(EV_GPS_ALIVE, 1, 0, 0, 0);
        }
#endif
        if (GpsNmeaRmcData.wait_sync)
        {
            // return FALSE;//临时屏蔽 by ewen
        }

        GpsNmeaRmcData.DataValidFlag = valid_flag;
        p = GetParamDataValue(p, &GpsNmeaRmcData.Latitude); // 纬度

        p = GetParamDataChar(p, &GpsNmeaRmcData.LatitudeDir, sizeof(GpsNmeaRmcData.LatitudeDir)); // 纬度方位

        p = GetParamDataValue(p, &GpsNmeaRmcData.Longtitude); // 经度

        p = GetParamDataChar(p, &GpsNmeaRmcData.LongtitudeDir, sizeof(GpsNmeaRmcData.LongtitudeDir)); // 经度方位

        p = GetParamDataValue(p, &GpsNmeaRmcData.Spd); // 对地速度

        p = GetParamDataValue(p, &GpsNmeaRmcData.cog); // 对地真航向

        p = GetParamDataChar(p, &GpsNmeaRmcData.Date[0], sizeof(GpsNmeaRmcData.Date)); // 日期

        gps_pluse_clear();
        GpsNmeaRmcData.wait_sync = 1;
        memset(&GpsNmeaRmcData.UtcTime[8], '0', 3); // 去除毫秒 GPS模块机制
        //    if(memcmp(GpsNmeaRmcData.UtcTime,LocalUtcTime,sizeof(GpsNmeaRmcData.UtcTime)))  //若gps第一次获取到时间或者1s内第一次获取到时间则有效或者是GPS断线后第一次获取的时间
        {
            struct rtc_time_t rtc;
            UtcTimeToSysTime(&rtc, &GpsNmeaRmcData, 0); // 先将BD的UTC时间转为RTC
            rtc_to_timespec(&rtc, &GpsNmeaRmcData.ts);  // 再将RTC转为timespec时间（主要用timespec）
            GpsNmeaRmcData.ts.tv_sec += 8 * 60 * 60;    // UTC->BEIJING
            GpsNmeaRmcData.DataValidFlag = 'Y';
            // zhl add
            if (gps_time_valid == 0)
            {
                // printf("GpsNmeaRmcData.ts.tv_sec:%lld\n", GpsNmeaRmcData.ts.tv_sec);
                shared_gps_time = GpsNmeaRmcData.ts.tv_sec;
                gps_time_valid = 1;
            }

            // rt_printf("gene_flag=%d\r\n",GpsNmeaRmcData.DataValidFlag);
            memcpy(GpsNmeaRmcData.UtcTime, LocalUtcTime, sizeof(GpsNmeaRmcData.UtcTime));
            // rt_printf("recv gps uart data!");
        }
        gps_dms_calc(GpsNmeaRmcData.Longtitude, &GpsNmeaRmcData.Longtitude_dd, &GpsNmeaRmcData.Longtitude_mm, &GpsNmeaRmcData.Longtitude_ss);
        gps_dms_calc(GpsNmeaRmcData.Latitude, &GpsNmeaRmcData.Latitude_dd, &GpsNmeaRmcData.Latitude_mm, &GpsNmeaRmcData.Latitude_ss);

#ifndef GPS_JWD_SPLIT
        g_sw_pub.ac_in[PUB_AC_GPS_LONGITUDE] = (qs16)(gps_angle_calc(GpsNmeaRmcData.Longtitude / 100.0f) * Q16_BASE);
        g_sw_pub.ac_in[PUB_AC_GPS_LATITUDE] = (qs16)(gps_angle_calc(GpsNmeaRmcData.Latitude / 100.0f) * Q16_BASE);
#else
        g_sw_pub.ac_in[PUB_AC_GPS_LONGITUDE_D] = GpsNmeaRmcData.Longtitude_dd * Q16_BASE;
        g_sw_pub.ac_in[PUB_AC_GPS_LONGITUDE_M] = GpsNmeaRmcData.Longtitude_mm * Q16_BASE;
        g_sw_pub.ac_in[PUB_AC_GPS_LONGITUDE_S] = GpsNmeaRmcData.Longtitude_ss * Q16_BASE;
        g_sw_pub.ac_in[PUB_AC_GPS_LATITUDE_D] = GpsNmeaRmcData.Latitude_dd * Q16_BASE;
        g_sw_pub.ac_in[PUB_AC_GPS_LATITUDE_M] = GpsNmeaRmcData.Latitude_mm * Q16_BASE;
        g_sw_pub.ac_in[PUB_AC_GPS_LATITUDE_S] = GpsNmeaRmcData.Latitude_ss * Q16_BASE;
#endif
    }
    else if (strstr(p1, "GSV") != NULL)
    {
        if (strstr(p1, "BD") != NULL)
        {
            u8 i;

            p = p1 + 7;
            p = gps_char_data_get(p, ',', &GpsNmeaRmcData.bd_msg_num);
            p = gps_char_data_get(p, ',', &GpsNmeaRmcData.bd_msg_no);
            p = gps_char_data_get(p, ',', &GpsNmeaRmcData.bd_sv_num);
            if (strstr(GpsNmeaRmcData.SingleType, "BD") != NULL)
            {
                g_sw_pub.ac_in[PUB_AC_GPS_NUM_SV] = (qs16)(GpsNmeaRmcData.bd_sv_num * Q16_BASE);
            }
            if (GpsNmeaRmcData.bd_sv_num > GPS_NUM_MAX)
                return FALSE;

            GpsNmeaRmcData.gsv_time = ustimer_get_origin();

            msg_no = GpsNmeaRmcData.bd_msg_no - 1;
            if (GpsNmeaRmcData.bd_msg_no * 4 < GPS_NUM_MAX)
            {
                for (i = 0; i < 4; i++)
                {
                    p = gps_char_data_get(p, ',', &GpsNmeaRmcData.bd_gsv[msg_no * 4 + i].id);
                    p = gps_char_data_get(p, ',', &GpsNmeaRmcData.bd_gsv[msg_no * 4 + i].ele);
                    p = gps_char_data_get(p, ',', &GpsNmeaRmcData.bd_gsv[msg_no * 4 + i].az);
                    p = gps_char_data_get(p, ',', &GpsNmeaRmcData.bd_gsv[msg_no * 4 + i].cn0);
                    GpsNmeaRmcData.bd_gsv[msg_no * 4 + i].valid = 1;
                    GpsNmeaRmcData.bd_online_time[msg_no * 4 + i] = ustimer_get_origin();
                }
            }
        }
        else if (strstr(p1, "GP") != NULL)
        {
            u8 i;

            p = p1 + 7;
            p = gps_char_data_get(p, ',', &GpsNmeaRmcData.gp_msg_num);
            p = gps_char_data_get(p, ',', &GpsNmeaRmcData.gp_msg_no);
            p = gps_char_data_get(p, ',', &GpsNmeaRmcData.gp_sv_num);

            //	rt_printf("GPS卫星：%d台\r\n", GpsNmeaRmcData.gp_sv_num);
            if (GpsNmeaRmcData.gp_sv_num > GPS_NUM_MAX)
                return FALSE;
            if (strstr(GpsNmeaRmcData.SingleType, "GN") != NULL)
            {
                g_sw_pub.ac_in[PUB_AC_GPS_NUM_SV] = (qs16)(GpsNmeaRmcData.gp_sv_num * Q16_BASE);
            }

            GpsNmeaRmcData.gsv_time = ustimer_get_origin();
            msg_no = GpsNmeaRmcData.gp_msg_no - 1;
            if (GpsNmeaRmcData.gp_msg_no * 4 < GPS_NUM_MAX)
            {
                for (i = 0; i < 4; i++)
                {
                    p = gps_char_data_get(p, ',', &GpsNmeaRmcData.gp_gsv[msg_no * 4 + i].id);
                    p = gps_char_data_get(p, ',', &GpsNmeaRmcData.gp_gsv[msg_no * 4 + i].ele);
                    p = gps_char_data_get(p, ',', &GpsNmeaRmcData.gp_gsv[msg_no * 4 + i].az);
                    p = gps_char_data_get(p, ',', &GpsNmeaRmcData.gp_gsv[msg_no * 4 + i].cn0);
                    GpsNmeaRmcData.gp_gsv[msg_no * 4 + i].valid = 1;
                    GpsNmeaRmcData.gp_online_time[msg_no * 4 + i] = ustimer_get_origin();
                }
            }
        }
    }
    else
    {
#if 0    
        if((data->GpsModuleRunStep==GPS_MODULE_NORMAL_RUN_STEP)&&(strstr(p,"TXT")==NULL))  //正常运行后若发现收到其他数据则下发配置指令
#else
        if ((data->GpsModuleRunStep == GPS_MODULE_NORMAL_RUN_STEP) && (strstr(p, "GSV") == NULL))
#endif
        {
            data->GpsModuleRunStep = GPS_MODULE_NEMA_SEL_STEP; // 若配置不成功可循环配置
        }
    }

    return TRUE;
}

void GpsSwitchBaudrateFun(GPS_COMM *data, GpsCtrlCmdEnumType GpsCtrlCmd)
{
    int ret;
    u8 chnl = data->chnl;
    GpsModuleRunStepEnum LocalGpsModuleRunStep = data->GpsModuleRunStep;

    memset(&g_tRsComm[chnl], 0, sizeof(g_tRsComm[chnl]));
    g_tRsComm[chnl].ptBuf = (BYTE *)&g_tRsIEC[chnl];

    GPSCommInit((GPS_COMM *)g_tRsComm[chnl].ptBuf, chnl);

    // uart_open(UART_CHANNEL[chnl],tRunPara.tUartPara[i].wBaud,tRunPara.tUartPara[i].wParity);
#if defined(BSP_DTU4)
    uart_open(UART_CHANNEL[chnl], GpsCommBuadrateTbl[GpsCtrlCmd], PARITY_NONE1);
#else
    ret = uart_open(UART_CHANNEL[chnl], GpsCommBuadrateTbl[GpsCtrlCmd], PARITY_NONE_1);
    if (ret < 0)
    {
        printf("%s: outside uart %d open failed!\n", __FUNCTION__, UART_CHANNEL[chnl]);
    }
#endif
    RS_Ena_Init(chnl);
    RS_Recv_Enable(chnl);
    data->GpsModuleRunStep = LocalGpsModuleRunStep; // 串口初始化 存储的数据都被清0
    // rt_printf("Set baud:%d\r\n",GpsCommBuadrateTbl[GpsCtrlCmd]);
}

void GpsModuleFunInit(GPS_COMM *data)
{
    static GpsCtrlCmdEnumType GpsSwitchBaudrate;
    static uint32_t us0 = 0;
    u8 count, chnl = data->chnl, i;
    int ret;

    if (us0 == 0)
        us0 = ustimer_get_origin();
    if (ustimer_delay_origin2(&us0, USTIMER_SEC * 2) > 0)
    {
        //     rt_printf("Step:%d\r\n",data->GpsModuleRunStep);
        switch (data->GpsModuleRunStep)
        {
        case GPS_MODULE_INIT_STEP:
            data->GpsModuleRunStep++;
            GpsSwitchBaudrate = GPS_SET_BUADRATE1_CMD;
            break;
        case GPS_MODULE_BAUD_CHECK_STEP:
            GpsSwitchBaudrateFun(data, GpsSwitchBaudrate);
            if (++GpsSwitchBaudrate > GPS_SET_BUADRATE5_CMD)
                GpsSwitchBaudrate = GPS_SET_BUADRATE0_CMD;
            // data->GpsModuleRunStep=GPS_MODULE_BAUD_SET_STEP;
            break;
        case GPS_MODULE_BAUD_SET_STEP:
            for (count = 0; count < sizeof(GpsCommBuadrateTbl) / 4; count++)
            {
                if (tRunPara.tUartPara[data->chnl].dBaud == GpsCommBuadrateTbl[count])
                    break;
            }
            if (count == sizeof(GpsCommBuadrateTbl) / 4)
                ; // ERROR
            else
            {
                GPS_Comm_Send(data, (char *)GpsNmeaCmdData[count], sizeof(GpsNmeaCmdData[count])); // 根据定值配置模块的波特率
            }
            data->GpsModuleRunStep++;
            break;
        case GPS_MODULE_BAUD_FINISH_STEP: // 找到合适的波特率
#if 1
            memset(&g_tRsComm[chnl], 0, sizeof(g_tRsComm[chnl]));
            g_tRsComm[chnl].ptBuf = (BYTE *)&g_tRsIEC[chnl];
            GPSCommInit((GPS_COMM *)g_tRsComm[chnl].ptBuf, chnl); // 运行步骤恢复初始状态

            ret = uart_open(UART_CHANNEL[chnl], tRunPara.tUartPara[chnl].dBaud, tRunPara.tUartPara[chnl].wParity);
            if (ret < 0)
            {
                printf("%s: outside uart %d open failed!\n", __FUNCTION__, UART_CHANNEL[chnl]);
            }

            RS_Ena_Init(chnl);
            RS_Recv_Enable(chnl);
#endif
            GPS_Comm_Send(data, (char *)GpsNmeaCmdData[GPS_SEL_UPLOAD_RMC_CMD], sizeof(GpsNmeaCmdData[GPS_SEL_UPLOAD_RMC_CMD]));
            //	rt_printf("GPS_MODULE_BAUD_FINISH_STEP.\r\n");
            data->GpsModuleRunStep = GPS_MODULE_UPDATE_FREQ_STEP; // GPS_MODULE_NORMAL_RUN_STEP;
            break;
        case GPS_MODULE_NEMA_SEL_STEP:
            //	rt_printf("GPS_MODULE_NEMA_SEL_STEP = %d.\r\n", sizeof(GpsNmeaCmdData[GPS_SEL_UPLOAD_RMC_CMD]));
            GPS_Comm_Send(data, (char *)GpsNmeaCmdData[GPS_SEL_UPLOAD_RMC_CMD], sizeof(GpsNmeaCmdData[GPS_SEL_UPLOAD_RMC_CMD]));
            data->GpsModuleRunStep++;
            break;
        case GPS_MODULE_NAVI_SYS_STEP:
            //	rt_printf("GPS_MODULE_NAVI_SYS_STEP.\r\n");
            GPS_Comm_Send(data, (char *)GpsNmeaCmdData[GPS_SEL_NAVI_SYS_CMD], sizeof(GpsNmeaCmdData[GPS_SEL_NAVI_SYS_CMD]));
            data->GpsModuleRunStep = GPS_MODULE_NORMAL_RUN_STEP;
            break;
        case GPS_MODULE_UPDATE_FREQ_STEP:
            //	rt_printf("GPS_MODULE_UPDATE_FREQ_STEP.\r\n");
            GPS_Comm_Send(data, (char *)GpsNmeaCmdData[GPS_SEL_UPDATE_FREQ_CMD], sizeof(GpsNmeaCmdData[GPS_SEL_UPDATE_FREQ_CMD]));
            data->GpsModuleRunStep++;
            break;
        case GPS_MODULE_SYS_STEP:
            if (tRunPara.GPS_Mode == 1) // 北斗
            {
                rt_printf("选择北斗系统.\r\n");
                GPS_Comm_Send(data, (char *)GpsNmeaCmdData[GPS_SEL_BD_SYS_CMD], sizeof(GpsNmeaCmdData[GPS_SEL_BD_SYS_CMD]));
            }
            else if (tRunPara.GPS_Mode == 2) // GPS
            {
                rt_printf("选择GPS系统.\r\n");
                GPS_Comm_Send(data, (char *)GpsNmeaCmdData[GPS_SEL_GPS_SYS_CMD], sizeof(GpsNmeaCmdData[GPS_SEL_GPS_SYS_CMD]));
            }
            else
            {
                rt_printf("选择北斗/GPS系统.\r\n");
                GPS_Comm_Send(data, (char *)GpsNmeaCmdData[GPS_SEL_NAVI_SYS_CMD], sizeof(GpsNmeaCmdData[GPS_SEL_NAVI_SYS_CMD]));
            }
            data->GpsModuleRunStep++;
            GpsNmeaRmcData.bd_sv_num = 0;
            GpsNmeaRmcData.gp_sv_num = 0;
            for (i = 0; i < GPS_NUM_MAX; i++)
            {
                GpsNmeaRmcData.bd_gsv[i].valid = 0;
                GpsNmeaRmcData.gp_gsv[i].valid = 0;
            }
            data->recv_wait_time = ustimer_get_origin();
            data->recv_handle = 0;
            break;
        case GPS_MODULE_NORMAL_RUN_STEP:
        default:
            break;
        }
    }
}

void GpsSendBaudrateCmd(GPS_COMM *p)
{
    u8 i = 0;
    char *ptr = &SendBuf[0];

    // memset(ptr,'A',20);
    // ptr+=20;
    //*ptr++='\r';
    //*ptr++='\n';

    ptr[i++] = '$';

#if 0 // xxxxxx 20220920 add 此函数没有用上，消除警告
    GPS_Comm_Send(p,ptr-22,22);
#endif
}

void GPS_Comm_Send(GPS_COMM *p, char *data, unsigned char datalen) // 发送命令
{
    unsigned char *ptr = &g_tRsComm[p->chnl].sendbuf[0];

    *ptr++ = datalen;
    // rt_printf("TX:%s\r\n",data);
    memcpy(ptr, data, datalen);
    g_tRsComm[p->chnl].nSendCnt = 1; // 第一个字节为长度
    g_tRsComm[p->chnl].bSend = TRUE;
}

#if 0
void GenerateSecdPluse(void)
{
    static uint32_t us0=0;
    static u8 state=0;

    if(us0==0)
        us0=ustimer_get_origin();

    switch(state)
    {
        case 0:
            if(ustimer_delay_origin2(&us0,USTIMER_MS*600)>0)
            {

                GPIO_KOUT0_OFF();
                GPIO_KOUT1_OFF();
                state=1;
                rt_printf("high=%d\r\n",ustimer_get_origin());
            }
            break;
        case 1:
            if(ustimer_delay_origin2(&us0,USTIMER_MS*50)>0)
            {
                GPIO_KOUT0_ON();
                GPIO_KOUT1_ON();
                state=0;
                rt_printf("low=%d\r\n",ustimer_get_origin());
            }
            break;
        default:
            break;
    }


}
#endif

char gps_data_valid_get(void)
{
    return GpsNmeaRmcData.data_vaild;
}

void gps_info_printf(void)
{
    struct rtc_time_t rtc;
    u16 i;

    rt_printf("GPS串口数据：");
    if (gps_data_valid_get() == 'A' || gps_data_valid_get() == 1)
    {
        rt_printf("\r\n");
        rt_printf(" 系统：%s\r\n", GpsNmeaRmcData.SingleType);
        timespec_to_rtc(GpsNmeaRmcData.ts, &rtc, 1);
        rt_printf(" 时间：%04d-%02d-%02d,%02d:%02d:%02d.%03d\r\n", 2000 + rtc.year, rtc.month, rtc.day, rtc.hour,
                  rtc.min, rtc.ms / 1000, rtc.ms % 1000);

        rt_printf(" 经度：%f°(%d°%d′%f″)\r\n", gps_angle_calc(GpsNmeaRmcData.Longtitude / 100.0f), GpsNmeaRmcData.Longtitude_dd, GpsNmeaRmcData.Longtitude_mm, GpsNmeaRmcData.Longtitude_ss);
        rt_printf(" 纬度：%f°(%d°%d′%f″)\r\n", gps_angle_calc(GpsNmeaRmcData.Latitude / 100.0f), GpsNmeaRmcData.Latitude_dd, GpsNmeaRmcData.Latitude_mm, GpsNmeaRmcData.Latitude_ss);
        rt_printf("==============================================\r\n");
        if (GpsNmeaRmcData.bd_sv_num > 0)
        {
            rt_printf("北斗卫星：%d台\r\n", GpsNmeaRmcData.bd_sv_num);
            for (i = 0; i < GPS_NUM_MAX; i++)
            {
                if ((GpsNmeaRmcData.bd_gsv[i].valid == 1) && (GpsNmeaRmcData.bd_gsv[i].id > 0))
                {
                    if (GpsNmeaRmcData.bd_gsv[i].id > 0)
                        rt_printf(" [卫星编号:%d ", GpsNmeaRmcData.bd_gsv[i].id);
                    else
                        rt_printf(" [卫星编号:- ");
                    if (GpsNmeaRmcData.bd_gsv[i].ele > 0)
                        rt_printf("仰角: %d ", GpsNmeaRmcData.bd_gsv[i].ele);
                    else
                        rt_printf("仰角: - ");
                    if (GpsNmeaRmcData.bd_gsv[i].az > 0)
                        rt_printf("方位角: %d ", GpsNmeaRmcData.bd_gsv[i].az);
                    else
                        rt_printf("方位角: - ");
                    if (GpsNmeaRmcData.bd_gsv[i].cn0 > 0)
                        rt_printf("载噪比: %d]\r\n", GpsNmeaRmcData.bd_gsv[i].cn0);
                    else
                        rt_printf("载噪比: -]\r\n");
                }
            }
        }
        else
        {
            rt_printf("北斗卫星：-\r\n");
        }
        rt_printf("==============================================\r\n");
        if (GpsNmeaRmcData.gp_sv_num > 0)
        {
            rt_printf("GPS卫星：%d台\r\n", GpsNmeaRmcData.gp_sv_num);
            for (i = 0; i < GPS_NUM_MAX; i++)
            {
                if ((GpsNmeaRmcData.gp_gsv[i].valid == 1) && (GpsNmeaRmcData.gp_gsv[i].id > 0))
                {
                    if (GpsNmeaRmcData.gp_gsv[i].id > 0)
                        rt_printf(" [卫星编号:%d ", GpsNmeaRmcData.gp_gsv[i].id);
                    else
                        rt_printf(" [卫星编号:- ");
                    if (GpsNmeaRmcData.gp_gsv[i].ele > 0)
                        rt_printf("仰角: %d ", GpsNmeaRmcData.gp_gsv[i].ele);
                    else
                        rt_printf("仰角: - ");
                    if (GpsNmeaRmcData.gp_gsv[i].az > 0)
                        rt_printf("方位角: %d ", GpsNmeaRmcData.gp_gsv[i].az);
                    else
                        rt_printf("方位角: - ");
                    if (GpsNmeaRmcData.gp_gsv[i].cn0 > 0)
                        rt_printf("载噪比: %d]\r\n", GpsNmeaRmcData.gp_gsv[i].cn0);
                    else
                        rt_printf("载噪比: -]\r\n");
                }
            }
        }
        else
        {
            rt_printf("GPS卫星：-\r\n");
        }
    }
    else if (gps_data_valid_get() == 'V')
    {
        rt_printf("\r\n");
        rt_printf(" 系统：无效\r\n");
        rt_printf(" 时间：无效\r\n");
        rt_printf(" 经度：无效\r\n");
        rt_printf(" 纬度：无效\r\n");
    }
    else
    {
        rt_printf("无\r\n");
    }
}

int is_gps_alive(void)
{
    return 0; // tRunPara.GPS_sync_enable && GpsNmeaRmcData.data_vaild;//TODO EWEN
}

// 从机时间同步
int gps_slave_sync(struct timespec *ts)
{
    if (tRunPara.bGPS && GpsNmeaRmcData.data_vaild) // 打开秒脉冲对时
    {
        gps_pluse_clear();
        GpsNmeaRmcData.ts = *ts;
        GpsNmeaRmcData.slave_vaild = 1;
        return 1;
    }

    return 0;
}

void gps_init_step_set(u8 step)
{
    u8 i;

    for (i = 0; i < CFG_UART_NUM_MAX; i++)
    {
        // ???????,??
        if (UART_CHANNEL[i] < 0)
        {
            continue;
        }

        if (tRunPara.tUartPara[i].wProtocol == PROTOCOL_GPS)
        {
            ((GPS_COMM *)g_tRsComm[i].ptBuf)->GpsModuleRunStep = step;
        }
    }
}

#endif