ins的一个代码解析片段

INS解析

static void ProtocolTranslateChar_NORMAL_55AA_SUMNOHEAD(PMYPROTOCOLDATA pData, uint8_t nCh)
{
	pData->pPROTOCOL[pData->nProtocolPos++] = nCh;
	if(pData->nProtocolPos == 1)
	{
		if(pData->pPROTOCOL[0] != 0x55)
			pData->nProtocolPos = 0;
	}
	else if(pData->nProtocolPos == 2)
	{
		if(pData->pPROTOCOL[1] == 0x55)
			pData->nProtocolPos = 1;
		else if(pData->pPROTOCOL[1] != 0xAA)
			pData->nProtocolPos = 0;
	}
	else if(pData->nProtocolPos >= pData->sParam.PROTOCOLSIZE)
	{
		int i = 0;
		uint8_t nCheck = 0;
		//复位指针
		pData->nProtocolPos = 0;
		for(i = 2; i < pData->sParam.PROTOCOLSIZE - 1; i++)
		{
			nCheck += pData->pPROTOCOL[i];
		}
		if(nCheck == pData->pPROTOCOL[pData->sParam.PROTOCOLSIZE - 1])//校验正确，响应数据
		{
			if(pData->sParam.CallBack)
				pData->sParam.CallBack(pData->sParam.PARAM, pData->pPROTOCOL, pData->sParam.PROTOCOLSIZE);
		}
		else//校验不正确，重新查找帧头
		{
			for(i = 2; i < pData->sParam.PROTOCOLSIZE; i++)
			{
				//迭代法进行重新查找帧头
				ProtocolTranslateChar_NORMAL_55AA_SUMNOHEAD(pData, pData->pPROTOCOL[i]);
			}
		}
	}
}



//惯导输出数据结构
#pragma pack(push)
#pragma pack(1)
typedef struct __tagINS_STATE
{
		uint8_t head0;                //0x55
    uint8_t head1;                //0xAA
    uint8_t FrameId;              //帧标识
//    double UTCTime;             //UTC时间
    uint8_t  year;
    uint8_t  mouth;
    uint8_t  day;
    uint8_t  hour;
    uint8_t  minutes;
    uint8_t  seconds;
    uint16_t milliseconds;
    //UTC time
    uint8_t GPSId;                //GPS 定位标识,0：无效 1：单点 2：伪距差分 3：RTK差分
    uint8_t InitId;               //初始启动标志
    uint8_t SateNum;              //卫星数目
    //组合导航状态位 0：待机 1：对准 5：零速校正 6：纯惯导  7：惯导/GNSS组合导航 8：惯导/轮速计组合导航   9: 惯导/SLAM组合导航;10：故障
    uint8_t NavState;
    float GPSEastSpeed;         //GPS东向速度
    float GPSNorthSpeed;        //GPS北向速度
    float GPSSkySpeed;          //GPS天向速度
    long NavLongitude;          //导航经度
		long NavLatitude;           //导航纬度
    float GPSHeight;            //GPS高度
    float NavHeadAngle;        //导航航向角
//    double INSDataTime;         //惯导数据时间
    uint32_t wheelspeedmeter;     //wheelspeed
    uint32_t INSDataTime;         //惯导数据时间
    float HormSpeedX;           //角速度X
    float HormSpeedY;           //角速度Y
    float HormSpeedZ;           //角速度Z，左为正
    float AccelerationX;        //加速度X
    float AccelerationY;        //加速度Y
    float AccelerationZ;        //加速度Z
    float NavEastSpeed;         //导航东向速度
    float NavNorthSpeed;        //导航北向速度
    float NavUpSpeed;          //导航天向速度
    long GPSLongitude;          //GPS经度,比例尺1e-7
    long GPSLatitude;           //krnws/GPS纬度,比例尺1e-7
    float GPSAngle;             //GPS航角度
    float NavPitchAngle;        //导航俯仰角
    float NavRollAngle;         //导航滚转角
    uint8_t checksum;
}INS_STATE,*PINS_STATE;
#pragma pack(pop)`在这里插入代码片`

GPS2UTC

//dLeapSecong  闰秒 填-18

void GPSTime_To_UTC(double dGpsWeek, double dGpsSecond, double dLeapSecong)
{

	int32_t Localzone = 8;  //时区

//	char GPSTimeToUTC[99] = {'\0'};

	int nYear, nMonth, nDay, nHour, nMinute, nSecond;
	int tm_shi = 0;   int tm_fen = 0;   int tm_miao = 0;   int tm_hmiao = 0;
	int iTianshu = 0;
	
	int startyear = 1980;
	int startmonth = 1;
	int startday = 6;

	int iGpsZh = (int)dGpsWeek;
	double iGpsMi = dGpsSecond;
	int GYM_LeapSecong = (int)dLeapSecong;
        double dSecToDay = 0.0;
 
	iGpsMi = iGpsMi + GYM_LeapSecong + Localzone*3600;  
	
	if(iGpsMi<0)
	{
	 iGpsMi += 604800;
	 iGpsZh = iGpsZh - 1;
	}
	else if(iGpsMi>=604800)
	{
	 iGpsMi -= 604800;
	 iGpsZh = iGpsZh + 1;
	}
	
	

	dSecToDay = floor(iGpsMi/3600.0/24.0);
	tm_shi = (int)floor((iGpsMi - dSecToDay*3600.0*24.0)/3600.0);
	tm_fen = (int)floor((iGpsMi - dSecToDay*3600.0*24.0 - tm_shi*3600.0)/60.0);
	tm_miao = (int)floor(iGpsMi - dSecToDay*3600.0*24.0 - tm_shi*3600.0 - tm_fen*60.0);
	tm_hmiao = (int)((iGpsMi-(int)iGpsMi)*100);
	iTianshu = iGpsZh*7 + (int)dSecToDay;
	while(iTianshu>0)
	{
		switch(startmonth)
		{
		case 1:
		case 3:
		case 5:
		case 7:
		case 8:
		case 10:
			startday++;
			if(startday>31)
			{
				startday = 1;
				startmonth++;
			}
			break;
		case 12:
			startday++;
			if(startday>31)
			{
				startday = 1;
				startmonth = 1;
				startyear++;
			}
			break;
		case 2:
			if((startyear%400==0)||(((startyear%4)==0)&&((startyear%100)!=0)))
			{
				startday++;
				if(startday>29)
				{
					startday = 1;
					startmonth ++;
				}
			}
			else
			{
				startday++;
				if(startday>28)
				{
					startday = 1;
					startmonth ++;
				}
			}
			break;
		case 4:
		case 6:
		case 9:
		case 11:
			startday++;
			if(startday>30)
			{
				startday = 1;
				startmonth++;
			}
			break;
		}
		iTianshu--;
    }
	nYear = startyear;
	nMonth = startmonth;
	nDay = startday;
	nHour = tm_shi;
	nMinute = tm_fen;
	nSecond = tm_miao;
	

	printf("\n%d%02d%02d,%02d%02d%02d.%d\n",nYear, nMonth, nDay, nHour, nMinute, nSecond,tm_hmiao);
//	sprintf(GPSTimeToUTC, "\n%d%02d%02d,%02d%02d%02d.%d\n",nYear, nMonth, nDay, nHour, nMinute, nSecond,tm_hmiao);
//	return GPSTimeToUTC;
		
}