From 2230c48d5db0e4c6e4ab5d43aec6f19c4676fc6a Mon Sep 17 00:00:00 2001 From: Silvano Seva Date: Mon, 11 Aug 2025 19:13:05 +0200 Subject: [PATCH] drivers: gps: added ring buffer for storing NMEA sentences. Added implementation for a lock-free ring buffer designed for storage and retrieval of full NMEA sentences. Data can be inserted either by char or by sentence and extracted only by full sentences. The size of the buffer, in byte, is defined via the CONFIG_NMEA_RBUF_SIZE macro. --- platform/drivers/GPS/nmea_rbuf.c | 131 +++++++++++++++++++++++++++++++ platform/drivers/GPS/nmea_rbuf.h | 99 +++++++++++++++++++++++ 2 files changed, 230 insertions(+) create mode 100644 platform/drivers/GPS/nmea_rbuf.c create mode 100644 platform/drivers/GPS/nmea_rbuf.h diff --git a/platform/drivers/GPS/nmea_rbuf.c b/platform/drivers/GPS/nmea_rbuf.c new file mode 100644 index 00000000..dec3b632 --- /dev/null +++ b/platform/drivers/GPS/nmea_rbuf.c @@ -0,0 +1,131 @@ +/*************************************************************************** + * Copyright (C) 2025 by Federico Amedeo Izzo IU2NUO, * + * Niccolò Izzo IU2KIN * + * Frederik Saraci IU2NRO * + * Silvano Seva IU2KWO * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 3 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, see * + ***************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include "nmea_rbuf.h" + +static inline size_t nmeaRbuf_size(struct nmeaRbuf *rbuf) +{ + if(rbuf->wrPos >= rbuf->rdPos) + return rbuf->wrPos - rbuf->rdPos; + else + return CONFIG_NMEA_RBUF_SIZE + rbuf->wrPos - rbuf->rdPos; +} + +void nmeaRbuf_reset(struct nmeaRbuf *rbuf) +{ + rbuf->wrPos = 0; + rbuf->rdPos = 0; + rbuf->rdLimit = 0; + rbuf->filling = false; +} + +int nmeaRbuf_putChar(struct nmeaRbuf *rbuf, const char c) +{ + if((rbuf->filling == false) && (c == '$')) + rbuf->filling = true; + + if(rbuf->filling) { + size_t next = (rbuf->wrPos + 1) % CONFIG_NMEA_RBUF_SIZE; + + // No more space, drop current sentence and restart + if(next == rbuf->rdPos) { + rbuf->filling = false; + rbuf->wrPos = rbuf->rdLimit; + + return -1; + } + + // Append the new character + rbuf->data[rbuf->wrPos] = c; + rbuf->wrPos = next; + + // Check if a full sentence is present + if(c == '\n') { + rbuf->filling = false; + rbuf->rdLimit = rbuf->wrPos; + } + } + + return 0; +} + +int nmeaRbuf_putSentence(struct nmeaRbuf *rbuf, const char *sentence) +{ + size_t len = strlen(sentence); + size_t next = (rbuf->wrPos + len) % CONFIG_NMEA_RBUF_SIZE; + size_t free = CONFIG_NMEA_RBUF_SIZE - nmeaRbuf_size(rbuf); + + // Bad-formatted string + if((sentence[0] != '$') || (sentence[len - 1] != '\n')) + return -1; + + // Not enough space + if(len >= free) + return -2; + + // Handle write pointer wrap-around + if((rbuf->wrPos + len) >= CONFIG_NMEA_RBUF_SIZE) { + size_t chunkSize = CONFIG_NMEA_RBUF_SIZE - rbuf->wrPos; + memcpy(&rbuf->data[rbuf->wrPos], sentence, chunkSize); + sentence += chunkSize; + len -= chunkSize; + rbuf->wrPos = 0; + } + + memcpy(&rbuf->data[rbuf->wrPos], sentence, len); + rbuf->wrPos = next; + rbuf->rdLimit = next; + + return 0; +} + +int nmeaRbuf_getSentence(struct nmeaRbuf *rbuf, char *buf, const size_t maxLen) +{ + size_t bufPos = 0; + char c; + + if(rbuf->rdPos == rbuf->rdLimit) + return 0; + + do { + // Pop one character from the buffer + c = rbuf->data[rbuf->rdPos]; + rbuf->rdPos += 1; + rbuf->rdPos %= CONFIG_NMEA_RBUF_SIZE; + + // Store it + buf[bufPos] = c; + if(bufPos < maxLen) + bufPos += 1; + + } while(c != '\n'); + + if(bufPos == maxLen) + return -1; + + return (int) bufPos; +} + diff --git a/platform/drivers/GPS/nmea_rbuf.h b/platform/drivers/GPS/nmea_rbuf.h new file mode 100644 index 00000000..fa5f052c --- /dev/null +++ b/platform/drivers/GPS/nmea_rbuf.h @@ -0,0 +1,99 @@ +/*************************************************************************** + * Copyright (C) 2025 by Federico Amedeo Izzo IU2NUO, * + * Niccolò Izzo IU2KIN * + * Frederik Saraci IU2NRO * + * Silvano Seva IU2KWO * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 3 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, see * + ***************************************************************************/ + +#ifndef NMEA_RBUF_H +#define NMEA_RBUF_H + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Ad-hoc ring buffer for storing NMEA sentences. + * + * The implementation of the ring buffer is lock-free, allowing to call the + * put/get functions also from whithin an IRQ. This implementation limits the + * buffer to have ONE producer and ONE consumer. + * The sentences can be put into the buffer either one character at a time or + * one sentence at a time; in either case the put functions ensure that only + * valid sentences are inserted in the buffer. Extraction of sentences from the + * buffer can be done only by a per-sentence basis. + */ + +struct nmeaRbuf { + size_t wrPos; + size_t rdPos; + size_t rdLimit; + bool filling; + char data[CONFIG_NMEA_RBUF_SIZE]; +}; + +/** + * Reset the internal state of a ring buffer. + * + * @param rbuf: pointer to ring buffer. + */ +void nmeaRbuf_reset(struct nmeaRbuf *rbuf); + +/** + * Insert an NMEA sentence character. + * This function implments a finite state machine guaranteeing that the stored + * characters are always part of a valid NMEA sentence. + * + * @param rbuf: pointer to ring buffer. + * @param c: incoming character. + * @return zero on success, -1 if the ring buffer is full. + */ +int nmeaRbuf_putChar(struct nmeaRbuf *rbuf, const char c); + +/** + * Insert a full NMEA sentence. + * The sentence has to begin with an '$' character and terminate with a '\n'. + * + * @param rbuf: pointer to ring buffer. + * @param sentence: NMEA sentence. + * @return zero on success, -1 if the sentence is not valid and -2 if the ring + * buffer is full + */ +int nmeaRbuf_putSentence(struct nmeaRbuf *rbuf, const char *sentence); + +/** + * Extract a full NMEA sentence. + * If the sentence is longer than the maximum size of the destination buffer, + * the characters not written in the destination are removed from the ring + * buffer anyways. + * + * @param rbuf: pointer to ring buffer. + * @param buf: pointer to NMEA sentence destination buffer. + * @param maxLen: maximum acceptable size for the destination buffer. + * @return the length of the extracted sentence or -1 if the sentence is longer + * than the maximum allowed size. If the ring buffer is empty, zero is returned. + */ +int nmeaRbuf_getSentence(struct nmeaRbuf *rbuf, char *buf, const size_t maxLen); + +#ifdef __cplusplus +} +#endif + +#endif // NMEA_RBUF_H