Devuan fork of gpsd
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
 

184 lines
4.7 KiB

  1. /*
  2. * This file is Copyright (c) 2010-2018 by the GPSD project
  3. * SPDX-License-Identifier: BSD-2-clause
  4. */
  5. #include <err.h>
  6. #include <errno.h>
  7. #include <fcntl.h>
  8. #include <stdarg.h>
  9. #include <stdio.h>
  10. #include <stdlib.h>
  11. #include <string.h>
  12. #include <sys/types.h>
  13. #include <termios.h>
  14. #include <time.h> /* For nanosleep() */
  15. #include <unistd.h>
  16. #define MODE_RAW 0
  17. #define MODE_NORMAL 1
  18. #define ASHSPD_9600 5
  19. #define ASHSPD_57600 8
  20. static int nmea_send(int , const char *, ... );
  21. static void nmea_add_checksum(char *);
  22. static void serial_speed(int, int);
  23. static void config_raw(int);
  24. static void config_normal(int);
  25. int main(int argc, char **argv) {
  26. int i, speed, op, fd;
  27. int rates[] = {57600, 9600, 115200, 4800, 19200, 1200, 0}; /* RTFM */
  28. char buf[BUFSIZ];
  29. /* check number of args */
  30. if (argc != 3){
  31. u: fprintf(stderr, "usage: ashctl <port> [raw|normal]\n"
  32. "normal = 9600, GGA+GSA+GSV+RMC+ZDA\n"
  33. "raw = 57600, normal+XPG+POS+SAT+MCA+PBN+SNV\n"
  34. );
  35. return 1;
  36. }
  37. /* validate command */
  38. if (strcmp(argv[2], "raw") == 0)
  39. op = MODE_RAW;
  40. else if (strcmp(argv[2], "normal") == 0)
  41. op = MODE_NORMAL;
  42. else
  43. goto u;
  44. if ((fd = open(argv[1], O_RDWR | O_NONBLOCK | O_NOCTTY, 0644)) == -1)
  45. err(1, "open");
  46. i = 0;
  47. /* spam receiver w/ config messages */
  48. while((speed = rates[i++])){
  49. fprintf(stderr,
  50. "\010\010\010\010\010\010\010\010"
  51. "\010\010\010\010\010\010\010\010"
  52. "\010\010\010\010\010\010\010\010"
  53. "\010\010\010\010\010\010\010\010"
  54. "configuring at %d bps... ", speed);
  55. serial_speed(fd, speed);
  56. if (op == MODE_NORMAL) {
  57. config_normal(fd);
  58. serial_speed(fd, 9600);
  59. } else if (op == MODE_RAW) {
  60. config_raw(fd);
  61. serial_speed(fd, 57600);
  62. }
  63. sleep(1);
  64. i = read(fd, buf, BUFSIZ-1);
  65. buf[i] = '\0';
  66. if (strstr(buf, "$PASH") || strstr(buf, "$GP"))
  67. goto done;
  68. }
  69. done: fprintf(stderr,
  70. "\010\010\010\010\010\010\010\010"
  71. "\010\010\010\010\010\010\010\010"
  72. "\010\010\010\010\010\010\010\010"
  73. "\010\010\010\010\010\010\010\010"
  74. "receiver configuration done \n");
  75. return 0;
  76. }
  77. static void serial_speed(int fd, int speed){
  78. struct termios term;
  79. tcgetattr(fd, &term);
  80. cfmakeraw(&term);
  81. cfsetospeed(&term, speed);
  82. cfsetispeed(&term, speed);
  83. if (tcsetattr(fd, TCSANOW | TCSAFLUSH, &term) == -1)
  84. err(1, "tcsetattr");
  85. tcflush(fd, TCIOFLUSH);
  86. return;
  87. }
  88. static void config_normal(int fd){
  89. nmea_send(fd, "$PASHS,NME,ALL,A,OFF"); /* silence outbound chatter */
  90. nmea_send(fd, "$PASHS,NME,ALL,B,OFF");
  91. nmea_send(fd, "$PASHS,NME,GGA,A,ON");
  92. nmea_send(fd, "$PASHS,NME,GSA,A,ON");
  93. nmea_send(fd, "$PASHS,NME,GSV,A,ON");
  94. nmea_send(fd, "$PASHS,NME,RMC,A,ON");
  95. nmea_send(fd, "$PASHS,NME,ZDA,A,ON");
  96. nmea_send(fd, "$PASHS,INI,%d,%d,,,0,",ASHSPD_9600, ASHSPD_9600);
  97. sleep(6); /* it takes 4-6 sec for the receiver to reboot */
  98. nmea_send(fd, "$PASHS,WAS,ON"); /* enable WAAS */
  99. }
  100. static void config_raw(int fd){
  101. nmea_send(fd, "$PASHS,NME,ALL,A,OFF"); /* silence outbound chatter */
  102. nmea_send(fd, "$PASHS,NME,ALL,B,OFF");
  103. nmea_send(fd, "$PASHS,NME,GGA,A,ON");
  104. nmea_send(fd, "$PASHS,NME,GSA,A,ON");
  105. nmea_send(fd, "$PASHS,NME,GSV,A,ON");
  106. nmea_send(fd, "$PASHS,NME,RMC,A,ON");
  107. nmea_send(fd, "$PASHS,NME,ZDA,A,ON");
  108. nmea_send(fd, "$PASHS,INI,%d,%d,,,0,",ASHSPD_57600, ASHSPD_9600);
  109. sleep(6); /* it takes 4-6 sec for the receiver to reboot */
  110. nmea_send(fd, "$PASHS,WAS,ON"); /* enable WAAS */
  111. nmea_send(fd, "$PASHS,NME,POS,A,ON"); /* Ashtech PVT solution */
  112. nmea_send(fd, "$PASHS,NME,SAT,A,ON"); /* Ashtech Satellite status */
  113. nmea_send(fd, "$PASHS,NME,MCA,A,ON"); /* MCA measurements */
  114. nmea_send(fd, "$PASHS,NME,PBN,A,ON"); /* ECEF PVT solution */
  115. nmea_send(fd, "$PASHS,NME,SNV,A,ON,10"); /* Almanac data */
  116. nmea_send(fd, "$PASHS,NME,XMG,A,ON"); /* exception messages */
  117. }
  118. static void nmea_add_checksum(char *sentence)
  119. /* add NMEA checksum to a possibly *-terminated sentence */
  120. {
  121. char *p = sentence;
  122. if (*p == '$') {
  123. unsigned char sum = '\0';
  124. char c;
  125. p++;
  126. while ( ((c = *p) != '*') && (c != '\0')) {
  127. sum ^= c;
  128. p++;
  129. }
  130. *p++ = '*';
  131. (void)snprintf(p, 5, "%02X\r\n", (unsigned int)sum);
  132. }
  133. }
  134. static int nmea_send(int fd, const char *fmt, ... )
  135. /* ship a command to the GPS, adding * and correct checksum */
  136. {
  137. size_t status;
  138. char buf[BUFSIZ];
  139. va_list ap;
  140. struct timespec delay;
  141. va_start(ap, fmt) ;
  142. (void)vsnprintf(buf, sizeof(buf)-5, fmt, ap);
  143. va_end(ap);
  144. (void)strncat(buf, "*", sizeof(buf)-1);
  145. nmea_add_checksum(buf);
  146. // (void)fputs(buf, stderr); /* debug output */
  147. tcflush(fd, TCIOFLUSH);
  148. status = (size_t)write(fd, buf, strlen(buf));
  149. tcdrain(fd);
  150. /* wait 100,000 uSec */
  151. delay.tv_sec = 0;
  152. delay.tv_nsec = 100000000L;
  153. nanosleep(&delay, NULL);
  154. if (status == strlen(buf)) {
  155. return (int)status;
  156. } else {
  157. perror("nmea_send");
  158. return -1;
  159. }
  160. }