gpc——car

#include <iostream>
#include <cstdio>
#include <ctime>
#include <vector>
#include <algorithm>
#include <cmath>
#include <stdio.h>
#include <stdlib.h>
#include <cstring>
#include <sys/time.h>
#include <unistd.h>
#include <string>
#include <math.h>
#include <errno.h>
#include <wiringSerial.h>

    /*socket tcp服务器端*/  
    #include <sys/stat.h>  
    #include <fcntl.h>  
    #include <errno.h>  
    #include <netdb.h>  
    #include <sys/types.h>  
    #include <sys/socket.h>  
    #include <netinet/in.h>  
    #include <arpa/inet.h>  
      
    #include <stdio.h>  
    #include <string.h>  
    #include <stdlib.h>  
    #include <unistd.h>
    #include <iostream>
      
    #define SERVER_PORT 5555  

#define PI 3.14159265358979
using namespace std;
int status=0;
int center=300;
double ne_data[40]={0.0000};
int          a[40]={0};
double gps_n;
double gps_e;
void dakr_ne(int *a ,int status ) ;


double EARTH_RADIUS = 6378137;//赤道半径(单位m)

double turn_rad(double d) ;
double GetJiaoDu(double lat1, double lng1, double lat2, double lng2);
double rad(double d) ;
double LantitudeLongitudeDist(double lon1, double lat1,double lon2, double lat2);
double GetDistance(double lon1,double lat1,double lon2, double lat2)  ;
string GetDirection(double lat1, double lng1, double lat2, double lng2);


int main()
{
/***********read navit point********************/
    FILE *fpRead=fopen("/home/ubuntu/out.txt","r");  
    if(fpRead==NULL)  
    {  
        return 0;  
    }  
    for(int i=0;i<40;i++)  
    {  
        fscanf(fpRead,"%d  ",&a[i]);  
        printf("%d ",a[i]);
        if(a[i]==0 && status==0)
         {
          status=i;   
         }  
    }
    status=status/2;
    printf("status=%d\r\n",status);
    dakr_ne(a , status ) ;
     for(int i=0;i<status;i++)
    {
     printf("alat = %.8f    lng = %.8f \n",ne_data[2*i],ne_data[2*i+1]);
    }
fclose(fpRead);

FILE *fpwrite=fopen("/home/ubuntu/info.txt","w");  
if(fpwrite==NULL)
{
  return 0;
}
     


int l_len;
int r_len;
int angle_car;
int distance_car;
int offset;;
double angle;
double distance;
int point_index=0;
double lat1=30.48166;     // point
double log1=114.40103;
double lat2=30.481575;   // new 2  2
double log2=114.400973;

string direction;
angle=GetJiaoDu( lat1,log1 , lat2, log2);
distance=GetDistance( log1, lat1,log2,  lat2)  ;
direction=GetDirection( lat1,  log1,  lat2, log2);
cout<<"angle:  "<<angle<<endl;
cout<<"direction:  "<<direction<<endl;
cout<<"distance:  "<<distance<<endl;

  int fd ;
  int fd_2 ;
  char GNRMC[1000];
  char buffer[3];
  char buffer_adder_go[7]={0x55,0xaa,0x06,0x71,0x30,0x30,0x30};  // go straght
  char buffer_adder[7]   ={0x55,0xaa,0x02,0x71,0x30,0x30,0x30};  // go straght
 // char buffer_adder_stop[3]={0x55,0xaa,0x02};  //stop
  char p;
  char N_LOG[10];
  int n_num[4];
  int point_num[2];
  double e_log=0.00000;
  double n_lat=0.00000;
  int p_id;
  int turn_offset=0;

  if ((fd = serialOpen ("/dev/ttyAMA0", 9600)) < 0)
  {
    fprintf (stderr, "Unable to open serial device: %s\n", strerror (errno)) ;
    return 1 ;
  }
  putchar (serialGetchar (fd)) ;
  fflush (stdout) ;

// Loop, getting and printing characters

        int serverSocket;  
        struct sockaddr_in server_addr;  
        struct sockaddr_in clientAddr;  
        int addr_len = sizeof(clientAddr);  
        int client;  
        char buffer_tcp[200];
        string str;
        int iDataNum;  
      
        if((serverSocket = socket(AF_INET, SOCK_STREAM, 0)) < 0)  
        {  
            perror("socket");  
            return 1;  
        }  
      
        bzero(&server_addr, sizeof(server_addr));  
        server_addr.sin_family = AF_INET;  
        server_addr.sin_port = htons(SERVER_PORT);  
        server_addr.sin_addr.s_addr = htonl(INADDR_ANY);  
        if(bind(serverSocket, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0)  
        {  
            perror("connect");  
            return 1;  
        }  
      
        if(listen(serverSocket, 5) < 0)   
        {  
            perror("listen");  
            return 1;  
        }  
       int index=0;
       int iPos[5] = {0};
        while(1)  
        {  
            printf("Listening on port: %d\n", SERVER_PORT);  
            client = accept(serverSocket, (struct sockaddr*)&clientAddr, (socklen_t*)&addr_len);  
            if(client > 0 || client==0)  
            {  
                perror("accept");  
                break;  
            }
         }
    printf("\nrecv client data...n");  
    printf("IP is %s\n", inet_ntoa(clientAddr.sin_addr));  
    printf("Port is %d\n", htons(clientAddr.sin_port));
  for (;;)
  {
 
        iDataNum = recv(client, buffer_tcp, 1024, 0);  
        if(iDataNum < 0)  
        {  
            perror("recv");  
            continue;  
        }  
        str=buffer_tcp;
        iPos[0] = str.find("$AISIMBA");  
        iPos[1] = str.find("LEFT:");                
        iPos[2] = str.find("RIGHT");                 
        iPos[3] = str.find("ANGLE");              
        iPos[4] = str.find("DISTANCE");
        printf("$AISIMBA %d LEFT %d RIGHT %d ANGLE %d DISTANCE %d\n",iPos[0],iPos[1],iPos[2],iPos[3],iPos[4]);
        int LEFT;
        int RIGHT;
        int ANGLE;
        int DISIANCE;
        LEFT    =atoi(&buffer_tcp[iPos[1]+5]);
        RIGHT   =atoi(&buffer_tcp[iPos[2]+6]);
        ANGLE   =atoi(&buffer_tcp[iPos[3]+6]);
        DISIANCE=atoi(&buffer_tcp[iPos[4]+9]);
        cout<<"LEFT="<<LEFT<<endl;
        cout<<"RIGHT="<<RIGHT<<endl;
        cout<<"ANGLE="<<ANGLE<<endl;
        cout<<"DISIANCE="<<DISIANCE<<endl;

        buffer_tcp[iDataNum] = '\0';  
        if(strcmp(buffer_tcp, "quit") == 0)  
            break;  
        printf("%drecv data is %s\n", iDataNum, buffer_tcp);  
        send(client, buffer_tcp, iDataNum, 0);  
        printf("index %d\n",index++);  

       l_len=LEFT;
       r_len=RIGHT;
       angle_car=ANGLE;
       distance_car=DISIANCE;
       offset=l_len-r_len;

       /********** go ***********/
       if(abs(angle_car)<5 && abs(offset)<60)                                    buffer_adder_go[2]=0x03;
       if(abs(angle_car)==5 && abs(offset)==60)                                  buffer_adder_go[2]=0x03;
       if(r_len==999 && angle_car==999 && l_len>(center-30) && l_len <(center+30) ) buffer_adder_go[2]=0x03;
       if(l_len==999 && angle_car==999 && r_len>(center-30) && r_len <(center+30) ) buffer_adder_go[2]=0x03;

       /**********turn left  ***********/
       
       if(l_len!=999 && r_len!=999)  // two lines
         {
          if( offset>60   )   
            {                                     
            buffer_adder_go[2]=0x04;
            turn_offset=(offset)/10;
            }
          if( angle_car>5 )   
            {                                     
            buffer_adder_go[2]=0x04;
            turn_offset=angle_car;
            }
          
         }       
       if(l_len!=999 && r_len==999)  // left line
         {
         if( l_len>(center+30)   )
           {
            buffer_adder_go[2]=0x04;
            turn_offset=2*(l_len-center)/10;
           }       
         }
       if(l_len==999 && r_len!=999)  // right line
         {
         if( r_len<(center-30)   )    
           {                                          
           buffer_adder_go[2]=0x04;
           turn_offset=2*(center-r_len)/10;
           }         
         }

       /*********turn right **********/
       if(l_len!=999 && r_len!=999)  // two lines
         {
          if( offset<-60   )
            {
             buffer_adder_go[2]=0x05;
             turn_offset=(-offset)/10;
            }
          if( angle_car<-5 )
            {
             buffer_adder_go[2]=0x05;
             turn_offset=-angle_car;
            }
         }       
       if(l_len!=999 && r_len==999)  // left line
         {
         if( l_len<(center-30)   )
           {                             
            buffer_adder_go[2]=0x05;
            turn_offset=2*(center-l_len)/10;           
           }   
         }
       if(l_len==999 && r_len!=999)  // right line
         {
         if( r_len>(center+30)   )  
           {                           
           buffer_adder_go[2]=0x05;
           turn_offset=2*(r_len-center)/10;     
           }       
         }

       /*********  stop **********/
       if((distance_car>0 && distance_car<300) || (l_len==999 && r_len==999))     buffer_adder_go[2]=0x06;
       

       if(buffer_adder_go[2]==0x03)
       {
        buffer_adder_go[4] =0x30;
        buffer_adder_go[5] =0x30;
        buffer_adder_go[6] =0x30;
        cout<<"***** go   *****" <<endl;
       fprintf(fpwrite,"*****go*****%d\r\n",turn_offset);
       }

       if(buffer_adder_go[2]==0x04)
       {
        int shi;
        int ge;
        turn_offset=turn_offset+5;
        if(turn_offset>25) turn_offset=25;
        shi=turn_offset/10;
        ge =turn_offset%10;      
        buffer_adder_go[4] =0x30;
        buffer_adder_go[5] =0x30+shi;
        buffer_adder_go[6] =0x30+ge;
       fprintf(fpwrite,"*****left*****%d\r\n",turn_offset);
       cout<<"*****left*****"<<turn_offset<<endl;
       }
       if(buffer_adder_go[2]==0x05)
       {
        int shi;
        int ge;
        turn_offset=turn_offset+5;
        if(turn_offset>25) turn_offset=25;
        shi=turn_offset/10;
        ge =turn_offset%10;
        buffer_adder_go[4] =0x30;
        buffer_adder_go[5] =0x30+shi;
        buffer_adder_go[6] =0x30+ge;
       cout<<"*****right *****"<<turn_offset<<endl;
       fprintf(fpwrite,"*****right*****%d\r\n",turn_offset);
       }
       if(buffer_adder_go[2]==0x06)
       {
        buffer_adder_go[4] =0x30;
        buffer_adder_go[5] =0x30;
        buffer_adder_go[6] =0x30;
        cout<<"*****stop *****" <<endl;
       fprintf(fpwrite,"*****stop*****%d\r\n",turn_offset);
       }

//       index++;
//       cout<<"index**************************************"<<index<<endl;
       cout<<"l_len***********************************:  "<<l_len<<endl;
       cout<<"r_len***********************************:  "<<r_len<<endl;
       cout<<"angle_car*******************************:  "<<angle_car<<endl;
       cout<<"distance_car***************************:  "<< distance_car<<endl;

      fprintf(fpwrite,"l_len=%d,r_len=%d,angle_car=%d,distance_car=%d\r\n",l_len,r_len,angle_car,distance_car);


/**************read txt_gps_point**************/

       fpRead=fopen("/home/ubuntu/gps_data.txt","r");  
       if(fpRead==NULL)  
       {  
        return 0;  
       }  
        fscanf(fpRead,"%lf  ",&gps_n);    
        fscanf(fpRead,"%lf  ",&gps_e);         
        n_lat=gps_n ;
        printf("turn_n_lat:  %0.8f\n",n_lat);     
        e_log=gps_e ;
        printf("turn_e_log:  %0.8f\n",e_log);
        printf("status:  %d\n",status);
        if(point_index<status)
        {
         distance =GetDistance ( e_log, n_lat,ne_data[point_index*2+1], ne_data[point_index*2]);
         angle    =GetJiaoDu   ( n_lat ,e_log,ne_data[point_index*2], ne_data[point_index*2+1]);
         direction=GetDirection( n_lat ,e_log,ne_data[point_index*2], ne_data[point_index*2+1]);
         cout<<"angle:       "<<angle<<endl;
         cout<<"direction:   "<<direction<<endl;
         cout<<"distance:    "<<distance<<endl;
//         gcvt(angle,3,buffer);

         if(angle>100 || angle==100)
         {
         gcvt(angle,3,buffer);
         }
         if(angle<100 && (angle<10 ||angle==10))
         {
           
           gcvt(angle,2,&buffer[1]);
           buffer[0]=0x30;
         }
         if(angle<10 )
         {  
           gcvt(angle,1,&buffer[2]);
           buffer[0]=0x30;
           buffer[1]=0x30;
         }

                        //  float to string
//       gcvt(angle,2,buffer);                //  float to string
//       gcvt(angle,1,buffer);                //  float to string
        if(distance>5)
             {
              buffer_adder[2]=0x01;
             }
          else
             point_index++;
              //buffer_adder[2]=0x02;
        }
        else
          buffer_adder[2]=0x02;  
 cout<<"point_index: "<<point_index<<endl;    
if( l_len==999 && r_len==999)
{        
   buffer_adder[4]=buffer[0];
   buffer_adder[5]=buffer[1];
   buffer_adder[6]=buffer[2];
// serialPuts (fd, buffer_adder) ;
   serialPuts (fd, buffer_adder_go) ;
   cout<<"gps "<<endl;

}
else
{
      serialPuts (fd, buffer_adder_go) ;
      cout<<"image "<<endl;
     
}

//   serialPuts (fd_2, buffer_adder_go) ;
//   serialPuts (fd_2, buffer) ;

     fclose(fpRead);
     usleep(100000);
//   putchar (serialGetchar (fd)) ;
    fflush (stdout) ;
  }
return 0;
}





 /**
     * 转化为angle(rad)
     * */  
double turn_rad(double d)  //3028.8661  11424.0736   to 30+28.8661/60
    {  
       int num[3];
       num[0]=0;
       num[1]=0;
       num[2]=0;
       double one_number=0;;
       double point=0;
       num[2]=d;
       num[0]=d;      //3028
       one_number=(double)((double)num[0]/100-(int)num[0]/100)*100;  //28  24
       num[0]=(num[0]-num[1])*0.01;                     //30  114
       point=d-num[2];  //0.8661  0.0736     
       point=(double) (num[0]+one_number/60+point/60);
       return point;  
    }  

double GetJiaoDu(double lat1, double lng1, double lat2, double lng2)
        {
            double x1 = lng1;
            double y1 = lat1;
            double x2 = lng2;
            double y2 = lat2;
            double pi = 3.1415926535898 ;
            double w1 = y1 / 180 * pi;
            double j1 = x1 / 180 * pi;
            double w2 = y2 / 180 * pi;
            double j2 = x2 / 180 * pi;
            double ret;
            if (j1 == j2)
            {
                if (w1 > w2) return 270; //北半球的情况，南半球忽略
                else if (w1 < w2) return 90;
                else return -1;//位置完全相同
            }
            ret = 4 * pow(sin((w1 - w2) / 2), 2) - pow(sin((j1 - j2) / 2) * (cos(w1) - cos(w2)), 2);
            ret = sqrt(ret);
            double temp = (sin(abs(j1 - j2) / 2) * (cos(w1) + cos(w2)));
            ret = ret / temp;
            ret = atan(ret) / pi * 180;
            if (j1 > j2) // 1为参考点坐标
            {
                if (w1 > w2) ret += 180;
                else ret = 180 - ret;
            }
            else if (w1 > w2) ret = 360 - ret;
            return ret;
        }

/// <summary>
    /// 计算方位 按360计算
    /// </summary>
    /// <param name="lat1">参照物纬度</param>
    /// <param name="lng1">参照物经度</param>
    /// <param name="lat2">目标纬度</param>
    /// <param name="lng2">目标经度</param>
    /// <returns></returns>
    string GetDirection(double lat1, double lng1, double lat2, double lng2)
    {
        double jiaodu = GetJiaoDu(lat1, lng1, lat2, lng2);
        if ((jiaodu <= 10 ) || (jiaodu > 350))  return "东";
        if ((jiaodu > 10)   && (jiaodu <= 80))  return "东北";
        if ((jiaodu > 80)   && (jiaodu <= 100)) return "北";
        if ((jiaodu > 100)  && (jiaodu <= 170)) return "西北";
        if ((jiaodu > 170)  && (jiaodu <= 190)) return "西";
        if ((jiaodu > 190)  && (jiaodu <= 260)) return "西南";
        if ((jiaodu > 260)  && (jiaodu <= 280)) return "南";
        if ((jiaodu > 280)  && (jiaodu <= 350)) return "东南";
        return 0;

    }
      
    /**
     * 转化为弧度(rad)
     * */  
   double rad(double d)  
    {  
       return d * 3.1415926535898  / 180.0;  
    }  
    /**
     * 基于googleMap中的算法得到两经纬度之间的距离,计算精度与谷歌地图的距离精度差不多，相差范围在0.2米以下
     * @param lon1 第一点的精度
     * @param lat1 第一点的纬度
     * @param lon2 第二点的精度
     * @param lat3 第二点的纬度
     * @return 返回的距离，单位km
     * */  
   double GetDistance(double lon1,double lat1,double lon2, double lat2)  
    {  
       double radLat1 = rad(lat1);  
       double radLat2 = rad(lat2);  
       double a = radLat1 - radLat2;  
       double b = rad(lon1) - rad(lon2);  
       double s = 2 * asin(sqrt(pow(sin(a/2),2)+cos(radLat1)*cos(radLat2)*pow(sin(b/2),2)));  
       s = s * EARTH_RADIUS;  
       //s = Math.round(s * 10000) / 10000;  
       return s;  
    }
/** 坐标转换  地图坐标转换成经纬度坐标 ***/
   void dakr_ne(int *a ,int status )
{
    int x = 12720831;
    int y = 3561915;
    double lng ;
    double lat ;
   for(int i=0;i<status;i++)
   {
    x=a[2*i];
    y=a[2*i+1];
    lng = x/6371000.8/PI*180;
    lat = atan(exp(y/6371000.8))/PI*360-90;
    printf("blat = %.8f    lng = %.8f \n",lat,lng);
    ne_data[2*i]=lat;
    ne_data[2*i+1]=lng;
   }
   
}

// g++ -o gps_car_tcp gps_car_tcp.cpp `pkg-config opencv --cflags --libs` -lwiringPi -lpthread