新手上路
- 积分
- 21
- 金钱
- 21
- 注册时间
- 2020-7-23
- 在线时间
- 9 小时
|

楼主 |
发表于 2020-10-29 21:02:58
|
显示全部楼层
while(reading_line_No<35000)
{
//*********************************************************Ñ-»·¶áè¡£¬2¢Çò¸3Öμ½ó¿ú********************************************************
new_data_read();
//data_read();
LED0_Toggle;
//*********************************************************¼ÆËã1y3ì**********************************************************************
GNSS_INS_Fusion_step();
GNSS_INS_Fusion_terminate();
//********************************************************½á1ûD′èëDÂμÄtxtÖD*********************************************
answer_write();
reading_line_No++;
}
void answer_write(void)
{
sprintf(buf, "%.10lf \t",GNSS_INS_Fusion_Y.Yaw_INS);
strcat(bufall,buf);
memset(buf,0,sizeof(buf));
sprintf(buf1, "%.10lf \t",GNSS_INS_Fusion_Y.Pos_new_Dec_single_lever[0]);
strcat(bufall,buf1);
memset(buf1,0,sizeof(buf1));
sprintf(buf2, "%.10lf \t",GNSS_INS_Fusion_Y.Pos_new_Dec_single_lever[1]);
strcat(bufall,buf2);
memset(buf2,0,sizeof(buf2));
sprintf(buf3, "%.10lf \r\n",GNSS_INS_Fusion_Y.Pos_new_Dec_single_lever[2]);
strcat(bufall,buf3);
memset(buf3,0,sizeof(buf3));
if(strlen(bufall)>990){
f_lseek(&objtxt2,f_size(&objtxt2));
fr2=f_write(&objtxt2,bufall,strlen(bufall),&brs);
f_sync(&objtxt2);
memset(bufall,0,sizeof(bufall));
}
}
void new_data_read(void)
{
int count=0;
f_lseek(&objtxt, j);
f_read(&objtxt,buffer_gnss_data,400,&brs);
ptr= strchr(buffer_gnss_data,enter);
buffer_gnss_data[ptr-buffer_gnss_data]=0x00;
j=j+ptr-buffer_gnss_data;
char*tokenPtr =strtok(buffer_gnss_data," ");
while(tokenPtr != NULL)
{
temp[count]=atof(tokenPtr);
tokenPtr = strtok(NULL, " ");
count++;
}
time=temp[0];
GNSS_INS_Fusion_U.IMU_ACC[0]=temp[1];
GNSS_INS_Fusion_U.IMU_ACC[1]=temp[2];
GNSS_INS_Fusion_U.IMU_ACC[2]=temp[3];
GNSS_INS_Fusion_U.IMU_Gyro[0]=temp[4];
GNSS_INS_Fusion_U.IMU_Gyro[1]=temp[5];
GNSS_INS_Fusion_U.IMU_Gyro[2]=temp[6];
GNSS_INS_Fusion_U.HeadingAngle_GNSS=temp[7];
GNSS_INS_Fusion_U.Latitude_GNSS_Int=temp[8];
GNSS_INS_Fusion_U.Longitude_GNSS_Int=temp[9];
GNSS_INS_Fusion_U.Latitude_GNSS_Dec=temp[10];
GNSS_INS_Fusion_U.Longitude_GNSS_Dec=temp[11];
GNSS_INS_Fusion_U.Pos_RMS_G[0]=temp[12];
GNSS_INS_Fusion_U.Pos_RMS_G[1]=temp[13];
GNSS_INS_Fusion_U.VehicleSpeed=temp[14];
GNSS_INS_Fusion_U. Quality_GNSS=temp[15];
GNSS_INS_Fusion_U.Vel_Level_GNSS=temp[16];
GNSS_INS_Fusion_U.Course_GNSS=temp[17];
GNSS_INS_Fusion_U.SteeringAngleValid=temp[18];
GNSS_INS_Fusion_U.SteeringAngle=temp[19];
GNSS_INS_Fusion_U.Heading_RMS_GNSS=temp[20];
GNSS_INS_Fusion_U.Soln_SVs_GNSS=temp[21];
GNSS_INS_Fusion_U.GNSS_Height=temp[22];
GNSS_INS_Fusion_U.Soln_SVs_Ante2_GNSS=temp[23];
GNSS_INS_Fusion_U.GNSS_Vel_Vertical=temp[24];
GNSS_INS_Fusion_U.GNSS_Vel_Latency=temp[25];
GNSS_INS_Fusion_U.Heading_time=temp[26];
GNSS_INS_Fusion_U.Pos_time=temp[27];
GNSS_INS_Fusion_U.Vel_time=temp[28];
GNSS_INS_Fusion_U.Pos_double[0]=temp[29];
GNSS_INS_Fusion_U.Pos_double[1]=temp[30];
GNSS_INS_Fusion_U.Pos_double[2]=temp[31];
GNSS_INS_Fusion_U.Lati_Dec_single=temp[32];
GNSS_INS_Fusion_U.Longi_Dec_single=temp[33];
GNSS_INS_Fusion_U.GNSS_Height_RMS=temp[34];
GNSS_INS_Fusion_U.IMU_TEMP=temp[35];
} |
|