Skip to content

Commit dff9e98

Browse files
brentrubrentru
committed
GPS - Sends RMC event response
1 parent 6797eaf commit dff9e98

File tree

4 files changed

+109
-107
lines changed

4 files changed

+109
-107
lines changed

‎src/components/gps/controller.cpp

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -140,10 +140,8 @@ void GPSController::update() {
140140
WS_DEBUG_PRINT("[gps] Parsing NMEA sentence: ");
141141
WS_DEBUG_PRINTLN(nmea_sentence);
142142
if (!drv->ParseNMEASentence(nmea_sentence)) {
143-
WS_DEBUG_PRINTLN("[gps] ERROR: Failed to parse NMEA sentence!");
144143
continue; // Skip this sentence if parsing failed
145-
}
146-
else {
144+
} else {
147145
has_gps_event = true; // We have a valid NMEA sentence
148146
}
149147

‎src/components/gps/hardware.cpp

Lines changed: 62 additions & 87 deletions
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@ GPSHardware::GPSHardware() {
2222
_driver_type = GPS_DRV_NONE;
2323
_nmea_baud_rate = DEFAULT_MTK_NMEA_BAUD_RATE;
2424
_nmea_update_rate = DEFAULT_MTK_NMEA_UPDATE_RATE;
25-
25+
2626
// Initialize NMEA buffer
2727
_nmea_buff.head = 0;
2828
_nmea_buff.tail = 0;
@@ -108,59 +108,61 @@ bool GPSHardware::Handle_GPSConfig(wippersnapper_gps_GPSConfig *gps_config) {
108108
I2cReadDiscard();
109109
// Iterate through the command sentences and send them to the GPS module
110110
for (size_t i = 0; i < gps_config->commands_ubxes_count; i++) {
111-
// TODO: Tuesday fix this frame decoder!
112-
/* pb_bytes_array_t *ubx_frame = &gps_config->commands_ubxes[i];
113-
// Validate minimum frame size
114-
if (ubx_frame->size < 8) {
115-
WS_DEBUG_PRINT("[gps] Invalid UBX frame size: ");
116-
WS_DEBUG_PRINTLN(ubx_frame->size);
117-
continue;
118-
}
119-
120-
// Validate sync bytes
121-
if (ubx_frame->bytes[0] != 0xB5 || ubx_frame->bytes[1] != 0x62) {
122-
WS_DEBUG_PRINTLN("[gps] Invalid UBX sync bytes");
123-
continue;
124-
}
125-
126-
// Validate frame size
127-
size_t expectedSize = 8 + payloadLength;
128-
if (ubx_frame->size != expectedSize) {
129-
WS_DEBUG_PRINT("[gps] Frame size mismatch. Expected: ");
130-
WS_DEBUG_PRINT(expectedSize);
131-
WS_DEBUG_PRINT(", Got: ");
132-
WS_DEBUG_PRINTLN(ubx_frame->size);
133-
continue;
134-
}
135-
136-
// Extract message components
137-
uint8_t msgClass = ubx_frame->bytes[2];
138-
uint8_t msgId = ubx_frame->bytes[3];
139-
uint16_t payloadLength = ubx_frame->bytes[4] | (ubx_frame->bytes[5] << 8);
140-
141-
// Get payload
142-
uint8_t *payload = NULL;
143-
if (payloadLength > 0) {
144-
payload = &ubx_frame->bytes[6];
145-
}
146-
147-
WS_DEBUG_PRINT("[gps] Sending UBX CMD #");
148-
WS_DEBUG_PRINT(i);
149-
WS_DEBUG_PRINT(" - Class: 0x");
150-
WS_DEBUG_PRINT(msgClass, HEX);
151-
WS_DEBUG_PRINT(", ID: 0x");
152-
WS_DEBUG_PRINT(msgId, HEX);
153-
WS_DEBUG_PRINT(", Payload len: ");
154-
WS_DEBUG_PRINTLN(payloadLength);
155-
156-
// Send the message
157-
UBXSendStatus status = _ubx_gps->sendMessageWithAck(msgClass, msgId, payload, payloadLength);
158-
159-
if (status != UBXSendStatus::UBX_SEND_SUCCESS) {
160-
WS_DEBUG_PRINTLN("[gps] Failed to send UBX message");
161-
} else {
162-
WS_DEBUG_PRINTLN("[gps] OK");
163-
} */
111+
// TODO: Tuesday fix this frame decoder!
112+
/* pb_bytes_array_t *ubx_frame = &gps_config->commands_ubxes[i];
113+
// Validate minimum frame size
114+
if (ubx_frame->size < 8) {
115+
WS_DEBUG_PRINT("[gps] Invalid UBX frame size: ");
116+
WS_DEBUG_PRINTLN(ubx_frame->size);
117+
continue;
118+
}
119+
120+
// Validate sync bytes
121+
if (ubx_frame->bytes[0] != 0xB5 || ubx_frame->bytes[1] != 0x62) {
122+
WS_DEBUG_PRINTLN("[gps] Invalid UBX sync bytes");
123+
continue;
124+
}
125+
126+
// Validate frame size
127+
size_t expectedSize = 8 + payloadLength;
128+
if (ubx_frame->size != expectedSize) {
129+
WS_DEBUG_PRINT("[gps] Frame size mismatch. Expected: ");
130+
WS_DEBUG_PRINT(expectedSize);
131+
WS_DEBUG_PRINT(", Got: ");
132+
WS_DEBUG_PRINTLN(ubx_frame->size);
133+
continue;
134+
}
135+
136+
// Extract message components
137+
uint8_t msgClass = ubx_frame->bytes[2];
138+
uint8_t msgId = ubx_frame->bytes[3];
139+
uint16_t payloadLength = ubx_frame->bytes[4] | (ubx_frame->bytes[5]
140+
<< 8);
141+
142+
// Get payload
143+
uint8_t *payload = NULL;
144+
if (payloadLength > 0) {
145+
payload = &ubx_frame->bytes[6];
146+
}
147+
148+
WS_DEBUG_PRINT("[gps] Sending UBX CMD #");
149+
WS_DEBUG_PRINT(i);
150+
WS_DEBUG_PRINT(" - Class: 0x");
151+
WS_DEBUG_PRINT(msgClass, HEX);
152+
WS_DEBUG_PRINT(", ID: 0x");
153+
WS_DEBUG_PRINT(msgId, HEX);
154+
WS_DEBUG_PRINT(", Payload len: ");
155+
WS_DEBUG_PRINTLN(payloadLength);
156+
157+
// Send the message
158+
UBXSendStatus status = _ubx_gps->sendMessageWithAck(msgClass, msgId,
159+
payload, payloadLength);
160+
161+
if (status != UBXSendStatus::UBX_SEND_SUCCESS) {
162+
WS_DEBUG_PRINTLN("[gps] Failed to send UBX message");
163+
} else {
164+
WS_DEBUG_PRINTLN("[gps] OK");
165+
} */
164166
}
165167
} else {
166168
WS_DEBUG_PRINTLN("[gps] ERROR: Unsupported GPS driver type!");
@@ -544,24 +546,17 @@ void GPSHardware::PollStoreSentences() {
544546
// hw serial for avability? more performant
545547
ulong update_rate = 1000 / _nmea_update_rate;
546548
ulong start_time = millis();
547-
WS_DEBUG_PRINTLN("[gps] Polling u-blox GPS driver for new sentences...");
548549
while (millis() - start_time < update_rate) {
549550
_ubx_gps_ddc->available();
550551
}
551-
WS_DEBUG_PRINTLN("[gps] Polling completed!");
552552
uint8_t buffer[MAX_LEN_NMEA_SENTENCE];
553553
String nmeaBuffer = "";
554554
int bytesAvailable = _ubx_gps_ddc->available();
555-
WS_DEBUG_PRINT("[gps] Reading u-blox GPS data, bytes available: ");
556-
WS_DEBUG_PRINTLN(bytesAvailable);
557-
size_t bytesToRead = min(bytesAvailable, 82);
558555
size_t bytesRead;
556+
size_t bytesToRead = min(bytesAvailable, 82);
559557
if (bytesAvailable > 0) {
560558
bytesRead = _ubx_gps_ddc->readBytes(buffer, bytesToRead);
561559
}
562-
WS_DEBUG_PRINT("[gps] Read ");
563-
WS_DEBUG_PRINT(bytesAvailable);
564-
WS_DEBUG_PRINTLN(" bytes from u-blox GPS, parsing sentences...\n");
565560
// Build NMEA sentences and parse when complete
566561
for (size_t i = 0; i < bytesRead; i++) {
567562
char c = buffer[i];
@@ -572,18 +567,14 @@ void GPSHardware::PollStoreSentences() {
572567
WS_DEBUG_PRINTLN(nmeaBuffer.c_str());
573568
// Push the NMEA sentence to the buffer
574569
WS_DEBUG_PRINTLN("[gps] Pushing NMEA sentence to buffer...");
575-
if (NmeaBufPush(nmeaBuffer.c_str()) == 0) {
576-
WS_DEBUG_PRINTLN("[gps] NMEA sentence pushed to buffer.");
577-
} else {
570+
if (NmeaBufPush(nmeaBuffer.c_str()) != 0) {
578571
WS_DEBUG_PRINTLN(
579572
"[gps] ERROR: Unable to push NMEA sentence to buffer!");
573+
} else {
574+
nmeaBuffer = ""; // Reset buffer for next sentence
580575
}
581-
// Reset buffer for next sentence
582-
nmeaBuffer = "";
583576
}
584577
}
585-
// Done
586-
WS_DEBUG_PRINTLN("[gps] u-blox GPS polling completed!");
587578
} else {
588579
WS_DEBUG_PRINTLN("[gps] ERROR: Unsupported GPS driver type for polling!");
589580
}
@@ -595,37 +586,23 @@ void GPSHardware::PollStoreSentences() {
595586
* @return 0 on success, -1 if the buffer is full.
596587
*/
597588
int GPSHardware::NmeaBufPush(const char *new_sentence) {
598-
WS_DEBUG_PRINT("[gps] NmeaBufPush called with sentence: ");
599-
if (!new_sentence) {
600-
WS_DEBUG_PRINTLN("NULL");
589+
if (!new_sentence)
601590
return -1;
602-
}
603-
WS_DEBUG_PRINTLN(new_sentence);
604-
605-
WS_DEBUG_PRINT("[gps] Buffer state - head: ");
606-
WS_DEBUG_PRINT(_nmea_buff.head);
607-
WS_DEBUG_PRINT(", tail: ");
608-
WS_DEBUG_PRINT(_nmea_buff.tail);
609-
WS_DEBUG_PRINT(", maxlen: ");
610-
WS_DEBUG_PRINTLN(_nmea_buff.maxlen);
611591

612592
int next = _nmea_buff.head + 1; // points to head after the current write
613593
if (next >= _nmea_buff.maxlen)
614594
next = 0; // wrap around
615595

616-
// If buffer is full, advance tail to overwrite oldest data
596+
// If buffer is full, overwrite oldest data
617597
if (next == _nmea_buff.tail) {
618598
_nmea_buff.tail = (_nmea_buff.tail + 1) % _nmea_buff.maxlen;
619599
}
620600

621-
WS_DEBUG_PRINTLN("[gps] About to call strncpy...");
622601
// Copy the new sentence into the buffer
623602
strncpy(_nmea_buff.sentences[_nmea_buff.head], new_sentence,
624603
MAX_LEN_NMEA_SENTENCE - 1);
625-
WS_DEBUG_PRINTLN("[gps] strncpy completed, setting null terminator...");
626604
_nmea_buff.sentences[_nmea_buff.head][MAX_LEN_NMEA_SENTENCE - 1] = '\0';
627605
_nmea_buff.head = next;
628-
WS_DEBUG_PRINTLN("[gps] NmeaBufPush completed successfully");
629606
return 0;
630607
}
631608

@@ -659,8 +636,6 @@ int GPSHardware::NmeaBufPop(char *sentence) {
659636
bool GPSHardware::ParseNMEASentence(char *sentence) {
660637
if (!sentence)
661638
return false;
662-
WS_DEBUG_PRINT("[gps] ParseNMEASentence: ");
663-
WS_DEBUG_PRINTLN(sentence);
664639
return _ada_gps->parse(sentence);
665640
}
666641

‎src/components/gps/hardware.h

Lines changed: 16 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -16,8 +16,8 @@
1616
#define WS_GPS_HARDWARE_H
1717
#include "Wippersnapper_V2.h"
1818
#include <Adafruit_GPS.h>
19-
#include <Adafruit_UBloxDDC.h>
2019
#include <Adafruit_UBX.h>
20+
#include <Adafruit_UBloxDDC.h>
2121

2222
#define CMD_MTK_QUERY_FW \
2323
"$PMTK605*31" ///< Request to query MediaTek firmware version
@@ -114,6 +114,7 @@ class GPSHardware {
114114
float GetSpeed();
115115
float GetAngle();
116116
float GetGeoidHeight();
117+
117118
private:
118119
bool QueryModuleType();
119120
bool DetectMtkUart();
@@ -122,19 +123,20 @@ class GPSHardware {
122123
bool BuildPmtkAck(char *msg_cmd, char *msg_resp);
123124
void I2cReadDiscard();
124125
void UartReadDiscard();
125-
GpsInterfaceType _iface_type; ///< Type of interface used by GPS
126-
GpsDriverType _driver_type; ///< Type of GPS driver used by GPS
127-
HardwareSerial *_hw_serial = nullptr; ///< Optional HardwareSerial instance
128-
TwoWire *_wire = nullptr; ///< Optional TwoWire instance
129-
Adafruit_GPS *_ada_gps = nullptr; ///< Optional Adafruit GPS instance
130-
Adafruit_UBloxDDC *_ubx_gps_ddc = nullptr; ///< Optional Adafruit UBlox DDC instance
131-
Adafruit_UBX *_ubx_gps = nullptr; ///< Optional Adafruit UBX instance
132-
uint32_t _addr; ///< Optional i2c address
133-
ulong _period; ///< Polling period for GPS data (Specified by IO), in ms
134-
ulong _period_prv; ///< Previous period for GPS data (Specified by IO), in ms
135-
ulong _kat_prv; ///< Last time the GPS hardware was polled, in ms
136-
int _nmea_update_rate; ///< NMEA update rate for GPS data, in Hz
137-
int _nmea_baud_rate; ///< NMEA baud rate for GPS data, in bits per second
126+
GpsInterfaceType _iface_type; ///< Type of interface used by GPS
127+
GpsDriverType _driver_type; ///< Type of GPS driver used by GPS
128+
HardwareSerial *_hw_serial = nullptr; ///< Optional HardwareSerial instance
129+
TwoWire *_wire = nullptr; ///< Optional TwoWire instance
130+
Adafruit_GPS *_ada_gps = nullptr; ///< Optional Adafruit GPS instance
131+
Adafruit_UBloxDDC *_ubx_gps_ddc =
132+
nullptr; ///< Optional Adafruit UBlox DDC instance
133+
Adafruit_UBX *_ubx_gps = nullptr; ///< Optional Adafruit UBX instance
134+
uint32_t _addr; ///< Optional i2c address
135+
ulong _period; ///< Polling period for GPS data (Specified by IO), in ms
136+
ulong _period_prv; ///< Previous period for GPS data (Specified by IO), in ms
137+
ulong _kat_prv; ///< Last time the GPS hardware was polled, in ms
138+
int _nmea_update_rate; ///< NMEA update rate for GPS data, in Hz
139+
int _nmea_baud_rate; ///< NMEA baud rate for GPS data, in bits per second
138140
int NmeaBufPush(
139141
const char *new_sentence); ///< Push a sentence to the NMEA ring buffer
140142
nmea_buffer_t _nmea_buff; ///< NMEA ring buffer for storing sentences

‎src/components/gps/model.cpp

Lines changed: 30 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -196,9 +196,8 @@ bool GPSModel::AddGpsEventGGA(wippersnapper_gps_GPSDateTime datetime,
196196
}
197197

198198
bool GPSModel::ProcessNMEASentence(char *sentence, GPSHardware *drv) {
199-
// Check for $GP or $GN prefixed sentences
200-
if (sentence[0] == '$' && sentence[1] == 'G' &&
201-
(sentence[2] == 'P' || sentence[2] == 'N'))
199+
// Check for prefix: $GP or $GN
200+
if (strncmp(sentence, "$GP", 3) != 0 && strncmp(sentence, "$GN", 3) != 0)
202201
return false;
203202

204203
wippersnapper_gps_GPSDateTime datetime = CreateGpsDatetime(
@@ -207,6 +206,34 @@ bool GPSModel::ProcessNMEASentence(char *sentence, GPSHardware *drv) {
207206
char lat_dir = drv->GetLatDir();
208207
char lon_dir = drv->GetLonDir();
209208
if (sentence[3] == 'R' && sentence[4] == 'M' && sentence[5] == 'C') {
209+
// Debug prints for Sentence variabesl
210+
WS_DEBUG_PRINTLN("[gps] Processing RMC sentence...");
211+
WS_DEBUG_PRINT("[gps] Hour: ");
212+
WS_DEBUG_PRINTLN(drv->GetHour());
213+
WS_DEBUG_PRINT("[gps] Minute: ");
214+
WS_DEBUG_PRINTLN(drv->GetMinute());
215+
WS_DEBUG_PRINT("[gps] Seconds: ");
216+
WS_DEBUG_PRINTLN(drv->GetSeconds());
217+
WS_DEBUG_PRINT("[gps] Milliseconds: ");
218+
WS_DEBUG_PRINTLN(drv->GetMilliseconds());
219+
WS_DEBUG_PRINT("[gps] Day: ");
220+
WS_DEBUG_PRINTLN(drv->GetDay());
221+
WS_DEBUG_PRINT("[gps] Month: ");
222+
WS_DEBUG_PRINTLN(drv->GetMonth());
223+
WS_DEBUG_PRINT("[gps] Year: ");
224+
WS_DEBUG_PRINTLN(drv->GetYear());
225+
WS_DEBUG_PRINT("[gps] Latitude: ");
226+
WS_DEBUG_PRINTLN(drv->GetLat());
227+
WS_DEBUG_PRINT("[gps] Latitude Direction: ");
228+
WS_DEBUG_PRINTLN(lat_dir);
229+
WS_DEBUG_PRINT("[gps] Longitude: ");
230+
WS_DEBUG_PRINTLN(drv->GetLon());
231+
WS_DEBUG_PRINT("[gps] Longitude Direction: ");
232+
WS_DEBUG_PRINTLN(lon_dir);
233+
WS_DEBUG_PRINT("[gps] Speed: ");
234+
WS_DEBUG_PRINTLN(drv->GetSpeed());
235+
WS_DEBUG_PRINT("[gps] Angle: ");
236+
WS_DEBUG_PRINTLN(drv->GetAngle());
210237
// Process RMC sentence
211238
if (!AddGpsEventRMC(datetime, drv->GetFix(), drv->GetLat(), &lat_dir,
212239
drv->GetLon(), &lon_dir, drv->GetSpeed(),

0 commit comments

Comments
 (0)