Skip to content

Commit 07f936c

Browse files
brentrubrentru
committed
GPS - Discard TwoWire buffer, refactoring within controller update()
1 parent 5a5a6e7 commit 07f936c

File tree

4 files changed

+140
-90
lines changed

4 files changed

+140
-90
lines changed

‎src/Wippersnapper_demo.ino.cpp

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
# 1 "/var/folders/ff/dmzflvf52tq9kzvt6g8jglxw0000gn/T/tmpc4hyqmti"
2+
#include <Arduino.h>
3+
# 1 "/Users/brentrubell/Documents/Arduino/libraries/Adafruit_Wippersnapper_Arduino/src/Wippersnapper_demo.ino"
4+
# 11 "/Users/brentrubell/Documents/Arduino/libraries/Adafruit_Wippersnapper_Arduino/src/Wippersnapper_demo.ino"
5+
#include "ws_adapters.h"
6+
ws_adapter_wifi wipper;
7+
8+
9+
#define WS_DEBUG
10+
void setup();
11+
void loop();
12+
#line 17 "/Users/brentrubell/Documents/Arduino/libraries/Adafruit_Wippersnapper_Arduino/src/Wippersnapper_demo.ino"
13+
void setup() {
14+
Serial.begin(115200);
15+
while (!Serial)
16+
delay(10);
17+
wipper.provision();
18+
wipper.connect();
19+
}
20+
21+
void loop() { wipper.run(); }

‎src/components/gps/controller.cpp

Lines changed: 95 additions & 72 deletions
Original file line numberDiff line numberDiff line change
@@ -189,96 +189,119 @@ void GPSController::update() {
189189
drv->SetPrvKat(millis());
190190
} */
191191

192+
// TODO: Did the polling periods get set? Let's print and check them
193+
WS_DEBUG_PRINT("[gps] Poll period: ");
194+
WS_DEBUG_PRINT(drv->GetPollPeriod());
195+
WS_DEBUG_PRINT(", Previous poll period: ");
196+
WS_DEBUG_PRINTLN(drv->GetPollPeriodPrv());
197+
192198
// Did read period elapse?
193199
ulong cur_time = millis();
194200
if (cur_time - drv->GetPollPeriodPrv() < drv->GetPollPeriod())
195201
continue; // Not yet elapsed, skip this driver
196202

197203
// Discard the GPS buffer before we attempt to do a fresh read
198-
size_t bytes_avail = ada_gps->available();
199-
if (bytes_avail > 0) {
200-
for (size_t i = 0; i < bytes_avail; i++) {
201-
ada_gps->read();
202-
}
203-
}
204+
WS_DEBUG_PRINTLN("[gps] Discarding GPS buffer...");
205+
WS_DEBUG_PRINT("iface type: ");
206+
WS_DEBUG_PRINTLN(drv->GetIfaceType());
204207

205-
// Unset the RX flag
206-
if (ada_gps->newNMEAreceived()) {
207-
ada_gps->lastNMEA();
208-
}
208+
if (drv->GetIfaceType() == GPS_IFACE_UART_HW) {
209+
// TODO: Refactor this into a function within hardware.cpp
210+
size_t bytes_avail = ada_gps->available();
211+
if (bytes_avail > 0) {
212+
for (size_t i = 0; i < bytes_avail; i++) {
213+
WS_DEBUG_PRINT("[gps] Reading byte: ");
214+
WS_DEBUG_PRINT(i);
215+
ada_gps->read();
216+
WS_DEBUG_PRINTLN("...OK!");
217+
}
218+
} else if (drv->GetIfaceType() == GPS_IFACE_I2C) {
219+
// For I2C, request and discard any stale data from the device
220+
WS_DEBUG_PRINT("[gps] Discarding stale I2C data...");
221+
drv->I2cReadDiscard();
222+
}
209223

210-
// Let's attempt to get a sentence from the GPS module
211-
// Read from the GPS module for update_rate milliseconds
212-
ulong update_rate = 1000 / drv->GetNmeaUpdateRate();
213-
ulong start_time = millis();
214-
215-
WS_DEBUG_PRINT("[gps] Reading GPS data for ");
216-
WS_DEBUG_PRINT(update_rate);
217-
WS_DEBUG_PRINTLN(" ms...");
218-
while (millis() - start_time < update_rate) {
219-
char c = ada_gps->read();
220-
// Check if we have a new NMEA sentence
224+
// Unset the RX flag
225+
WS_DEBUG_PRINTLN("[gps] Unsetting RX flag...");
221226
if (ada_gps->newNMEAreceived()) {
222-
// If we have a new sentence, push it to the buffer
223-
char *last_nmea = ada_gps->lastNMEA();
224-
NmeaBufPush(ada_gps->lastNMEA());
227+
ada_gps->lastNMEA();
225228
}
226-
}
227229

228-
// Parse each NMEA sentence in the buffer
229-
char nmea_sentence[MAX_LEN_NMEA_SENTENCE];
230-
bool has_gps_event = false;
231-
while (NmeaBufPop(nmea_sentence) != -1) {
232-
// Parse the NMEA sentence
233-
WS_DEBUG_PRINT("[gps] Parsing NMEA sentence: ");
234-
WS_DEBUG_PRINTLN(nmea_sentence);
235-
if (!ada_gps->parse(nmea_sentence)) {
236-
continue; // Skip parsing this sentence if parsing failed
237-
} else {
238-
_gps_model->CreateGPSEvent();
239-
has_gps_event = true;
230+
// Let's attempt to get a sentence from the GPS module
231+
// Read from the GPS module for update_rate milliseconds
232+
ulong update_rate = 1000 / drv->GetNmeaUpdateRate();
233+
ulong start_time = millis();
234+
235+
WS_DEBUG_PRINT("[gps] Reading GPS data for ");
236+
WS_DEBUG_PRINT(update_rate);
237+
WS_DEBUG_PRINTLN(" ms...");
238+
while (millis() - start_time < update_rate) {
239+
char c = ada_gps->read();
240+
// Check if we have a new NMEA sentence
241+
if (ada_gps->newNMEAreceived()) {
242+
// If we have a new sentence, push it to the buffer
243+
char *last_nmea = ada_gps->lastNMEA();
244+
NmeaBufPush(ada_gps->lastNMEA());
245+
}
240246
}
241247

242-
// Build the GPSEvent message from the sentence
243-
wippersnapper_gps_GPSDateTime datetime = _gps_model->CreateGpsDatetime(
244-
ada_gps->hour, ada_gps->minute, ada_gps->seconds,
245-
ada_gps->milliseconds, ada_gps->day, ada_gps->month, ada_gps->year);
246-
if (strncmp(nmea_sentence, "$GPRMC", 6) == 0) {
247-
_gps_model->AddGpsEventRMC(
248-
datetime, ada_gps->fix, ada_gps->latitude, &ada_gps->lat,
249-
ada_gps->longitude, &ada_gps->lon, ada_gps->speed, ada_gps->angle);
250-
} else if (strncmp(nmea_sentence, "$GPGGA", 6) == 0) {
251-
_gps_model->AddGpsEventGGA(
252-
datetime, ada_gps->fix, ada_gps->latitude, &ada_gps->lat,
253-
ada_gps->longitude, &ada_gps->lon, ada_gps->satellites,
254-
ada_gps->HDOP, ada_gps->altitude, ada_gps->geoidheight);
255-
} else {
256-
WS_DEBUG_PRINTLN(
257-
"[gps] WARNING - Parsed sentence is not type RMC or GGA!");
248+
// Parse each NMEA sentence in the buffer
249+
char nmea_sentence[MAX_LEN_NMEA_SENTENCE];
250+
bool has_gps_event = false;
251+
while (NmeaBufPop(nmea_sentence) != -1) {
252+
// Parse the NMEA sentence
253+
WS_DEBUG_PRINT("[gps] Parsing NMEA sentence: ");
254+
WS_DEBUG_PRINTLN(nmea_sentence);
255+
if (!ada_gps->parse(nmea_sentence)) {
256+
continue; // Skip parsing this sentence if parsing failed
257+
} else {
258+
_gps_model->CreateGPSEvent();
259+
has_gps_event = true;
260+
}
261+
262+
// Build the GPSEvent message from the sentence
263+
wippersnapper_gps_GPSDateTime datetime = _gps_model->CreateGpsDatetime(
264+
ada_gps->hour, ada_gps->minute, ada_gps->seconds,
265+
ada_gps->milliseconds, ada_gps->day, ada_gps->month, ada_gps->year);
266+
if (strncmp(nmea_sentence, "$GPRMC", 6) == 0) {
267+
_gps_model->AddGpsEventRMC(datetime, ada_gps->fix, ada_gps->latitude,
268+
&ada_gps->lat, ada_gps->longitude,
269+
&ada_gps->lon, ada_gps->speed,
270+
ada_gps->angle);
271+
} else if (strncmp(nmea_sentence, "$GPGGA", 6) == 0) {
272+
_gps_model->AddGpsEventGGA(
273+
datetime, ada_gps->fix, ada_gps->latitude, &ada_gps->lat,
274+
ada_gps->longitude, &ada_gps->lon, ada_gps->satellites,
275+
ada_gps->HDOP, ada_gps->altitude, ada_gps->geoidheight);
276+
} else {
277+
WS_DEBUG_PRINTLN(
278+
"[gps] WARNING - Parsed sentence is not type RMC or GGA!");
279+
}
258280
}
259-
}
260281

261-
// We did not create a GPSEvent because the NMEA sentences were not
262-
// GGA/RMC or parsed correctly
263-
if (!has_gps_event) {
264-
WS_DEBUG_PRINTLN("[gps] No GPSEvent created from NMEA sentences!");
265-
continue;
266-
}
282+
// We did not create a GPSEvent because the NMEA sentences were not
283+
// GGA/RMC or parsed correctly
284+
if (!has_gps_event) {
285+
WS_DEBUG_PRINTLN("[gps] No GPSEvent created from NMEA sentences!");
286+
continue;
287+
}
267288

268-
// Encode and publish to IO
269-
WS_DEBUG_PRINT("[gps] Encoding and publishing GPSEvent to IO...");
270-
bool did_encode = _gps_model->EncodeGPSEvent();
271-
if (!did_encode) {
272-
WS_DEBUG_PRINTLN("[gps] ERROR: Failed to encode GPSEvent!");
273-
} else {
274-
// Publish the GPSEvent to IO
275-
if (!WsV2.PublishSignal(wippersnapper_signal_DeviceToBroker_gps_event_tag,
276-
_gps_model->GetGPSEvent())) {
277-
WS_DEBUG_PRINTLN("[gps] ERROR: Failed to publish GPSEvent!");
289+
// Encode and publish to IO
290+
WS_DEBUG_PRINT("[gps] Encoding and publishing GPSEvent to IO...");
291+
bool did_encode = _gps_model->EncodeGPSEvent();
292+
if (!did_encode) {
293+
WS_DEBUG_PRINTLN("[gps] ERROR: Failed to encode GPSEvent!");
278294
} else {
279-
WS_DEBUG_PRINTLN("[gps] GPSEvent published successfully!");
295+
// Publish the GPSEvent to IO
296+
if (!WsV2.PublishSignal(
297+
wippersnapper_signal_DeviceToBroker_gps_event_tag,
298+
_gps_model->GetGPSEvent())) {
299+
WS_DEBUG_PRINTLN("[gps] ERROR: Failed to publish GPSEvent!");
300+
} else {
301+
WS_DEBUG_PRINTLN("[gps] GPSEvent published successfully!");
302+
}
280303
}
304+
drv->SetPollPeriodPrv(cur_time);
281305
}
282-
drv->SetPollPeriodPrv(cur_time);
283306
}
284307
}

‎src/components/gps/hardware.cpp

Lines changed: 22 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,18 @@ GPSHardware::~GPSHardware() {
3535
}
3636
}
3737

38+
/*!
39+
* @brief Reads and discards data requested by the I2C bus.
40+
*/
41+
void GPSHardware::I2cReadDiscard() {
42+
_wire->flush();
43+
uint8_t bytes_requested = 32;
44+
_wire->requestFrom(_addr, bytes_requested);
45+
while (_wire->available()) {
46+
_wire->read();
47+
}
48+
}
49+
3850
/*!
3951
* @brief Handles a GPSConfig message from the protobuf stream.
4052
* @param stream
@@ -77,30 +89,16 @@ bool GPSHardware::Handle_GPSConfig(wippersnapper_gps_GPSConfig *gps_config) {
7789
while (_hw_serial->available() > 0) {
7890
_hw_serial->read();
7991
}
92+
} else if (_iface_type == GPS_IFACE_I2C) {
93+
I2cReadDiscard();
8094
}
8195
WS_DEBUG_PRINT("[gps] Sending command to MediaTek GPS: ");
8296
WS_DEBUG_PRINTLN(gps_config->commands[i]);
83-
// Clear the tx and rx buffer before sending the command
84-
WS_DEBUG_PRINT("[gps] Clearing buffer before sending command...");
85-
// Clear any pending GPS data
86-
uint8_t buf[32];
87-
int maxReads = 10; // Prevent infinite loop
88-
while (maxReads-- > 0) {
89-
Wire.requestFrom(PA1010D_I2C_ADDRESS, 32);
90-
if (Wire.available() == 0)
91-
break; // No more data
92-
93-
while (Wire.available()) {
94-
Wire.read(); // Discard
95-
}
96-
delay(10); // Give GPS time to refill buffer
97-
}
98-
WS_DEBUG_PRINTLN("cleared!");
9997
// Send the command to the GPS module
10098
_ada_gps->sendCommand(gps_config->commands[i]);
10199
WS_DEBUG_PRINTLN("[gps] Command sent, waiting for response...");
102100
// and wait for the corresponding response from the GPS module
103-
if (!_ada_gps->waitForSentence(msg_resp, 100)) {
101+
if (!_ada_gps->waitForSentence(msg_resp, 255)) {
104102
WS_DEBUG_PRINT("[gps] ERROR: Failed to get response | cmd:");
105103
WS_DEBUG_PRINTLN(gps_config->commands[i]);
106104
return false;
@@ -394,4 +392,10 @@ ulong GPSHardware::GetPrvKat() { return _kat_prv; }
394392
* @brief Returns the driver type of the GPS hardware.
395393
* @returns The driver type of the GPS hardware.
396394
*/
397-
GpsDriverType GPSHardware::GetDriverType() { return _driver_type; }
395+
GpsDriverType GPSHardware::GetDriverType() { return _driver_type; }
396+
397+
/*!
398+
* @brief Returns the interface type of the GPS hardware.
399+
* @returns The interface type of the GPS hardware.
400+
*/
401+
GpsInterfaceType GPSHardware::GetIfaceType() { return _iface_type; }

‎src/components/gps/hardware.h

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -72,6 +72,8 @@ class GPSHardware {
7272
bool Handle_GPSConfig(wippersnapper_gps_GPSConfig *gps_config);
7373
Adafruit_GPS *GetAdaGps();
7474
GpsDriverType GetDriverType();
75+
GpsInterfaceType GetIfaceType();
76+
void I2cReadDiscard();
7577

7678
private:
7779
bool QueryModuleType();

0 commit comments

Comments
 (0)