Нужна помощь Парсинг GPS NMEA

[rtest]

Стоит задача по лайту парсить с gps модуля пару значений, нашел как раз в статье на хабре пример подходящий, но значения не выдает..
Данные с модуля точно идут, т.е. если я проброс полный данных в порт делаю, то данные NMEA идут.
Вот код

#include <SoftwareSerial.h>
SoftwareSerial mySerial(D2, D1); // RX, TX

char st = '0';
String time;
String lat;
char lat_s;
String lon;
char lon_s;
char qual;
String sats;
String alt;



boolean renew = false;

void setup() {
  Serial.begin(115200);
  mySerial.begin(115200);

}

String readVal() {
boolean rot = true;
String buffer;
while(rot) {
int b = mySerial.read();
if ( (('0' <= (char)b) && ((char)b <= '9')) || ((char)b == '.') || (('A' <= (char)b) && ((char)b <= 'Z')) ) { // считываем значения, содержащие цифры, буквы и точку
buffer += (char)b;
}
if ( (char)b == ',' ) rot = false; // заканчиваем читать, когда встречаем запятую
delay(10);
}
return buffer;
}
void loop()
{
int b = mySerial.read();
//Serial.write(b);
//Serial.write(st);

switch ( st ) { // перебираем состояния
case '0': if('$' == b) st = '1';
break;
case '1': if('G' == b) st = '2'; else st = '0';
break;
case '2': if('P' == b) st = '3'; else st = '0';
break;
case '3': if('G' == b) st = '4'; else st = '0';
break;
case '4': if('G' == b) st = '5'; else st = '0';
break;
case '5': if('A' == b) st = '6'; else st = '0';
break;
case '6': if(',' == b) { // распознали $GPGGA
time = readVal();
lat = readVal();
lat.replace('.',',');
lat_s = (char)readVal()[0];
lon = readVal();
lon.replace('.',',');
lon_s = (char)readVal()[0];
qual = (char)readVal()[0];
sats = readVal();
readVal(); // skip empty
alt = readVal();

renew = true;
st = '0';
} else st = '0';
break;

}

//if (renew) {
Serial.print("Time: " + time + " Lat/Lon: " + lat_s + lat + " " + lon_s + lon + "\r\n");

renew = false;
//}
delay(10);

}

Брал его тут (с описанием)
habrahabr.ru/sandbox/47751/
Менял только пины, скорость, кавычки «Time: » в строке

Serial.print("Time: " + time + " Lat/Lon: " + lat_s + lat + " " + lon_s + lon + "\r\n");

И закоментировал оборот if (renew) {} - вокруг этой же строки..

Буду признателен за любые подсказки..

 

[enjoynering]

На гитхабе есть пару неплохих библиотек парсеров NMEA. На в вскидку tinyGPS.

 

[rtest]

На гитхабе есть пару неплохих библиотек парсеров NMEA. На в вскидку tinyGPS.

Нее, она громоздкая.. Излишне не нужная.. Мне то пару значений выдрать из всего потока..
Странно, если дебаг раскомментировать, данные входящие видны, но парсинг не работает..

 

[rtest]

Так, как я понял, портится переменная 'b', которая получает данные из порта
int b = mySerial.read();
В ней какой то мусор в перемешку с данными, не могу понять почему.
Портится видимо после этого кода, на входе поток нормальный

String readVal() {
boolean rot = true;
String buffer;
while(rot) {
int b = mySerial.read();
if ( (('0' <= (char)b) && ((char)b <= '9')) || ((char)b == '.') || (('A' <= (char)b) && ((char)b <= 'Z')) ) { // считываем значения, содержащие цифры, буквы и точку
buffer += (char)b;
}
if ( (char)b == ',' ) rot = false; // заканчиваем читать, когда встречаем запятую
delay(10);
}
return buffer;
}