fleg
/
OpenRTX
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Silvano Seva 2025-08-11 19:13:05 +02:00
parent 498aa309cd
commit 2230c48d5d
2 changed files with 230 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

131
platform/drivers/GPS/nmea_rbuf.c Normal file
View File

@ -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 <http://www.gnu.org/licenses/> *
***************************************************************************/
#include <hwconfig.h>
#include <stdint.h>
#include <stddef.h>
#include <string.h>
#include <errno.h>
#include <stdio.h>
#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;
}

99
platform/drivers/GPS/nmea_rbuf.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/> *
***************************************************************************/
#ifndef NMEA_RBUF_H
#define NMEA_RBUF_H
#include <stdint.h>
#include <stddef.h>
#include <stdbool.h>
#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