daily pastebin goal
56%
SHARE
TWEET

Untitled

a guest Jan 5th, 2018 225 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <stdio.h>
  2. #include <string.h>
  3. #include <termios.h>
  4. #include <unistd.h>
  5. #include <sys/types.h>
  6. #include <sys/stat.h>
  7. #include <fcntl.h>
  8.  
  9. void usage( void )
  10. {
  11.     printf("Usage: gpsControl <mode> -d <device>\n");
  12.     printf("Modes are:\n");  
  13.     printf("   -s = stationary mode\n");  
  14.     printf("   -p = portable mode\n");
  15.     printf("   -d = device \n");    
  16. }
  17.  
  18. unsigned char ReadBByteFromSerial(int fd, int *k)
  19. {
  20.     unsigned char tmp;
  21.    
  22.     (*k)++;
  23.      
  24.     if (read(fd, &tmp, 1) > 0)
  25.     {
  26.         // if (tmp & 0x80)
  27.         // {
  28.             // printf("<%2x> ", tmp);
  29.         // }
  30.         // else
  31.         // {
  32.             // printf("%c", tmp);
  33.         // }
  34.     }
  35.    
  36.     return tmp;
  37. }
  38.  
  39.  
  40. int main(int argc, char *argv[])
  41. {
  42.     const  char cfg_cfg[]  =  {0xB5, 0x62, 0x06, 0x09, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x1D, 0xAB};
  43.    
  44.     const char portable[]  =  {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
  45.                                0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x10};
  46.     const char sationary[] =  {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x02, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
  47.                                0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x4E, 0x60};    
  48.     const char walking[]   =  {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x03, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
  49.                                0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x13, 0x76};
  50.     const char automotive[] =  {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x04, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
  51.                                0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x14, 0x98};    
  52.     const char sea[]       =  {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x05, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
  53.                                0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x15, 0xBA};    
  54.     const char airborne1[] =  {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
  55.                                0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC};    
  56.     const char airborne2[] =  {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x07, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
  57.                                0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xFE};    
  58.     const unsigned char airborne4[] =  {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x08, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, \
  59.                                0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x20};
  60.     const unsigned char ack_ack[] =    {0xB5, 0x62, 0x05, 0x01, 0x02, 0x00, 0x06, 0x24, 0x32, 0x5B};
  61.     char buff[50000];
  62.    
  63.     unsigned char *toSend;
  64.     int len, fd, Retries;
  65.     int option = 0;
  66.     int k = 0;
  67.     int OK;
  68.     char devName[50];
  69.     struct termios options;
  70.    
  71.     memset( devName, 0, sizeof(devName));
  72.     while(( option = getopt( argc, argv, "spd:")) != -1)
  73.     {
  74.         switch( option)
  75.         {
  76.             case 'd':
  77.                 printf("Configuring device %s\n", optarg );
  78.                 strncpy( devName, optarg, sizeof(devName ));                
  79.                 break;
  80.             case 's':
  81.                 printf("Set GPS for stationary mode\n");
  82.                 toSend = (char*)sationary;
  83.                 len = sizeof(sationary);              
  84.                 break;
  85.   /*          case 'a':
  86.                 printf("Set GPS for Automotive mode\n");
  87.                 toSend = (char*)automotive;
  88.                 len = sizeof(automotive);                
  89.                 break;
  90.    */          
  91.             case 'p':
  92.                 printf("Set GPS for portable mode\n");
  93.                 toSend = (char*)portable;
  94.                 len = sizeof(portable);              
  95.                 break;
  96. /*                
  97.             case 'S':
  98.                 printf("Set GPS for Sea mode\n");
  99.                 toSend = (char*)sea;
  100.                 len = sizeof(sea);                    
  101.                 break;
  102.             case 'w':
  103.                 printf("Set GPS for Walking mode\n");
  104.                 toSend = (char*)walking;
  105.                 len = sizeof(walking);                          
  106.                 break;                
  107.             case '1':
  108.                 printf("Set GPS for Airborne <1G mode\n");
  109.                 toSend = (char*)airborne1;
  110.                 len = sizeof(airborne1);                            
  111.                 break;
  112.             case '2':
  113.                 printf("Set GPS for Airborne <2G mode\n");
  114.                 toSend = (char*)airborne2;
  115.                 len = sizeof(airborne2);                      
  116.                 break;
  117.             case '4':
  118.                 printf("Set GPS for Airborne <4G mode\n");
  119.                 toSend = (char*)airborne4;
  120.                 len = sizeof(airborne4);                              
  121.                 break;
  122.  */                
  123.             default:
  124.                usage();
  125.                return(0);
  126.         }        
  127.     }
  128.  
  129.     if( devName[0] == 0)
  130.     {
  131.         usage();
  132.         return( 0 );
  133.     }
  134.        
  135.     fd = open(devName, O_RDWR); //  | O_NOCTTY);
  136.     if (fd < 0)
  137.     {
  138.         printf("Cannot open device '%s'\n", devName);
  139.         return 0;
  140.     }
  141.      
  142.     // get the current options
  143.     tcgetattr(fd, &options);
  144.  
  145.     options.c_lflag &= ~ECHO;
  146.     options.c_cc[VMIN]  = 0;
  147.     options.c_cc[VTIME] = 10;
  148.  
  149.     cfsetispeed(&options, B9600);
  150.     cfsetospeed(&options, B9600);
  151.     options.c_cflag &= ~CSTOPB;
  152.     options.c_cflag &= ~CSIZE;
  153.     options.c_cflag |= CS8;
  154.     options.c_oflag &= ~ONLCR;
  155.     options.c_oflag &= ~OPOST;
  156.     options.c_iflag &= ~IXON;
  157.     options.c_iflag &= ~IXOFF;
  158.  
  159.     tcsetattr(fd, TCSANOW, &options);
  160.  
  161.     Retries = 0;  
  162.     OK = 0;
  163.    
  164.     while ((Retries < 5) && !OK)
  165.     {
  166.         printf (">>>>>>>>>>>>>>>>>>>>> SENDING >>>>>>>>>>>>>>>\n");
  167.         write(fd, toSend, len);
  168.        
  169.         k = 0;
  170.         Retries++;
  171.        
  172.         while ((k < 1000) && !OK)
  173.         {
  174.             if (ReadBByteFromSerial(fd, &k) == 0xb5)
  175.             {
  176.                 if (ReadBByteFromSerial(fd, &k) == 0x62)
  177.                 {
  178.                     if (ReadBByteFromSerial(fd, &k) == 0x05)
  179.                     {
  180.                         if (ReadBByteFromSerial(fd, &k) == 0x01)
  181.                         {
  182.                             ReadBByteFromSerial(fd, &k);
  183.                             ReadBByteFromSerial(fd, &k);
  184.                             if (ReadBByteFromSerial(fd, &k) == toSend[2])
  185.                             {
  186.                                 if (ReadBByteFromSerial(fd, &k) == toSend[3])
  187.                                 {
  188.                                     OK = 1;
  189.                                 }
  190.                             }
  191.                         }
  192.                     }
  193.                 }
  194.             }
  195.         }
  196.     }
  197.    
  198.     if (OK)
  199.     {
  200.         printf("GPS mode set OK\n");
  201.     }
  202.     else
  203.     {
  204.         printf("** FAILED to set GPS Mode**\n");
  205.     }
  206.    
  207.     close(fd);
  208.  
  209.    
  210.     return 0;
  211. }
RAW Paste Data
Top