18
18
/* !
19
19
* @brief Constructor for GPSController.
20
20
*/
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
+ }
22
27
23
28
/* !
24
29
* @brief Destructor for GPSController.
@@ -64,6 +69,54 @@ bool GPSController::AddGPS(HardwareSerial *serial,
64
69
return true ;
65
70
}
66
71
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
+
67
120
/* !
68
121
* @brief Updates the GPSController, polling the GPS hardware for data.
69
122
* This function checks if the read period has elapsed and processes the GPS
@@ -99,6 +152,70 @@ void GPSController::update() {
99
152
}
100
153
101
154
// 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
+ }
0 commit comments