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