#define SERIAL2_RX 16 // GPIO16
#define SERIAL2_TX 17 // GPIO17
String inputFromSerial = "";
String inputFromSerial2 = "";
void setup() {
Serial.begin(115200);
Serial2.begin(115200, SERIAL_8N1, SERIAL2_RX, SERIAL2_TX);
Serial.println("ESP32 Serial <-> Serial2 중계 (\\n 기준, \\r 추가, lep/lec 파싱)");
}
void loop() {
// Serial 입력 → Serial2로 전송
while (Serial.available()) {
char ch = (char)Serial.read();
if (ch != '\n') {
inputFromSerial += ch;
}
if (ch == '\n') {
Serial2.print(inputFromSerial);
Serial2.print('\r'); // CR 추가
inputFromSerial = "";
}
}
// Serial2 입력 → 파싱 및 출력
while (Serial2.available()) {
char ch = (char)Serial2.read();
inputFromSerial2 += ch;
if (ch == '\n') {
inputFromSerial2.trim();
if (inputFromSerial2.startsWith("POS,")) {
parseLEP(inputFromSerial2);
} else if (inputFromSerial2.startsWith("DIST,")) {
parseLEC(inputFromSerial2);
} else {
Serial.print("[Serial2 → Serial] 수신: ");
Serial.println(inputFromSerial2);
}
inputFromSerial2 = "";
}
}
}
// ✅ LEP 결과 파서: POS,x,y,z,qf
void parseLEP(String line) {
Serial.println("[LEP 위치 결과]");
int idx = 0;
String parts[5];
while (line.length() > 0 && idx < 5) {
int comma = line.indexOf(',');
if (comma == -1) {
parts[idx++] = line;
break;
} else {
parts[idx++] = line.substring(0, comma);
line = line.substring(comma + 1);
}
}
Serial.print("X: "); Serial.println(parts[1]);
Serial.print("Y: "); Serial.println(parts[2]);
Serial.print("Z: "); Serial.println(parts[3]);
Serial.print("품질(QF): "); Serial.println(parts[4]);
Serial.println();
}
// 안전한 split 유틸(간단)
static void splitByComma(const String& s, std::vector<String>& out) {
out.clear();
int start = 0;
while (start <= s.length()) {
int comma = s.indexOf(',', start);
if (comma == -1) {
out.push_back(s.substring(start));
break;
} else {
out.push_back(s.substring(start, comma));
start = comma + 1;
}
}
}
// ✅ LEC 결과 파서(안전한 토큰 파싱): DIST, n, (ANk, id, x, y, z, d)*, POS, x, y, z, qf
void parseLEC(String line) {
Serial.println("[LEC 거리 + 위치 결과]");
line.trim();
std::vector<String> t;
splitByComma(line, t);
if (t.size() < 2 || t[0] != "DIST") {
Serial.println("→ 형식 오류: DIST 헤더 없음");
return;
}
int i = 1;
int anchorCount = t[i++].toInt(); // 예: 4
int parsedAnchors = 0;
// Anchor 묶음 파싱: ANk, id, x, y, z, d
while (i + 5 < (int)t.size() && t[i].startsWith("AN")) {
String anLabel = t[i++]; // AN0, AN1, ...
String anId = t[i++]; // 3364, 35BE, ...
float x = t[i++].toFloat(); // -1.00
float y = t[i++].toFloat(); // 11.48
float z = t[i++].toFloat(); // 0.00
float d = t[i++].toFloat(); // 2.80
Serial.print(anLabel); Serial.print(" (ID ");
Serial.print(anId); Serial.print("): ");
Serial.print("x="); Serial.print(x);
Serial.print(", y="); Serial.print(y);
Serial.print(", z="); Serial.print(z);
Serial.print(" → 거리: "); Serial.print(d); Serial.println("m");
parsedAnchors++;
}
if (parsedAnchors != anchorCount) {
Serial.print("→ 경고: DIST가 가리킨 Anchor 수(");
Serial.print(anchorCount);
Serial.print(")와 실제 파싱 수(");
Serial.print(parsedAnchors);
Serial.println(") 불일치");
}
// POS, x, y, z, qf
if (i < (int)t.size() && t[i] == "POS") {
// parseLEP는 "POS,..." 전체 문자열을 기대하므로 그대로 만들어 전달
String posLine = "POS";
for (int k = i + 1; k < (int)t.size(); ++k) {
posLine += ",";
posLine += t[k];
}
Serial.println("▶ 태그 위치:");
parseLEP(posLine);
} else {
Serial.println("→ POS 정보 없음");
}
}
