Mercurial > ublox > ublox8
view ubx.c @ 102:3c514e476693
ubx: add UBX-CFG-TP5 & UBX-TIM-TP definitions
Signed-off-by: Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
author | Josef 'Jeff' Sipek <jeffpc@josefsipek.net> |
---|---|
date | Fri, 22 Sep 2023 09:51:51 -0400 |
parents | 5e4137c51146 |
children |
line wrap: on
line source
/* * Copyright (c) 2019-2021,2023 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 <jeffpc/io.h> #include <jeffpc/hexdump.h> #include <jeffpc/synch.h> #include <jeffpc/list.h> #include "ubx.h" #include "rfc1145.h" static LOCK_CLASS(ack_queue_lc); static struct lock ack_lock; static struct list ack_waiters; struct ack_waiter { struct list_node node; struct cond cond; enum ubx_msg_id id; bool finished; bool success; }; void ubx_init_queue(void) { MXINIT(&ack_lock, &ack_queue_lc); list_create(&ack_waiters, sizeof(struct ack_waiter), offsetof(struct ack_waiter, node)); } const char *ubx_msg_name(enum ubx_msg_id id) { const char *names[0xffff] = { [UBX_ACK_ACK] = "UBX-ACK-ACK", [UBX_ACK_NAK] = "UBX-ACK-NAK", [UBX_CFG_GNSS] = "UBX-CFG-GNSS", [UBX_CFG_PRT] = "UBX-CFG-PRT", [UBX_CFG_MSG] = "UBX-CFG-MSG", [UBX_CFG_NAV5] = "UBX-CFG-NAV5", [UBX_CFG_TP5] = "UBX-CFG-TP5", [UBX_MON_HW] = "UBX-MON-HW", [UBX_MON_VER] = "UBX-MON-VER", [UBX_NAV_CLOCK] = "UBX-NAV-CLOCK", [UBX_NAV_POSECEF] = "UBX-NAV-POSECEF", [UBX_NAV_PVT] = "UBX-NAV-PVT", [UBX_NAV_SAT] = "UBX-NAV-SAT", [UBX_RXM_MEASX] = "UBX-RXM-MEASX", [UBX_RXM_RAWX] = "UBX-RXM-RAWX", [UBX_RXM_RLM] = "UBX-RXM-RLM", [UBX_RXM_SFRBX] = "UBX-RXM-SFRBX", [UBX_SEC_UNIQID] = "UBX-SEC-UNIQID", [UBX_TIM_TP] = "UBX-TIM-TP", }; if ((id > ARRAY_LEN(names)) || (names[id] == NULL)) return "???"; return names[id]; } static int __send_ubx(int fd, enum ubx_msg_id id, const void *data, size_t len) { struct ubx_header hdr = { .sync = { UBX_SYNC_BYTE_1, UBX_SYNC_BYTE_2 }, .class = id >> 8, .id = id & 0xff, .len = cpu16_to_le(len), }; uint16_t cksum; int ret; { char out[len*2+1]; hexdumpz(out, data, len, false); fprintf(stderr, "> %02x%02x %02x %02x (%s) %04x %s\n", hdr.sync[0], hdr.sync[1], hdr.class, hdr.id, ubx_msg_name((hdr.class << 8) | hdr.id), le16_to_cpu(hdr.len), out); } ret = xwrite(fd, &hdr, sizeof(hdr)); if (ret) return ret; ret = xwrite(fd, data, len); if (ret) return ret; cksum = cpu16_to_le(rfc1145_cksum(hdr.class, hdr.id, hdr.len & 0xff, hdr.len >> 8, data, len)); ret = xwrite(fd, &cksum, sizeof(cksum)); if (ret) return ret; return 0; } void notify_ubx_ack(const uint8_t *data, size_t len, bool success) { struct ack_waiter *cur, *tmp; enum ubx_msg_id id; ASSERT3U(len, ==, 2); id = mkmsgid(data[0], data[1]); fprintf(stderr, "%s for %02x %02x (%s)\n", success ? "ACK" : "NAK", data[0], data[1], ubx_msg_name(id)); MXLOCK(&ack_lock); list_for_each_safe(cur, tmp, &ack_waiters) { if (cur->id != id) continue; cur->finished = true; cur->success = success; CONDSIG(&cur->cond); /* remove from queue */ list_remove(&ack_waiters, cur); } MXUNLOCK(&ack_lock); } int send_ubx(int fd, enum ubx_msg_id id, const void *data, size_t len) { return __send_ubx(fd, id, data, len); } int send_ubx_with_ack(int fd, enum ubx_msg_id id, const void *data, size_t len) { struct ack_waiter waiter; int ret; CONDINIT(&waiter.cond); waiter.finished = false; waiter.id = id; MXLOCK(&ack_lock); list_insert_tail(&ack_waiters, &waiter); MXUNLOCK(&ack_lock); ret = __send_ubx(fd, id, data, len); if (ret) return ret; MXLOCK(&ack_lock); while (!waiter.finished) CONDWAIT(&waiter.cond, &ack_lock); MXUNLOCK(&ack_lock); CONDDESTROY(&waiter.cond); return waiter.success ? 0 : -ENOTSUP; } int enable_ubx_msg(int fd, enum ubx_msg_id id, int port, int rate) { uint8_t raw[8] = { id >> 8, id & 0xff, 0, 0, 0, 0, 0, 0 }; raw[2 + port] = rate; return send_ubx_with_ack(fd, UBX_CFG_MSG, raw, sizeof(raw)); } bool parse_and_process_ubx_message(const uint8_t *buf, size_t len, const uint64_t tick, void(*f)(const struct ubx_header *, const uint8_t *, size_t, enum ubx_msg_id, uint64_t)) { struct ubx_header hdr; uint16_t ubx_len; memcpy(&hdr, buf, sizeof(hdr)); if ((hdr.sync[0] != UBX_SYNC_BYTE_1) || (hdr.sync[1] != UBX_SYNC_BYTE_2)) { fprintf(stderr, "UBX message sync byte mismatch %02x %02x\n", hdr.sync[0], hdr.sync[1]); return false; } ubx_len = le16_to_cpu(hdr.len); if (ubx_len != (len - sizeof(hdr) - 2)) { fprintf(stderr, "UBX message length differs (got %u exp %zu)\n", ubx_len, len - sizeof(hdr) - 2); return false; } if (f) f(&hdr, &buf[sizeof(hdr)], ubx_len, mkmsgid(hdr.class, hdr.id), tick); return true; }