Skip to content

Commit 77c553a

Browse files
committed
GPS - Circular buffer implementation for GPS reading and parsing
1 parent 6509167 commit 77c553a

File tree

2 files changed

+134
-4
lines changed

2 files changed

+134
-4
lines changed

src/components/gps/controller.cpp

Lines changed: 121 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,12 @@
1818
/*!
1919
* @brief Constructor for GPSController.
2020
*/
21-
GPSController::GPSController() { _gps_model = new GPSModel(); }
21+
GPSController::GPSController() {
22+
_gps_model = new GPSModel();
23+
_nmea_buff.head = 0;
24+
_nmea_buff.tail = 0;
25+
_nmea_buff.maxlen = MAX_NMEA_SENTENCES;
26+
}
2227

2328
/*!
2429
* @brief Destructor for GPSController.
@@ -64,6 +69,54 @@ bool GPSController::AddGPS(HardwareSerial *serial,
6469
return true;
6570
}
6671

72+
/*!
73+
* @brief Pushes a new NMEA sentence into the circular buffer.
74+
* @param new_sentence Pointer to the new NMEA sentence to be added.
75+
* @return 0 on success, -1 if the buffer is full.
76+
*/
77+
int GPSController::NmeaBufPush(const char *new_sentence) {
78+
if (!new_sentence)
79+
return -1;
80+
81+
int next = _nmea_buff.head + 1; // points to head after the current write
82+
if (next >= _nmea_buff.maxlen)
83+
next = 0; // wrap around
84+
85+
// If buffer is full, advance tail to overwrite oldest data
86+
if (next == _nmea_buff.tail) {
87+
_nmea_buff.tail = (_nmea_buff.tail + 1) % _nmea_buff.maxlen;
88+
}
89+
90+
// Copy the new sentence into the buffer
91+
strncpy(_nmea_buff.sentences[_nmea_buff.head], new_sentence,
92+
MAX_LEN_NMEA_SENTENCE - 1);
93+
_nmea_buff.sentences[_nmea_buff.head][MAX_LEN_NMEA_SENTENCE - 1] = '\0';
94+
_nmea_buff.head = next;
95+
return 0;
96+
}
97+
98+
/*!
99+
* @brief Pops a NMEA sentence from the circular buffer, FIFO order.
100+
* @param sentence Pointer to the buffer where the popped sentence will be
101+
* stored.
102+
* @return 0 on success, -1 if the buffer is empty.
103+
*/
104+
int GPSController::NmeaBufPop(char *sentence) {
105+
// Is the buffer empty?
106+
if (_nmea_buff.head == _nmea_buff.tail)
107+
return -1;
108+
109+
int next =
110+
_nmea_buff.tail + 1; // next is where tail will point to after this read.
111+
if (next >= _nmea_buff.maxlen)
112+
next = 0;
113+
114+
// Copy sentence from tail
115+
strcpy(sentence, _nmea_buff.sentences[_nmea_buff.tail]);
116+
_nmea_buff.tail = next; // Advance tail
117+
return 0;
118+
}
119+
67120
/*!
68121
* @brief Updates the GPSController, polling the GPS hardware for data.
69122
* This function checks if the read period has elapsed and processes the GPS
@@ -99,6 +152,70 @@ void GPSController::update() {
99152
}
100153

101154
// Let's attempt to get a sentence from the GPS module
102-
// TODO: Are we expecting RMC, GGA, or other sentences?
103-
// TODO: Use a timeout for the read here
104-
}
155+
// Convert the NMEA update rate to milliseconds
156+
ulong update_rate = 1000 / drv->GetNmeaUpdateRate();
157+
158+
// Read from the GPS module for update_rate milliseconds
159+
ulong start_time = millis();
160+
int read_calls = 0;
161+
while (millis() - start_time < update_rate) {
162+
if (read_calls > 9) {
163+
// Check if we have a new NMEA sentence
164+
if (drv->GetAdaGps()->newNMEAreceived()) {
165+
// If we have a new sentence, push it to the buffer and mark the
166+
// received flag as false
167+
// TODO: Check result of this operation actually
168+
NmeaBufPush(drv->GetAdaGps()->lastNMEA());
169+
read_calls = 0; // Keep reading until update_rate elapses
170+
}
171+
char c = drv->GetAdaGps()->read();
172+
read_calls++;
173+
}
174+
}
175+
176+
// We are done reading for this period
177+
178+
// TODO: This is for debugging purposes only, remove later!
179+
WS_DEBUG_PRINT("[gps] Read ");
180+
WS_DEBUG_PRINT(read_calls);
181+
WS_DEBUG_PRINTLN(" times from GPS module.");
182+
// Pop off the buffer and parse
183+
char nmea_sentence[MAX_LEN_NMEA_SENTENCE];
184+
int rc = NmeaBufPop(nmea_sentence);
185+
if (rc == -1) {
186+
WS_DEBUG_PRINTLN("[gps] NMEA sentence buffer empty!");
187+
continue; // No sentences to process, skip this driver
188+
}
189+
// Parse the NMEA sentence
190+
if (!drv->GetAdaGps()->parse(nmea_sentence)) {
191+
WS_DEBUG_PRINT("[gps] ERROR: Failed to parse NMEA sentence: ");
192+
WS_DEBUG_PRINTLN(nmea_sentence);
193+
continue; // Skip this driver if parsing failed
194+
}
195+
Serial.print("Fix: ");
196+
Serial.print((int)drv->GetAdaGps()->fix);
197+
Serial.print(" quality: ");
198+
Serial.println((int)drv->GetAdaGps()->fixquality);
199+
if (drv->GetAdaGps()->fix) {
200+
Serial.print("Location: ");
201+
Serial.print(drv->GetAdaGps()->latitude, 4);
202+
Serial.print(drv->GetAdaGps()->lat);
203+
Serial.print(", ");
204+
Serial.print(drv->GetAdaGps()->longitude, 4);
205+
Serial.println(drv->GetAdaGps()->lon);
206+
Serial.print("Speed (knots): ");
207+
Serial.println(drv->GetAdaGps()->speed);
208+
Serial.print("Angle: ");
209+
Serial.println(drv->GetAdaGps()->angle);
210+
Serial.print("Altitude: ");
211+
Serial.println(drv->GetAdaGps()->altitude);
212+
Serial.print("Satellites: ");
213+
Serial.println((int)drv->GetAdaGps()->satellites);
214+
Serial.print("Antenna status: ");
215+
Serial.println((int)drv->GetAdaGps()->antenna);
216+
}
217+
// TODO: Successfully parsed the NMEA sentence, update the model
218+
219+
drv->SetPollPeriodPrv(cur_time);
220+
}
221+
}

src/components/gps/controller.h

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,16 @@
2020
#include "model.h"
2121
#include <Adafruit_GPS.h>
2222

23+
#define MAX_NMEA_SENTENCES 10 ///< Size of the NMEA buffer
24+
#define MAX_LEN_NMEA_SENTENCE 82 ///< Maximum length of a NMEA sentence
25+
26+
typedef struct {
27+
char sentences[MAX_NMEA_SENTENCES][MAX_LEN_NMEA_SENTENCE];
28+
int head;
29+
int tail;
30+
int maxlen;
31+
} nmea_buffer_t;
32+
2333
class Wippersnapper_V2; ///< Forward declaration
2434
class GPSModel; ///< Forward declaration
2535
class GPSHardware; ///< Forward declaration
@@ -35,8 +45,11 @@ class GPSController {
3545
void update();
3646

3747
private:
48+
int NmeaBufPush(const char *new_sentence);
49+
int NmeaBufPop(char *sentence);
3850
GPSModel *_gps_model; ///< GPS model instance
3951
std::vector<GPSHardware *> _gps_drivers; ///< GPS hardware instances
52+
nmea_buffer_t _nmea_buff; ///< NMEA buffer for storing sentences
4053
};
4154
extern Wippersnapper_V2 WsV2; ///< Wippersnapper V2 instance
4255
#endif // WS_GPS_CONTROLLER_H

0 commit comments

Comments
 (0)