Mercurial > ublox > ublox8
view capture.c @ 87:0e4ab6c2a99c
ubx: actually wait for acks/naks
Signed-off-by: Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
author | Josef 'Jeff' Sipek <jeffpc@josefsipek.net> |
---|---|
date | Mon, 22 Feb 2021 08:46:16 -0500 |
parents | 310a6abb7454 |
children | 2875fe2d8fd5 |
line wrap: on
line source
/* * Copyright (c) 2019-2021 Josef 'Jeff' Sipek <jeffpc@josefsipek.net> * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE * SOFTWARE. */ #include <termios.h> #include <jeffpc/types.h> #include <jeffpc/error.h> #include <jeffpc/io.h> #include "iothread.h" #include "ubx.h" #define DEFAULT_UBXPORT 3 static char file_buffer[65536 + 8]; /* worst case message */ static void usage(const char *prog) { fprintf(stderr, "Usage: %s [-cegrs] -d <device> [-u <ubxport>] " "-l <log>\n", prog); fprintf(stderr, "\n"); fprintf(stderr, " -c enable BeiDou\n"); fprintf(stderr, " -e enable Galileo\n"); fprintf(stderr, " -g enable GPS\n"); fprintf(stderr, " -r enable GLONASS\n"); fprintf(stderr, " -s enable SBAS & QZSS\n"); fprintf(stderr, " -d <device> path or - for standard in\n"); fprintf(stderr, " -u <ubxport> port number (default %d; 0=DCC, " "1=UART, 3=USB, 4=SPI)\n", DEFAULT_UBXPORT); fprintf(stderr, " -l <log> path for framed UBX message log\n"); fprintf(stderr, " -v verbose\n"); exit(1); } static int cfg_port(FILE *file, bool ro, uint8_t ubxport, bool beidou, bool galileo, bool gps, bool glonass, bool sbas) { struct ubx_cfg_prt_uart prt_uart = { .port = ubxport, .mode = cpu32_to_le((0x00 << 12) | /* 1 stop bit */ (0x04 << 9) | /* none */ (0x03 << 6)), /* 8-bit */ .baud_rate = cpu32_to_le(115200), .in_proto_mask = cpu16_to_le(0x01), /* UBX only */ .out_proto_mask = cpu16_to_le(0x01), /* UBX only */ }; struct ubx_cfg_prt_usb prt_usb = { .port = 3, .in_proto_mask = cpu16_to_le(0x01), /* UBX only */ .out_proto_mask = cpu16_to_le(0x01), /* UBX only */ }; struct ubx_cfg_nav5 nav5 = { .mask = cpu16_to_le(0x0001), /* only dynamics model */ .dyn_model = 0, /* portable */ }; struct ubx_cfg_gnss gnss = { .num_trk_ch_use = 0xff, .num_cfg_blocks = UBX_CFG_GNSS_NUM_BLOCKS, .cfg = { { .gnssid = GNSSID_GPS, .res_trk_ch = 4, .max_trk_ch = 8, .flags = cpu32_to_le(0x00010000) /* L1 */ | cpu32_to_le(gps ? 0x1 : 0x0), }, { .gnssid = GNSSID_GALILEO, .res_trk_ch = 8, .max_trk_ch = 10, .flags = cpu32_to_le(0x00010000) /* E1 */ | cpu32_to_le(galileo ? 0x1 : 0x0), }, { .gnssid = GNSSID_GLONASS, .res_trk_ch = 6, .max_trk_ch = 8, .flags = cpu32_to_le(0x00010000) /* L1 */ | cpu32_to_le(glonass ? 0x1 : 0x0), }, { .gnssid = GNSSID_BEIDOU, .res_trk_ch = 6, .max_trk_ch = 8, .flags = cpu32_to_le(0x00010000) /* B1I */ | cpu32_to_le(beidou ? 0x1 : 0x0), }, { .gnssid = GNSSID_SBAS, .res_trk_ch = 3, .max_trk_ch = 4, .flags = cpu32_to_le(0x00010000) /* L1 */ | cpu32_to_le(sbas ? 0x1 : 0x0), }, { .gnssid = GNSSID_QZSS, .res_trk_ch = 4, .max_trk_ch = 8, .flags = cpu32_to_le(0x00010000) /* L1C */ | cpu32_to_le(sbas ? 0x1 : 0x0), }, }, }; void *prt; int ret; int i; if (ro) return 0; switch (ubxport) { case 0: panic("not yet implemented - DCC port"); case 1: prt = &prt_uart; break; case 3: prt = &prt_usb; break; case 4: panic("not yet implemeted - SPI port"); default: panic("unknown ubxport number"); } /* request version info */ ret = send_ubx(file, UBX_MON_VER, NULL, 0); if (ret) return ret; /* request serial number */ ret = send_ubx(file, UBX_SEC_UNIQID, NULL, 0); if (ret) return ret; /* disable NMEA, enable UBX */ ret = send_ubx_with_ack(file, UBX_CFG_PRT, prt, sizeof(struct ubx_cfg_prt_uart)); if (ret) return ret; /* set dynamics mode */ ret = send_ubx_with_ack(file, UBX_CFG_NAV5, &nav5, sizeof(struct ubx_cfg_nav5)); if (ret) return ret; /* enable gnss systems */ ret = send_ubx_with_ack(file, UBX_CFG_GNSS, &gnss, sizeof(struct ubx_cfg_gnss)); if (ret) return ret; struct { enum ubx_msg_id id; int rate; } enable_msgs[] = { { UBX_MON_HW, 15 }, /* jamming info */ { UBX_NAV_CLOCK, 6 }, /* clock */ { UBX_NAV_POSECEF, 6 }, /* ECEF position */ { UBX_NAV_PVT, 1 }, /* position, velocity, time */ { UBX_NAV_SAT, 6 }, /* satellite info */ { UBX_RXM_RAWX, 1 }, /* raw measurement data */ { UBX_RXM_RLM, 1 }, /* SAR RLM */ { UBX_RXM_SFRBX, 1 }, /* raw subframes */ }; for (i = 0; i < ARRAY_LEN(enable_msgs); i++) { enum ubx_msg_id id = enable_msgs[i].id; int rate = enable_msgs[i].rate; fprintf(stderr, "Enabling %s (%04x)...\n", ubx_msg_name(id), id); ret = enable_ubx_msg(file, id, ubxport, rate); if (ret) return ret; } return 0; } int main(int argc, char **argv) { struct stat stat; struct termios termios; bool input_readonly; const char *device; const char *log; bool enable_beidou; bool enable_galileo; bool enable_gps; bool enable_glonass; bool enable_sbas; uint8_t ubxport; bool verbose; FILE *ifile; FILE *rfile; int opt; int ret; int fd; device = NULL; log = NULL; enable_beidou = false; enable_galileo = false; enable_gps = false; enable_glonass = false; enable_sbas = false; ubxport = DEFAULT_UBXPORT; verbose = false; while ((opt = getopt(argc, argv, "+cegrsd:l:u:v")) != -1) { switch (opt) { case 'c': enable_beidou = true; break; case 'e': enable_galileo = true; break; case 'g': enable_gps = true; break; case 'r': enable_glonass = true; break; case 's': enable_sbas = true; break; case 'd': device = optarg; break; case 'l': log = optarg; break; case 'u': if (str2u8(optarg, &ubxport)) { fprintf(stderr, "Error: '%s' is not " "a valid ublox port.\n", optarg); usage(argv[0]); } if (ubxport > 6) { fprintf(stderr, "Error: invalid ubxport" " - must be [0,6] (got %u)\n", ubxport); usage(argv[0]); } break; case 'v': verbose = true; break; default: usage(argv[0]); } } if (optind != argc) usage(argv[0]); if (!device) { fprintf(stderr, "Missing device name\n"); usage(argv[0]); } if (!log) { fprintf(stderr, "Missing log name\n"); usage(argv[0]); } if (!enable_beidou && !enable_galileo && !enable_gps && !enable_glonass && !enable_sbas) { fprintf(stderr, "Missing GNSS constellation selection\n"); usage(argv[0]); } if (!strcmp(device, "/dev/stdin") || !strcmp(device, "-")) { device = "/dev/stdin"; input_readonly = true; } else { ret = xstat(device, &stat); if (ret < 0) { fprintf(stderr, "Error: failed to stat input: %s\n", xstrerror(ret)); return 4; } input_readonly = !S_ISCHR(stat.st_mode); } fd = xopen(device, input_readonly ? O_RDONLY : O_RDWR, 0); if (fd < 0) { fprintf(stderr, "Error: Could not open device %s: %s\n", device, xstrerror(fd)); usage(argv[0]); } if (!input_readonly) { if (tcgetattr(fd, &termios) < 0) { fprintf(stderr, "Error: Failed to get tty attrs: %s\n", xstrerror(-errno)); return 6; } memset(&termios, 0, sizeof(termios)); termios.c_cc[VTIME] = 0; termios.c_cc[VMIN] = 1; termios.c_iflag = IGNPAR; termios.c_oflag = 0; termios.c_cflag = CS8 | CREAD | CLOCAL; termios.c_lflag = 0; if (cfsetspeed(&termios, B115200) < 0) { fprintf(stderr, "Error: Failed to set baud rate: %s\n", xstrerror(-errno)); return 7; } if (tcsetattr(fd, TCSAFLUSH, &termios)) { fprintf(stderr, "Error: Failed to set attrs on device: %s\n", xstrerror(-errno)); return 8; } } ifile = fdopen(fd, input_readonly ? "rb" : "wb+"); if (ifile == NULL) { fprintf(stderr, "Error: Failed to fdopen: %s\n", xstrerror(-errno)); return 9; } rfile = fopen(log, "wb"); if (rfile == NULL) { fprintf(stderr, "Error: Failed to fopen log: %s\n", xstrerror(-errno)); return 9; } if (setvbuf(ifile, file_buffer, _IOFBF, sizeof(file_buffer)) == EOF) fprintf(stderr, "Warn: Failed to mark stream as buffered\n"); /* make sure ack queuing is set up */ ubx_init_queue(); /* start the iothread before we make any writes to the device */ ret = iothread_start(ifile, rfile, verbose, !input_readonly); if (ret) { fprintf(stderr, "Error: Failed to spawn I/O thread: %s\n", xstrerror(ret)); return 10; } ret = cfg_port(ifile, input_readonly, ubxport, enable_beidou, enable_galileo, enable_gps, enable_glonass, enable_sbas); if (ret) { fprintf(stderr, "Error: Failed to configure port: %s\n", xstrerror(ret)); return 11; } for (;;) sleep(3600); fclose(ifile); fclose(rfile); return 0; }