Guest User

Untitled

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

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×