#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
extern int g_sins_gps_fd;
extern int g_udp_socket;
float g_pitch,g_roll,g_heading_ang;
float g_xG,g_yG,g_zG,g_x_angspeed,g_y_angspeed,g_z_angspeed,g_xclj,g_yclj,g_zclj;
char g_utc_hour;
char g_utc_minute;
char g_utc_second;
char g_satellite_cnt;
float g_antenna_height;
double g_lng,g_lat;
char g_lng_flag,g_lat_flag;
typedef struct upd_data{
float xG;
float yG;
float zG;
float xspeed;
float yspeed;
float zspeed;
float xclj;
float yclj;
float zclj;
float pitch;
float roll;
float heading_ang;
char lng_flag;
char lat_flag;
double lng;
double lat;
} UDP_DATA,*PUDP_DATA;
#if 0
char cmd_getpitch[5] ={0x68,0x04,0x0,0x01,0x05};
char cmd_getpitch_ret[8];
char cmd_getroll[5] ={0x68,0x04,0x0,0x02,0x06};
char cmd_getroll_ret[8];
char cmd_getAngular[5] = {0x68,0x04,0x0,0x03,0x07};
char cmd_getAngular_ret[8];
char cmd_getAxisAngle[5] = {0x77,0x04,0x0,0x04,0x08};
char cmd_getAxisAngle_ret[14];
char cmd_set_bd[6] = {0x68,0x05,0x00,0x0b,0x02,0x12};
char cmd_set_bd_ret[6];
char cmd_set_add[6] = {0x68,0x05,0x00,0x0f,0x01,0x15};
char cmd_set_add_ret[6];
char cmd_get_addr[5] = {0x68,0x04,0x00,0x1f,0x23};
char cmd_get_addr_ret[6];
char cmd_set_ang_mode[6] = {0x68,0x05,0x00,0x0c,0x00,0x11};
char cmd_set_ang_mode_ret[6];
char cmd_get_G[5] = {0x77,0x04,0x00,0x54,0x58};
char cmd_get_G_ret[14];
char cmd_ang_speed[5] = {0x77,0x04,0x00,0x50,0x54};
char cmd_ang_speed_ret[14];
char cmd_save[5] = {0x68,0x04,0x00,0x0a,0x0e};
char cmd_save_ret[6] ;
float get_pitch()
{
float ret;
write(g_sins_gps_fd,cmd_getpitch,5);
read(g_sins_gps_fd,cmd_getpitch_ret,8);
ret = ang_change(cmd_getpitch_ret[4],cmd_getpitch_ret[5],cmd_getpitch_ret[6]);
return ret;
}
float get_roll()
{
float ret;
write(g_sins_gps_fd,cmd_getroll,5);
read(g_sins_gps_fd,cmd_getroll_ret,8);
ret = ang_change(cmd_getroll_ret[4],cmd_getroll_ret[5],cmd_getroll_ret[6]);
return ret;
}
float get_Angular()//// pitch roll
{
float ret;
write(g_sins_gps_fd,cmd_getAngular,5);
read(g_sins_gps_fd,cmd_getAngular_ret,8);
ret = ang_change(cmd_getAngular_ret[4],cmd_getAngular_ret[5],cmd_getAngular_ret[6]);
return ret;
}
void get_AxisAngle(float *pit,float* rol,float* reserve)
{
write(g_sins_gps_fd,cmd_getAxisAngle,5);
read(g_sins_gps_fd,cmd_getAxisAngle_ret,14);
*pit = ang_change(cmd_getAxisAngle_ret[4],cmd_getAxisAngle_ret[5],cmd_getAxisAngle_ret[6]);
*rol = ang_change(cmd_getAxisAngle_ret[7],cmd_getAxisAngle_ret[8],cmd_getAxisAngle_ret[9]);
*reserve = ang_change(cmd_getAxisAngle_ret[10],cmd_getAxisAngle_ret[11],cmd_getAxisAngle_ret[12]);
}
void get_G(float *xG,float* yG,float* zG)
{
write(g_sins_gps_fd,cmd_get_G,5);
read(g_sins_gps_fd,cmd_get_G_ret,14);
*xG = ang_change(cmd_get_G_ret[4],cmd_get_G_ret[5],cmd_get_G_ret[6]);
*xG = ang_change(cmd_get_G_ret[7],cmd_get_G_ret[8],cmd_get_G_ret[9]);
*zG = ang_change(cmd_get_G_ret[10],cmd_get_G_ret[11],cmd_get_G_ret[12]);
}
void get_ang_speed(float *xspeed,float* yspeed,float* zspeed)
{
write(g_sins_gps_fd,cmd_ang_speed,5);
read(g_sins_gps_fd,cmd_ang_speed_ret,14);
*xspeed = ang_change(cmd_ang_speed_ret[4],cmd_ang_speed_ret[5],cmd_ang_speed_ret[6]);
*yspeed = ang_change(cmd_ang_speed_ret[7],cmd_ang_speed_ret[8],cmd_ang_speed_ret[9]);
*zspeed = ang_change(cmd_ang_speed_ret[10],cmd_ang_speed_ret[11],cmd_ang_speed_ret[12]);
}
int sins_save()
{
float ret;
write(g_sins_gps_fd,cmd_save,5);
read(g_sins_gps_fd,cmd_save_ret,6);
if (cmd_save_ret[4] == 0x00) {
return 0;
} else {
return -1;
}
}
#endif
float ang_change(unsigned char d1,unsigned char d2,unsigned char d3)
{
unsigned char data[10];
float ret;
if ((d1 0x10 ) == 0 ) {
data[0] = '+';
} else {
data[0] = '-';
}
//printf("sssssss%d,%d,%d\n",d1,d2,d3);
d1 = d1 0x0f;
data[1] = d1+48;
//printf("%d data[1] = %d\n",d1,data[1]);
data[2] = (((d20xf0)>>4) + 48);
//printf(" %d data[2] = %d\n",(d20xf0)>>4,data[2]);
data[3] = (d20x0f)+48;
data[4] = '.';
data[5] = ((d30xf0)>>4)+48;
data[6] = (d30x0f)+48;
data[7] = '\0';
//printf("str is %s\n",data);
ret = atof(data);
return ret;
}
int open_sinsport()
{
int fd;
fd = open( "/dev/ttyUSB0", O_RDWR);
if (-1 == fd)
{
perror("Can't Open Serial Port");
return(-1);
}
else
{
printf("open ttyUSB0 .....\n");
}
#if 1
if(fcntl(fd, F_SETFL, 0)39, 80 90
unsigned char gps_dt[39],sins_dt[41],tmp[90];
unsigned char checksum = 0;
unsigned char checksuma = 0;
unsigned char udp_buf[128];
int i,j;
int ret;
struct sockaddr_in address;
int tmp_xG,tmp_yG,tmp_zG,tmp_xspeed,tmp_yspeed,tmp_zspeed,tmp_xclj,tmp_yclj,tmp_zclj,tmp_pitch,tmp_roll,tmp_heading_ang;
int tmp_lng;
int tmp_lat;
//v1.1 add
int tmp_height;
bzero(address,sizeof(address));
address.sin_family=AF_INET;
address.sin_port=htons(9000);
int cnt;
while(1) {
ret = read(g_sins_gps_fd,tmp,1);
if (tmp[0] != 0x77) {
continue;
}
ret = read(g_sins_gps_fd,tmp+1,1);
cnt = 0;
if (tmp[1] == 0x26) {
while(cnt != 0x25) {
ret = read(g_sins_gps_fd,gps_dt+cnt+2,0x25-cnt);
cnt += ret;
}
} else if (tmp[1] == 0x28) {
while(cnt != 0x27) {
ret = read(g_sins_gps_fd,sins_dt+cnt+2,0x27-cnt);
cnt += ret;
}
} else {
printf("recv sins_gps data err\n");
continue;
}
gps_dt[0] = 0x77;
gps_dt[1] = 0x26;
sins_dt[0] = 0x77;
sins_dt[1] = 0x28;
tcflush(g_sins_gps_fd, TCIOFLUSH );
if (cnt == 0x25) {
checksuma = 0;
for (i = 1;i<38;i++) {
checksuma += gps_dt[i];
}
if (checksuma != gps_dt[38]) {
printf("gps data recv err...%x----%x\n",checksuma,gps_dt[38]);
continue;
}
g_lng = *((double*)gps_dt[4]);
g_lng = g_lng/100;
g_lat = *((double*)gps_dt[13]);
g_lat = g_lat/100;
if (gps_dt[12] == 0x01) {
g_lng_flag = 'E';
} else {
g_lng_flag = 'S';
}
if (gps_dt[21] == 0x01) {
g_lat_flag = 'N';
} else {
g_lat_flag = 'W';
}
/* v1.1 add start*/
g_utc_hour = gps_dt[30];
g_utc_minute = gps_dt[31];
g_utc_second = gps_dt[32];
g_satellite_cnt = gps_dt[33];
g_antenna_height = *((float *)gps_dt[34]);
g_antenna_height = g_antenna_height/1000.0f;
//printf("time:%d-%d-%d,cnt:%d,height:%f\n",g_utc_hour,g_utc_minute,g_utc_second,g_satellite_cnt,g_antenna_height);
} else if (cnt == 0x27) {
checksum = 0;
for (i = 1;i<40;i++) {
checksum += sins_dt[i];
}
if (checksum != sins_dt[40]) {
printf("sins data recv err...%x---%x\n",checksum,sins_dt[40]);
continue;
}
g_pitch = ang_change(sins_dt[4],sins_dt[5],sins_dt[6]);
g_roll = ang_change(sins_dt[7],sins_dt[8],sins_dt[9]);
g_heading_ang = ang_change(sins_dt[10],sins_dt[11],sins_dt[12]);
g_xG = ang_change(sins_dt[13],sins_dt[14],sins_dt[15]);
g_yG = ang_change(sins_dt[16],sins_dt[17],sins_dt[18]);
// printf("zG:%x %x %x\n",sins_dt[19],sins_dt[20],sins_dt[21]);
g_zG = ang_change(sins_dt[19],sins_dt[20],sins_dt[21]);
g_x_angspeed = ang_change(sins_dt[22],sins_dt[23],sins_dt[24]);
g_y_angspeed = ang_change(sins_dt[25],sins_dt[26],sins_dt[27]);
g_z_angspeed = ang_change(sins_dt[28],sins_dt[29],sins_dt[30]);
g_xclj = ang_change(sins_dt[31],sins_dt[32],sins_dt[33]);
g_yclj = ang_change(sins_dt[34],sins_dt[35],sins_dt[36]);
g_zclj = ang_change(sins_dt[37],sins_dt[38],sins_dt[39]);
g_xG = (g_xG/100)*9.807;
g_yG = (g_yG/100)*9.807;
g_zG = (g_zG/100)*9.807;
g_xclj = g_xclj/1000;
g_yclj = g_yclj/1000;
g_zclj = g_zclj/1000;
}
#if 0
printf("gps data:");
for (j = 0;j<31;j++) {
printf(" %x ",gps_dt[j]);
}
printf("\r\n");
#endif
#if 0
printf("sins data:");
for (j = 0;j<41;j++) {
printf(" %x ",sins_dt[j]);
}
printf("\r\n");
#endif
#if 0
printf("tmp data %d:",ret);
for (j = 0;j<72;j++) {
printf(" %x ",tmp[j]);
}
printf("\r\n");
printf("\r\n");
#endif
tmp_xG = (int)(g_xG*100);
tmp_yG = (int)(g_yG*100);
tmp_zG = (int)(g_zG*100);
tmp_xspeed = (int)(g_x_angspeed*10000);
tmp_yspeed = (int)(g_y_angspeed*10000);
tmp_zspeed = (int)(g_z_angspeed*10000);
tmp_xclj = (int)(g_xclj);
tmp_yclj = (int)(g_yclj);
tmp_zclj = (int)(g_zclj);
tmp_pitch = (int)(g_pitch*10000);
tmp_roll = (int)(g_roll*10000);
tmp_heading_ang = (int)(g_heading_ang*10000);
tmp_lng = (int)(g_lng*1000000);
tmp_lat = (int)(g_lat*1000000);
//v1.1 add
tmp_height = (int)(g_antenna_height*1000);
udp_buf[0] = 0xAA;
udp_buf[1] = 0xAA;
memcpy(udp_buf+2,(char*)tmp_xG,4);
memcpy(udp_buf+6,(char*)tmp_yG,4);
memcpy(udp_buf+10,(char*)tmp_zG,4);
memcpy(udp_buf+14,(char*)tmp_xspeed,4);
memcpy(udp_buf+18,(char*)tmp_yspeed,4);
memcpy(udp_buf+22,(char*)tmp_zspeed,4);
memcpy(udp_buf+26,(char*)tmp_xclj,4);
memcpy(udp_buf+30,(char*)tmp_yclj,4);
memcpy(udp_buf+34,(char*)tmp_zclj,4);
memcpy(udp_buf+38,(char*)tmp_pitch,4);
memcpy(udp_buf+42,(char*)tmp_roll,4);
memcpy(udp_buf+46,(char*)tmp_heading_ang,4);
udp_buf[50] = g_lng_flag;//E S
udp_buf[51] = g_lat_flag;//N W
memcpy(udp_buf+52,(char*)tmp_lng,4);
memcpy(udp_buf+56,(char*)tmp_lat,4);
//v1.1 add
udp_buf[60] = g_utc_hour;
udp_buf[61] = g_utc_minute;
udp_buf[62] = g_utc_second;
udp_buf[63] = g_satellite_cnt;
memcpy(udp_buf + 64, (char*)tmp_height,4);
sendto(g_udp_socket,udp_buf,68,0,(struct sockaddr *)address,sizeof(address));
//v1.1 add
//g_satellite_cnt = g_satellite_cnt / 10;
#if 0
if (testcnt == 40) {
// printf("%d-------%d------%d----\n",tmp_xG,tmp_yG,tmp_zG);
// printf("%d-------%d------%d----\n",*(int*)udp_buf[30],*(int*)udp_buf[34],*(int*)udp_buf[10]);
testcnt = 0;
}
testcnt++;
#endif
//sleep(1);
//usleep(50000);
}
}