Guest User

Untitled

a guest
Jan 5th, 2018
1,601
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