diff --git a/docs/NTRIP-단일기준국-전환-조사.md b/docs/NTRIP-단일기준국-전환-조사.md new file mode 100644 index 0000000..f42a584 --- /dev/null +++ b/docs/NTRIP-단일기준국-전환-조사.md @@ -0,0 +1,238 @@ +# VRS(가상 기지국) → 단일기준국(실제 상시관측소) 전환 + +> 배경: GNSS 운용 담당자로부터 "가상 기지국(VRS) 말고 주변의 진짜 기지국을 우선으로 잡아라"는 +> 조언을 받았고, 실제로 **RTK Fixed 도달 자체가 안 되는** 상황이라 VRS 사용을 중단하기로 결정. +> 조사·구현일 2026-08-22. 캐스터는 직접 조회해 확인했고, 코드는 로컬에서 컴파일·단위검증까지 +> 마쳤다. 하드웨어 검증은 §7 절차로 로봇에서 해야 한다. + +--- + +## 1. 결론 먼저 + +**UM982 모듈 설정 문제가 아니다.** 수신기에는 "어느 기지국을 잡을지" 고르는 명령이 없다 +(Unicore N4 매뉴얼 §4.6 `CONFIG RTK` 확인). 기지국 선택은 전적으로 **NTRIP 마운트포인트**가 +결정한다. + +그리고 조사 중에 **Fixed 가 안 되는 유력한 원인**을 따로 찾았다 — §3. 마운트포인트를 바꿔야 +하는 이유가 "기준 프레임 일관성" 뿐만이 아니게 됐다. + +--- + +## 2. Fixed 실패의 유력 원인 — VRS 스트림과 UM982 의 위성군 불일치 + +### 2.1 두 자료를 대조하면 나온다 + +**(A) VRS 스트림에 실제로 들어있는 것** — [진단기록 §2.5](UM982-RTK-Fixed-진단기록_2026-07-30.md) +에서 파이썬으로 직접 접속해 파싱한 결과: + +``` +1005/1007 기준국 좌표/안테나 +1030~1033 네트워크 RTK 보조 +1075 / 1085 / 1095 / 1115 GPS / GLONASS / Galileo / QZSS MSM5 +※ 북두(BeiDou) 보정 메시지는 이 마운트포인트에 없음 +``` + +**(B) UM982 가 지원한다고 매뉴얼에 적힌 것** — Appendix 2 "Supported RTCM V3 messages": + +| 위성군 | 매뉴얼 기재 | VRS 가 보내는 것 | 판정 | +|---|---|---|---| +| GPS | 1001~1004, **1074/1075** | 1075 | ✅ 쓸 수 있음 | +| GLONASS | 1009~1012, **1084/1085** | 1085 | ✅ 쓸 수 있음 | +| BeiDou | 1104, **1123~1127** | **없음** | ❌ 지원하는데 안 옴 | +| Galileo | **1091~1097 없음** (1094 만 base 출력 예시에 등장) | 1095 | ❌ 올 수 있는데 못 씀 | +| QZSS | **1111~1117 아예 없음** | 1115 | ❌ 올 수 있는데 못 씀 | + +### 2.2 무슨 뜻인가 + +VRS 는 4개 위성군 보정을 보내지만, 그중 UM982 가 RTK 에 실제로 쓸 수 있는 건 **GPS 와 GLONASS +둘뿐일 가능성이 높다**. 동시에 UM982 가 잘 지원하는 **BeiDou 는 이 VRS 가 아예 안 보낸다** — +한국에서 북두는 가시위성이 많은데 통째로 놓치는 셈이다. + +진단기록에서 "위성 31개 잡히는데 Fixed 가 안 된다"고 했던 것과 앞뒤가 맞는다. **수신기가 추적하는 +위성 수(nSV)와, 기지국 보정이 있어서 RTK 모호성 해석에 실제로 기여하는 위성 수는 다르다.** +`CONFIG SIGNALGROUP 4 5` 로 5개 위성군을 추적해도, 보정이 안 오거나 못 읽는 위성군은 RTK 에 +기여하지 못한다. + +> ⚠️ 이건 **가설**이다. 매뉴얼 Appendix 2 가 불완전할 가능성도 있다(1094 가 다른 절에는 나오는 +> 걸 보면 목록 관리가 엉성하다). §7.1 로 확정할 것. + +### 2.3 그래서 기준국 선택 기준이 바뀐다 +소스테이블의 위성군 문자열(`GPS+GLO+GAL+BDS+QZS`)이 아니라 **실제로 오는 메시지 ID** 로 판단해야 +한다. **BeiDou MSM(1124/1125)을 보내는 관측소가 진짜 이득**이고, Galileo/QZSS 는 있으나 마나다. + +--- + +## 3. VRS 를 그만 써야 하는 나머지 이유 + +### 3.1 기준 프레임이 움직인다 +VRS 는 GGA 로 보고한 위치 근처에 가상 기지국을 만들고, 로버가 임계거리를 넘으면 **가상 기지국을 +새 위치로 재생성**한다. 그때마다 기준점이 바뀌어 해에 불연속이 생기고 RTK 재초기화가 일어날 수 +있다. FAST-LIVO2-RTK 백엔드는 GNSS 해를 GTSAM 팩터로 넣으므로, 궤적 도중의 기준 프레임 변경은 +그대로 제약조건 불일치가 된다. 한 스캔 세션 동안 기준점은 고정돼야 한다. + +### 3.2 사후처리(PPK)가 불가능하다 +VRS 스트림은 서버가 그 순간 합성한 것이라 재현이 안 된다. 실제 관측소를 쓰면 +[gnssdata.or.kr](https://gnssdata.or.kr) 에서 해당 시간대 RINEX 를 받아 재처리할 수 있다. + +### 3.3 대가 — 거리 의존성 +단일기준국은 베이스라인이 멀수록 나빠진다(대략 1km당 1mm + 전리층 비상관). 실무상 10~20km 이내가 +좋고, 30km 넘어가면 Fixed 가 눈에 띄게 어려워진다. → 드라이버가 베이스라인을 로깅하고 임계 초과 +시 경고하도록 했다(§5). + +--- + +## 4. 캐스터 실측 조사 + +`tools/ntrip_probe.py table` 로 소스테이블을 직접 받아 확인(조회는 인증 불필요). + +| 캐스터 | 서버 | 결과 | +|---|---|---| +| RTS1.ngii.go.kr:2101 | Trimble Pivot 5.2 | STR 5개 **전부 `solution=1`(네트워크)**. VRS-RTCM23/31/34, VRS-CMRx, RTK-RTCM32 | +| RTS2.ngii.go.kr:2101 | Geo++ GNSMART | FKP-RTCM31, VRS-RTCM31/32, SSR-SSRG — **전부 네트워크** | +| **gnssdata.or.kr:2101** | NTRIP Caster 1.1 | **STR 562개 전부 `solution=0`(단일기준국), `nmea=0`, 고유 관측소 176곳** ✅ | +| gnss.eseoul.go.kr:2101 | Trimble Pivot 4.3 | VRS 외에 서울권 관측소별 `solution=0` 스트림 제공 (대안) | + +즉 **RTS1/RTS2 에는 단일기준국이 아예 없다.** 진짜 기준국은 GNSS 데이터 통합센터에 있다. +마운트포인트 명명 = `<4자리 관측소코드>-<포맷>` (예: `DBON-RTCM32`). 포맷은 `-RTCM32` 를 쓴다. + +RTS1 의 `RTK-RTCM32` 는 이름상 단일기준국 서비스일 수 있으나 소스테이블은 네트워크로 신고한다. +표기는 못 믿는다 — VRS-RTCM34 도 `1004,1014/1015/1016` 을 신고하지만 실제로는 `1030~1033, MSM5` +가 온다. 계정을 그대로 쓸 수 있어 제일 간단한 선택지이므로 §7.2 로 정체를 확인할 가치가 있다. + +⚠️ **인증**: gnssdata.or.kr 은 무인증 시 `401`. RTS1 계정(`hyuk6578`)이 여기서도 되는지 **미확인** +이고, 이게 최우선 확인 항목이다. 안 되면 별도 가입 필요(위치기준과 031-210-2655). + +--- + +## 5. 구현 완료 내역 + +### 5.1 설정 — [`config/um982.yaml`](../src/um982_driver/config/um982.yaml) +```yaml +ntrip_host: "gnssdata.or.kr" +ntrip_mountpoint: "AUTO" # 최근접 단일기준국 자동 선택 (또는 "DBON-RTCM32" 처럼 고정) +ntrip_auto_format: "RTCM 3.2" +max_baseline_km: 30.0 +gga_send_period: 0.0 # 단일기준국은 GGA 업링크 불필요 +``` + +> 수정 후 **반드시 리빌드**. `install/` 은 복사본이라 리빌드 없이는 반영 안 된다 +> (진단기록 §2.3 에서 이것 때문에 NTRIP 이 빈 계정으로 붙던 전례). +> `colcon build --packages-select um982_driver` 또는 `--symlink-install`. + +### 5.2 드라이버 — [`um982_driver_node.cpp`](../src/um982_driver/src/um982_driver_node.cpp) + +**① AUTO 최근접 기준국 선택** +소스테이블을 받아 `solution=0` 인 STR 만 후보로 두고 현재 위치에서 거리순 정렬, 가장 가까운 +곳에 붙는다. 접속 실패 시 다음 후보로 자동 전환(관측소 점검/장애 대비, 최대 5곳). +"주변에 있는 진짜 기지국을 우선으로 잡는다"를 그대로 구현한 것. + +**② GGA 대기 제거** +기존에는 첫 유효 GGA 가 나올 때까지 NTRIP 접속을 막았다. VRS 는 그래야 하지만 단일기준국은 +`nmea=0` 이라 불필요하고, 이 대기 때문에 **수신기가 자력으로 fix 를 얻기 전까지 RTCM 이 한 +바이트도 안 들어갔다**. 조건이 나쁜 곳에서 콜드스타트할 때 수렴이 늦어지는 원인. +이제 `gga_send_period <= 0` 이면 즉시 접속한다(AUTO 는 위치가 필요해 첫 GGA 까지만 기다림). + +**③ RTCM 스니퍼 — 캐스터가 보낸 것** +주입 경로에서 RTCM3 프레임을 뜯어 30초마다 요약한다: +- 메시지 종류별 카운트 +- 보정이 오는 위성군 +- 네트워크 RTK 메시지(1014/1015/1016/1030~1035) 발견 시 **경고** → 단일기준국이 아님 +- **매뉴얼 미기재 메시지 발견 시 경고** (§2 의 Galileo/QZSS 문제를 런타임에 바로 드러냄) +- 1005/1006 에서 기지국 ID·좌표·**베이스라인 km** 를 로깅, 좌표가 움직이면 VRS 로 판정해 경고 + +**④ RTCMSTATUS — 수신기가 받아들인 것** +기동 시 `RTCMSTATUSA ONCHANGED` 를 RAM 에만 설정(SAVECONFIG 안 함)하고, 수신기가 뱉는 +`#RTCMSTATUSA` 를 파싱해 로깅한다. 매뉴얼 §7.3.56 기준 `,,,,`. +**③과 ④를 대조하면 "캐스터는 보냈는데 수신기가 안 쓴" 위성군이 그대로 드러난다** — §2 가설의 +결정적 검증 수단. + +**⑤ 접속 거부 사유 로깅** +401(계정)과 404(마운트포인트 오타/미개방)를 구분해 찍는다. + +### 5.3 발견해 고친 버그 — RTCM 파서 스톨 +프레임 추출에서 `0xD3` 만 보고 동기를 잡으면, 관측치 페이로드 안의 우연한 `0xD3` 을 프레임 +시작으로 착각한다. 그 가짜 길이(최대 1023B)만큼 데이터가 더 쌓일 때까지 **파서가 멈춘다**. +RTCM3 는 길이 상위 바이트의 reserved 6bit 가 항상 0 이므로 이걸로 걸러내도록 수정 +(`(buf[j+1] & 0xFC) != 0` 이면 오동기). 파이썬 도구에도 동일 수정 적용. + +### 5.4 로컬 검증 +ROS 없는 맥에서 스텁 헤더로 `-Wall -Wextra` 컴파일 통과. RTCM 헬퍼는 단위테스트로 검증: +1005/1006 파싱·ECEF→위경도 왕복(1e-7도), 안테나고, 베이스라인 거리(파이썬 참조구현과 일치), +프레임 추출(연접/1바이트 분할/선행 쓰레기/가짜 0xD3/CRC 깨짐 복구), 메시지 분류 함수 — **전부 통과**. +※ 이건 로컬 검증이고, **실제 하드웨어·실제 스트림 검증은 안 됐다**(§7). + +--- + +## 6. 조사 도구 — [`tools/ntrip_probe.py`](../tools/ntrip_probe.py) + +표준 라이브러리만 쓰는 단독 실행 스크립트. 드라이버를 안 건드리고 캐스터를 조사할 때. + +```bash +python3 ntrip_probe.py table gnssdata.or.kr 2101 --filter DBON +python3 ntrip_probe.py near gnssdata.or.kr 2101 <위도> <경도> --format RTCM3.2 +python3 ntrip_probe.py sniff gnssdata.or.kr 2101 DBON-RTCM32 --user --pass --secs 60 +``` + +`sniff` 판정 기준: + +| 관찰 | 의미 | +|---|---| +| 1014/1015/1016/1030~1035 존재 | ❌ 네트워크 해 (MAC/FKP/VRS) | +| 1005/1006 좌표가 관측 중 이동 | ❌ VRS | +| 네트워크 메시지 없음 + 1005/1006 고정 | ✅ 단일기준국 | +| 1075/1085/1095/1115/1125 구성 | 어느 위성군 보정이 오는지 — **1124/1125(BDS) 유무가 핵심** | + +--- + +## 7. 로봇에서 할 검증 (순서대로) + +### 7.1 §2 가설 확정 — 먼저 이것부터 +**아직 VRS 인 상태로** 드라이버를 새로 빌드해 띄우고 로그를 본다. +``` +[um982_driver]: RTCM 누적 ... | 메시지: 1005x.. 1030x.. 1075x.. 1085x.. 1095x.. 1115x.. +[um982_driver]: ⚠️ 매뉴얼 미기재 메시지: 1095(Galileo) 1115(QZSS) — 이 위성군은 RTK 에 안 쓰일 수 있다 +[um982_driver]: 수신기 RTCM 수용: 1075,1234,0,9,... +``` +**`수신기 RTCM 수용:` 줄에 1095/1115 가 안 나오면 §2 가설이 맞다.** 나오면 가설은 틀렸고 +원인을 다시 찾아야 한다. 어느 쪽이든 이 로그 한 번이면 결판난다. + +### 7.2 계정 확인 +```bash +python3 tools/ntrip_probe.py sniff gnssdata.or.kr 2101 DBON-RTCM32 --user hyuk6578 --pass --secs 60 +``` +`401` 이면 별도 가입 필요. 그동안의 대안으로 RTS1 `RTK-RTCM32` 정체도 확인: +```bash +python3 tools/ntrip_probe.py sniff RTS1.ngii.go.kr 2101 RTK-RTCM32 \ + --user hyuk6578 --pass --gga-from-latlon <위도> <경도> --secs 60 +``` + +### 7.3 관측소 선정 +`near` 로 작업 위치 기준 후보를 뽑고, `sniff` 로 **BDS(1124/1125)를 보내는 곳**을 고른다. +소스테이블 좌표는 대부분 소수점 2자리(약 1km)이고 4개 그룹은 여러 관측소가 같은 좌표로 잘못 +적혀 있으니(GGEO/JJHG/SGWI/YNGU 가 모두 38.29,128.14), 실제 거리는 `sniff` 의 1005/1006 으로 확인. + +### 7.4 전환 후 A/B +난간 위치(진단기록에서 Fixed 나왔던 조건)에서 VRS vs 단일기준국으로 Fixed 수렴시간·h_acc 비교. +안테나 위치가 여전히 지배적 변수이므로(진단기록 §3) **같은 위치에서** 비교해야 의미가 있다. + +--- + +## 8. 미확정 사항 + +- [ ] §2 가설 — 수신기가 Galileo/QZSS MSM 을 실제로 버리는가 (7.1 로 확정) +- [ ] RTS1 계정이 gnssdata.or.kr 에서 통하는지 +- [ ] RTS1 `RTK-RTCM32` 가 단일기준국인지 +- [ ] 작업 위치 기준 최근접 관측소 확정 +- [ ] AUTO 선택 로직의 실스트림 동작 (로컬 컴파일·단위테스트만 함) + +--- + +## 9. 관련 파일 + +| 파일 | 내용 | +|---|---| +| [`tools/ntrip_probe.py`](../tools/ntrip_probe.py) | 소스테이블 조회 / 최근접 기준국 / 스트림 VRS 판정 | +| [`src/um982_driver/config/um982.yaml`](../src/um982_driver/config/um982.yaml) | 캐스터·마운트포인트·GGA 설정 | +| [`src/um982_driver/src/um982_driver_node.cpp`](../src/um982_driver/src/um982_driver_node.cpp) | AUTO 선택, RTCM 스니퍼, RTCMSTATUS 로깅 | +| [`docs/UM982-RTK-Fixed-진단기록_2026-07-30.md`](UM982-RTK-Fixed-진단기록_2026-07-30.md) | VRS 스트림 실제 구성(§2.5), 안테나 위치가 지배적(§3), 리빌드 함정(§2.3) | +| `docs/Unicore Reference Commands Manual ... R1.4.pdf` | Appendix 2 지원 RTCM 목록, §4.6 CONFIG RTK, §7.3.56 RTCMSTATUS | diff --git a/src/um982_driver/README.md b/src/um982_driver/README.md index 3f19386..56307de 100644 --- a/src/um982_driver/README.md +++ b/src/um982_driver/README.md @@ -12,11 +12,16 @@ UM982(Unicore) → `gnss_comm/msg/GnssPVTSolnMsg` 드라이버 노드. FAST-LIVO 같은 노드 안에 있다: ``` -NTRIP(VRS) ──RTCM3──▶ UM982 ──BESTNAV(binary)+GGA(ascii)──▶ um982_driver ──GnssPVTSolnMsg──▶ 백엔드 - ▲ │ - └────────────────────── GGA(VRS 유지) ───────────────────┘ +NTRIP(단일기준국) ──RTCM3──▶ UM982 ──BESTNAV(binary)+GGA(ascii)──▶ um982_driver ──GnssPVTSolnMsg──▶ 백엔드 + ▲ │ + └──────────── GGA (VRS 쓸 때만; 단일기준국은 불필요) ──────────┘ ``` +> VRS(가상 기지국) 대신 실제 상시관측소를 쓴다. 배경·근거는 +> [`docs/NTRIP-단일기준국-전환-조사.md`](../../docs/NTRIP-단일기준국-전환-조사.md). +> 드라이버가 주입 중인 RTCM 을 스니핑해 기지국 좌표·베이스라인·위성군 구성을 로깅하므로, +> 붙은 곳이 진짜 단일기준국인지 로그로 바로 확인된다. + ## 수신기 사전 설정 드라이버 실행 전에 UM982 가 아래를 출력하도록 저장돼 있어야 한다(리포 루트 `rtk/config_all.py`): ``` @@ -55,7 +60,14 @@ ros2 run um982_driver um982_driver_node --ros-args \ ## 파라미터 (config/um982.yaml) - `serial_port`, `serial_baud`(460800), `gps_topic` -- `use_ntrip`, `ntrip_host/port/mountpoint/user/pass`, `gga_send_period` +- `use_ntrip`, `ntrip_host/port/mountpoint/user/pass` +- `ntrip_mountpoint`: 관측소 코드 직접 지정(`"DBON-RTCM32"`) 또는 `"AUTO"` + (소스테이블에서 `solution=0` 인 최근접 기준국 자동 선택, 실패 시 다음 후보로 전환) +- `ntrip_auto_format`(기본 `"RTCM 3.2"`): AUTO 선택 시 포맷 필터 +- `gga_send_period`: **0 이면 GGA 업링크 없이 즉시 접속**(단일기준국). VRS 쓸 때만 10.0 +- `max_baseline_km`(기본 30): 기지국 거리 초과 시 경고 +- `enable_rtcm_status_log`(기본 true): 기동 시 `RTCMSTATUSA ONCHANGED` 설정(RAM 만), + 수신기가 실제로 받아들인 RTCM 을 로깅 - `publish_fixed_only`(기본 false), `inflate_cov_when_not_fixed`(기본 true) > 백엔드는 fix 품질을 거르지 않으므로, RTK Fixed 가 아닌 해는 `inflate_cov_when_not_fixed` diff --git a/src/um982_driver/RTK-녹화-명세.md b/src/um982_driver/RTK-녹화-명세.md new file mode 100644 index 0000000..9070d52 --- /dev/null +++ b/src/um982_driver/RTK-녹화-명세.md @@ -0,0 +1,200 @@ +# RTK(UM982) ROS 2 녹화 명세서 + +> 대상: UM982(WTRTK-982) RTK 수신 데이터를 ROS 2 로 발행·**녹화**하기 위한 이관/구성 명세. +> 구현체: `um982_driver` (C++ ROS 2 노드). 근거 문서: `rtk/UM982-명령어검토-및-바이너리포팅.md`. +> 검증: 국가 통합기준점 U0373/U0374 대비 수평 1.4~2.6cm (rtk/Log.md). + +--- + +## 1. 개요 & 데이터 흐름 + +``` +NTRIP(VRS,KGD2002) ──RTCM3──▶ UM982 ──BESTNAV(binary)+GGA(ascii)──▶ um982_driver + ▲ │ + └──────────────── GGA(VRS 유지, 10s) ◀───────────────────────────┘ + │ GnssPVTSolnMsg (10Hz) + ▼ + /ublox_driver/receiver_pvt + │ + ros2 bag record ──▶ .db3/.mcap +``` + +- 시리얼 포트는 **한 프로세스만 점유** → RTCM 주입과 BESTNAV 파싱이 **한 노드**(`um982_driver`)에 통합. +- 녹화는 이 노드가 발행하는 토픽을 `ros2 bag record` 로 담는다. + +--- + +## 2. 이관 대상 & 워크스페이스 구성 (무엇을 어떻게 옮기나) + +**옮길 패키지 2개** (그 외 의존 없음 — 시리얼=termios, NTRIP=POSIX 소켓): + +| 패키지 | 역할 | 위치(원본) | +|---|---|---| +| `gnss_comm` | `GnssPVTSolnMsg`/`GnssTimeMsg` 메시지 정의 | `FAST-LIVO2-RTK-ROS2/thirdparty/gnss_comm` | +| `um982_driver` | 드라이버 노드(파싱+NTRIP+발행) | `FAST-LIVO2-RTK-ROS2/src/um982_driver` | + +**녹화 전용 최소 워크스페이스** (전체 FAST-LIVO2 빌드 불필요): +``` +fhd_rtk_ws/ +└── src/ + ├── gnss_comm/ ← thirdparty/gnss_comm 복사 + └── um982_driver/ ← src/um982_driver 복사 +``` +```bash +cd fhd_rtk_ws +colcon build --packages-select gnss_comm um982_driver +source install/setup.bash +``` +> ROS 2 Humble, C++17. `um982_driver` 는 `rclcpp`, `gnss_comm` 만 의존. + +--- + +## 3. 사전 조건 — 수신기 설정 (필수) + +드라이버 실행 전 UM982 가 아래를 **저장(SAVECONFIG)** 하고 있어야 한다. 리포 `rtk/config_all.py` 한 번 실행: + +| 항목 | 값 | 이유 | +|---|---|---| +| `BESTNAVB COM3 0.1` | 10Hz 바이너리 | **파싱 소스** | +| `GPGGA COM3 1` | 1Hz | NTRIP VRS 업링크 | +| COM3 baud | `460800` | RTCM+로그 대역 | +| `MODE ROVER` | — | 로버 측위 | +| `CONFIG RTK RELIABILITY 3 1` | 기본 | 오확정 방지 | + +> ⚠️ `FRESET` 시 baud 가 115200 으로 초기화됨 → `rtk/set_baud.py` 재실행 필요. + +--- + +## 4. 토픽 & 메시지 명세 + +### 4.1 토픽 + +| 항목 | 값 | +|---|---| +| 토픽명 | `/ublox_driver/receiver_pvt` (파라미터 `gps_topic`) | +| 타입 | `gnss_comm/msg/GnssPVTSolnMsg` | +| 발행 주기 | BESTNAV 주기 = **10 Hz** (0.1s) | +| QoS | `KeepLast(2000)`, 기본 신뢰성(reliable) | +| 프레임/스탬프 | **메시지에 std_msgs/Header 없음.** 시간은 메시지 내부 `time.week/tow` | + +### 4.2 `GnssPVTSolnMsg` 필드 (드라이버가 채우는 값) + +| 필드 | 타입/단위 | 채움 | BESTNAV 소스 | +|---|---|---|---| +| `time.week` | uint32 (GPS week) | ✅ | 헤더 Wn | +| `time.tow` | float64 (초) | ✅ | 헤더 Ms÷1000 | +| `latitude` | float64 (deg) | ✅ | lat | +| `longitude` | float64 (deg) | ✅ | lon | +| `altitude` | float64 (m, **타원체고**) | ✅ | hgt(MSL)+undulation | +| `height_msl` | float64 (m) | ✅ | hgt | +| `h_acc` | float64 (m) | ✅ | √(latσ²+lonσ²) | +| `v_acc` | float64 (m) | ✅ | hgtσ | +| `vel_n/vel_e/vel_d` | float64 (m/s, NED) | ✅ | hor·cos(trk)/hor·sin(trk)/−vert | +| `vel_acc` | float64 (m/s) | ✅ | Horspd std | +| `num_sv` | uint8 | ✅ | #solnSVs | +| `fix_type` | uint8 (0=no,3=3D) | ✅ | pos_type | +| `valid_fix` | bool | ✅ | pos_type≠NONE | +| `diff_soln` | bool | ✅ | pos_type∉{NONE,SINGLE} | +| `carr_soln` | uint8 (0/1/2) | ✅ | 48/49/50→2, 32/33/34→1, else 0 | +| `p_dop` | float64 | ➖ 0 | BESTNAV 미제공(필요시 PVTSLN) | + +> `carr_soln`: **2=RTK Fixed(NARROW_INT 등), 1=RTK Float, 0=단독/DGPS**. + +--- + +## 5. 드라이버 파라미터 명세 (`config/um982.yaml`) + +| 파라미터 | 기본값 | 설명 | +|---|---|---| +| `serial_port` | `/dev/ttyUSB0` | UM982 COM3(USB). 리눅스 `/dev/ttyUSB*` | +| `serial_baud` | `460800` | 수신기 저장값과 일치 | +| `gps_topic` | `/ublox_driver/receiver_pvt` | 발행 토픽(백엔드 `gps.gps_topic` 와 일치) | +| `use_ntrip` | `true` | false 면 RTCM 주입 안 함(외부 보정 시) | +| `ntrip_host/port/mountpoint` | `RTS1.ngii.go.kr` / `2101` / `VRS-RTCM34` | NTRIP 접속 | +| `ntrip_user/pass` | `""` | ⚠️ 리포 평문 커밋 지양 — yaml/CLI/환경변수로 주입 | +| `gga_send_period` | `10.0` | VRS 유지용 GGA 재전송 주기(s) | +| `publish_fixed_only` | `false` | true=RTK Fixed 해만 발행 | +| `inflate_cov_when_not_fixed` | `true` | fix 아님/코스팅 시 h_acc·v_acc 부풀림 | +| `max_corr_age` | `30.0` | diff_age 초과 시 코스팅 간주(공분산↑). 0=비활성 | + +> NTRIP 끊겨도 UM982 는 `RTK TIMEOUT`(기본 600s) 동안 Fixed 를 코스팅 유지 → `max_corr_age` 로 신뢰 낮춤. + +--- + +## 6. 시간 의미 (중요) + +- 메시지에 Header/stamp 가 **없다.** 시간은 `time.week`(GPS week) + `time.tow`(주 내 초). +- UNIX 복원: `unix = week·604800 + tow + 315964800 − 18(leap)` (백엔드 `optimization.cpp::gpsHandler` 와 동일). +- 유효 조건: 헤더 TimeStatus=`FINE` & week>1 (fix 전에는 week=1/UNKNOWN). +- `ros2 bag` 은 **수신 시각(bag timestamp)** 을 별도로 기록하지만, **권위 시각은 메시지 내부 week/tow**. 센서와의 시각 동기는 이 값 기준. + +--- + +## 7. 녹화 절차 + +### 7.1 실행 +```bash +source install/setup.bash +ros2 launch um982_driver um982_driver.launch.py # 또는 아래 run +# 자격증명을 CLI 로 주입: +ros2 run um982_driver um982_driver_node --ros-args \ + --params-file src/um982_driver/config/um982.yaml \ + -p ntrip_user:= -p ntrip_pass:= +``` +콘솔에 `RTK FIXED` 전환 로그가 뜨고, `ros2 topic hz /ublox_driver/receiver_pvt` ≈ 10Hz 확인. + +### 7.2 GNSS 단독 녹화 +```bash +ros2 bag record -o rtk_$(date +%F_%H%M) /ublox_driver/receiver_pvt +``` + +### 7.3 센서 + GNSS 동시 녹화 (FAST-LIVO2 후처리용) +```bash +ros2 bag record -o run_$(date +%F_%H%M) \ + /livox/lidar /livox/imu /left_camera/image \ + /ublox_driver/receiver_pvt \ + --storage mcap # 없으면 sqlite3(기본) +``` +> 초기 수십 초 **RTK Fixed + 충분한 이동** 후 녹화(백엔드 SVD 정합 전제). `rtk/UM982-녹화-및-연동-구현.md` §7 참조. + +### 7.4 (권장) 원시 BESTNAV 동시 녹화 — 재처리 대비 +현재 드라이버는 파싱 결과만 발행한다. 파싱 로직을 나중에 고쳐 재생성하려면 **원시 바이트도 함께** 남기는 것을 권장(별도 옵션 필요 — §10). + +--- + +## 8. 녹화 데이터 포맷 & 재생 + +- 저장: ROS 2 bag (`sqlite3` `.db3` 기본, 또는 `mcap` `.mcap`). +- 확인: `ros2 bag info run_YYYY-MM-DD_HHMM` → 토픽/메시지 수/기간. +- 재생: `ros2 bag play run_YYYY-MM-DD_HHMM` → `/ublox_driver/receiver_pvt` 재발행. +- FAST-LIVO2 백엔드는 이 토픽을 구독해 첫 fix 를 ENU 원점으로 잡고 GTSAM 융합(`gps.gps_topic` 일치 필요). + +--- + +## 9. 품질/필터 (녹화되는 데이터의 신뢰) + +- 백엔드에 fix 품질 필터가 **없으므로** 드라이버 단에서 처리: + - `publish_fixed_only=true` → RTK Fixed 해만 녹화(가장 보수적). + - `inflate_cov_when_not_fixed=true` + `max_corr_age` → fix 아님/코스팅 시 `h_acc/v_acc` 를 키워 후처리 가중 약화. +- CRC(Unicore Appendix 1) 통과 프레임만 발행 → 손상 프레임 녹화 안 됨. + +--- + +## 10. 확장(옵션) — 원시 스트림 동시 녹화 + +재처리·디버깅을 위해 원시 BESTNAV/GGA 바이트를 별도 토픽으로 녹화하려면 드라이버에 +`std_msgs/msg/ByteMultiArray`(또는 String) 발행부를 추가하면 된다(예: `/um982/raw`). +필요 시 요청 → 드라이버에 `publish_raw` 파라미터로 추가. + +--- + +## 11. 검증 체크리스트 + +- [ ] `rtk/config_all.py` 로 수신기 설정 저장(BESTNAVB/GPGGA, 460800, MODE ROVER) +- [ ] `colcon build --packages-select gnss_comm um982_driver` 성공 +- [ ] `serial_port` / NTRIP 자격증명(config or CLI) 확인 +- [ ] `ros2 topic hz /ublox_driver/receiver_pvt` ≈ 10Hz, 콘솔 `RTK FIXED` +- [ ] `ros2 topic echo` 로 lat/lon/altitude, time.week(>1)/tow, carr_soln=2 확인 +- [ ] `ros2 bag record` → `ros2 bag info` 로 토픽·기간 확인 +- [ ] (동시녹화) 센서 4종 + GNSS 토픽 모두 bag 에 존재 +``` diff --git a/src/um982_driver/config/um982.yaml b/src/um982_driver/config/um982.yaml index dd75087..0db3299 100644 --- a/src/um982_driver/config/um982.yaml +++ b/src/um982_driver/config/um982.yaml @@ -7,14 +7,23 @@ um982_driver: # ── 발행 토픽 (백엔드 gps.gps_topic 와 일치) ── gps_topic: "/ublox_driver/receiver_pvt" - # ── NTRIP (VRS) ── + # ── NTRIP (단일기준국) ── + # RTS1/RTS2 는 마운트포인트가 전부 네트워크 해(VRS/FKP)뿐이다. 실제 상시관측소는 + # GNSS 데이터 통합센터(gnssdata.or.kr) 에 있다 — 176곳, 전부 solution=0. + # 배경/근거: docs/NTRIP-단일기준국-전환-조사.md use_ntrip: true - ntrip_host: "RTS1.ngii.go.kr" + ntrip_host: "gnssdata.or.kr" ntrip_port: 2101 - ntrip_mountpoint: "VRS-RTCM34" + ntrip_mountpoint: "AUTO" # "AUTO" = 소스테이블에서 최근접 단일기준국 자동 선택. + # 고정하려면 관측소 코드 직접 지정(예: "DBON-RTCM32") + ntrip_auto_format: "RTCM 3.2" # AUTO 선택 시 포맷 필터(소스테이블 format 필드와 정확히 일치) + max_baseline_km: 30.0 # 기지국 거리 초과 시 경고. 단일기준국은 멀수록 Fixed 가 어렵다 ntrip_user: "" # ⚠️ 여기에 채우거나 launch 인자/환경변수로 주입(리포에 평문 저장 지양) ntrip_pass: "" - gga_send_period: 10.0 # VRS 유지용 GGA 재전송 주기(초) + gga_send_period: 0.0 # 0 = GGA 업링크 안 함. 단일기준국은 nmea=0 이라 불필요하고, + # 0 이면 첫 fix 를 기다리지 않고 즉시 접속한다. + # VRS 로 되돌릴 때만 10.0 으로. (AUTO 는 위치가 필요해 + # 0 이어도 첫 GGA 까지는 기다린다) # ── 품질 처리 (백엔드에 fix 품질 필터가 없음) ── publish_fixed_only: false # true = RTK Fixed(carr_soln=2) 해만 발행 diff --git a/src/um982_driver/src/um982_driver_node.cpp b/src/um982_driver/src/um982_driver_node.cpp index a99da2a..ed6ec9b 100644 --- a/src/um982_driver/src/um982_driver_node.cpp +++ b/src/um982_driver/src/um982_driver_node.cpp @@ -24,10 +24,14 @@ #include #include +#include #include #include #include +#include +#include #include +#include #include #include #include @@ -75,6 +79,163 @@ uint32_t block_crc32(const uint8_t *buf, size_t n) { return crc; } +// ── RTCM3 스니핑 (주입 스트림 검증) ───────────────────────────────────────── +// 프레임: 0xD3 | 6bit reserved + 10bit len | payload | CRC-24Q 3B +// 목적 두 가지: +// (1) 붙은 마운트포인트가 실제 단일기준국인지 — 1005/1006 좌표 고정 + 네트워크 메시지 부재 +// (2) UM982 가 실제로 쓸 수 있는 위성군 보정이 오는지 (아래 rtcm_documented 참고) +const std::array &crc24q_table() { + static const std::array t = [] { + std::array a{}; + for (int i = 0; i < 256; ++i) { + uint32_t crc = static_cast(i) << 16; + for (int j = 0; j < 8; ++j) { + crc <<= 1; + if (crc & 0x1000000) crc ^= 0x1864CFBUL; + } + a[i] = crc & 0xFFFFFF; + } + return a; + }(); + return t; +} + +uint32_t crc24q(const uint8_t *buf, size_t n) { + const auto &tab = crc24q_table(); + uint32_t crc = 0; + for (size_t i = 0; i < n; ++i) + crc = ((crc << 8) & 0xFFFFFF) ^ tab[((crc >> 16) ^ buf[i]) & 0xFF]; + return crc; +} + +// MSB-first 비트 리더 (RTCM DF 필드) +struct Bits { + const uint8_t *b; + size_t pos{0}; + explicit Bits(const uint8_t *p) : b(p) {} + uint64_t u(int n) { + uint64_t v = 0; + for (int i = 0; i < n; ++i) { v = (v << 1) | ((b[pos >> 3] >> (7 - (pos & 7))) & 1); ++pos; } + return v; + } + int64_t s(int n) { + uint64_t v = u(n); + return (v & (1ULL << (n - 1))) ? static_cast(v) - (1LL << n) : static_cast(v); + } +}; + +struct BasePos { int id; double x, y, z, ant_h; bool has_ant_h; }; + +// 1005(ARP) / 1006(ARP + 안테나고) → ECEF(m) +bool parse_base_pos(const uint8_t *pl, size_t len, BasePos &out) { + if (len < 19) return false; + Bits b(pl); + int msg = static_cast(b.u(12)); + if (msg != 1005 && msg != 1006) return false; + if (msg == 1006 && len < 21) return false; + out.id = static_cast(b.u(12)); + b.u(6); // ITRF realization year + b.u(1); b.u(1); b.u(1); b.u(1); // GPS/GLO/GAL indicator, reference station indicator + out.x = static_cast(b.s(38)) * 1e-4; + b.u(1); b.u(1); // single receiver osc, reserved + out.y = static_cast(b.s(38)) * 1e-4; + b.u(2); // quarter cycle indicator + out.z = static_cast(b.s(38)) * 1e-4; + out.has_ant_h = (msg == 1006); + out.ant_h = out.has_ant_h ? static_cast(b.u(16)) * 1e-4 : 0.0; + return true; +} + +void ecef_to_lla(double x, double y, double z, double &lat, double &lon, double &alt) { + constexpr double a = 6378137.0, f = 1.0 / 298.257223563; + constexpr double e2 = f * (2.0 - f); + lon = std::atan2(y, x); + double p = std::hypot(x, y); + lat = std::atan2(z, p * (1.0 - e2)); + double n = a; + for (int i = 0; i < 10; ++i) { + n = a / std::sqrt(1.0 - e2 * std::sin(lat) * std::sin(lat)); + alt = p / std::cos(lat) - n; + lat = std::atan2(z, p * (1.0 - e2 * n / (n + alt))); + } + n = a / std::sqrt(1.0 - e2 * std::sin(lat) * std::sin(lat)); + alt = p / std::cos(lat) - n; + lat *= 180.0 / M_PI; + lon *= 180.0 / M_PI; +} + +double haversine_km(double lat1, double lon1, double lat2, double lon2) { + constexpr double R = 6371.0, D = M_PI / 180.0; + double dlat = (lat2 - lat1) * D, dlon = (lon2 - lon1) * D; + double h = std::sin(dlat / 2) * std::sin(dlat / 2) + + std::cos(lat1 * D) * std::cos(lat2 * D) * std::sin(dlon / 2) * std::sin(dlon / 2); + return 2 * R * std::asin(std::sqrt(std::min(1.0, h))); +} + +// 버퍼에서 CRC 가 맞는 완성 프레임만 뽑아 콜백에 넘기고, 소비분을 앞에서 제거한다. +// on_frame(msg_type, payload, payload_len). 미완성 프레임은 버퍼에 남겨 다음 recv 를 기다린다. +template +void extract_rtcm_frames(std::vector &buf, F &&on_frame) { + size_t i = 0; + while (true) { + size_t j = i; + while (j < buf.size() && buf[j] != 0xD3) ++j; + if (j + 3 > buf.size()) { i = j; break; } + // 길이 상위 바이트의 reserved 6bit 는 RTCM3 에서 항상 0. 이걸 안 보면 관측치 페이로드 + // 안의 우연한 0xD3 을 프레임 시작으로 착각해, 그 가짜 길이만큼(최대 1023B) 데이터가 + // 더 쌓일 때까지 파서가 멈춘다. + if ((buf[j + 1] & 0xFC) != 0) { i = j + 1; continue; } + size_t len = (static_cast(buf[j + 1] & 0x03) << 8) | buf[j + 2]; + size_t total = 3 + len + 3; + if (buf.size() - j < total) { i = j; break; } + const uint8_t *f = buf.data() + j; + uint32_t got = (static_cast(f[total - 3]) << 16) + | (static_cast(f[total - 2]) << 8) + | static_cast(f[total - 1]); + if (crc24q(f, total - 3) != got) { i = j + 1; continue; } // 오정렬 → 다음 0xD3 부터 + if (len >= 2) on_frame((static_cast(f[3]) << 4) | (f[4] >> 4), f + 3, len); + i = j + total; + } + buf.erase(buf.begin(), buf.begin() + i); +} + +// 네트워크 RTK(VRS/FKP/MAC) 전용 메시지 — 하나라도 있으면 단일기준국이 아니다 +const char *network_msg_name(int t) { + switch (t) { + case 1014: return "Network Aux Station(MAC)"; + case 1015: case 1016: case 1017: return "MAC correction diff"; + case 1030: case 1031: return "Network RTK residual"; + case 1032: return "Physical Ref Station Pos(VRS)"; + case 1034: case 1035: return "FKP gradient"; + default: return nullptr; + } +} + +const char *msm_constellation(int t) { + if (t >= 1071 && t <= 1077) return "GPS"; + if (t >= 1081 && t <= 1087) return "GLONASS"; + if (t >= 1091 && t <= 1097) return "Galileo"; + if (t >= 1101 && t <= 1107) return "SBAS"; + if (t >= 1111 && t <= 1117) return "QZSS"; + if (t >= 1121 && t <= 1127) return "BeiDou"; + return nullptr; +} + +// Unicore N4 매뉴얼 Appendix 2 "Supported RTCM V3 messages" 기재분. +// ⚠️ Galileo MSM(1091~1097)과 QZSS MSM(1111~1117)은 매뉴얼에 없다. 이 메시지로만 오는 +// 위성군 보정은 수신기가 못 쓸 수 있고, 그러면 RTK 에 실제로 기여하는 위성이 확 준다. +bool rtcm_documented(int t) { + if (t >= 1001 && t <= 1004) return true; // GPS L1/L2 + if (t >= 1009 && t <= 1012) return true; // GLONASS L1/L2 + if (t == 1074 || t == 1075) return true; // GPS MSM4/5 + if (t == 1084 || t == 1085) return true; // GLONASS MSM4/5 + if (t >= 1123 && t <= 1127) return true; // BDS MSM3~7 + if (t == 1104) return true; // BDS observables (Unicore) + if (t == 1005 || t == 1006 || t == 1007 || t == 1033) return true; + if (t == 1019 || t == 1020 || t == 1042 || t == 1045 || t == 1046) return true; + return false; +} + speed_t to_speed(int baud) { switch (baud) { case 9600: return B9600; case 19200: return B19200; @@ -117,6 +278,12 @@ class Um982Driver : public rclcpp::Node { inflate_cov_ = declare_parameter("inflate_cov_when_not_fixed", true); max_corr_age_ = declare_parameter("max_corr_age", 30.0); // 보정 나이 초과 시 코스팅으로 간주 gga_period_ = declare_parameter("gga_send_period", 10.0); + // ntrip_mountpoint 가 "AUTO" 면 소스테이블을 받아 현재 위치에서 가장 가까운 + // 단일기준국(solution=0)을 고른다. 아래는 그 선택 기준. + auto_format_ = declare_parameter("ntrip_auto_format", "RTCM 3.2"); + max_baseline_km_ = declare_parameter("max_baseline_km", 30.0); // 초과 시 경고만 + + if (ntrip_mp_ != "AUTO" && ntrip_mp_ != "auto") active_mp_ = ntrip_mp_; pub_ = create_publisher(gps_topic_, rclcpp::QoS(rclcpp::KeepLast(2000))); @@ -127,6 +294,13 @@ class Um982Driver : public rclcpp::Node { RCLCPP_INFO(get_logger(), "UM982 %s @ %d → 발행 %s", serial_port_.c_str(), serial_baud_, gps_topic_.c_str()); + // RAM 에만 적용되는 로그 활성화(SAVECONFIG 안 하므로 재부팅 시 원복). 수신기가 어떤 RTCM 을 + // 실제로 받아들이는지 보려면 이게 켜져 있어야 한다. + if (declare_parameter("enable_rtcm_status_log", true)) { + static constexpr char kCmd[] = "RTCMSTATUSA ONCHANGED\r\n"; + ::write(serial_fd_, kCmd, sizeof(kCmd) - 1); + } + running_ = true; serial_thread_ = std::thread(&Um982Driver::serial_loop, this); if (use_ntrip_) ntrip_thread_ = std::thread(&Um982Driver::ntrip_loop, this); @@ -179,6 +353,7 @@ class Um982Driver : public rclcpp::Node { // 동기 이전(head)에서 GGA 추출 size_t head_end = (sync == std::string::npos) ? buf.size() : sync; scan_gga(buf, search, head_end); + scan_rtcm_status(buf, search, head_end); if (sync == std::string::npos) { if (buf.size() > 2) buf.erase(buf.begin(), buf.end() - 2); // 동기 경계 보존 @@ -228,6 +403,26 @@ class Um982Driver : public rclcpp::Node { } } + // 수신기가 "받아들인" RTCM 을 보여주는 진단 로그(#RTCMSTATUSA, 매뉴얼 §7.3.56). + // 캐스터가 "보낸" 것(log_rtcm_summary)과 대조하면, 보냈는데 수신기가 안 쓰는 위성군이 드러난다. + // 출력 형식: #RTCMSTATUSA,...;,,,,* + void scan_rtcm_status(const std::vector &buf, size_t begin, size_t end) { + static constexpr char kTag[] = "#RTCMSTATUSA"; + constexpr size_t kTagLen = sizeof(kTag) - 1; + for (size_t p = begin; p + kTagLen < end; ++p) { + if (buf[p] != '#') continue; + if (std::memcmp(&buf[p], kTag, kTagLen) != 0) continue; + size_t e = p; + while (e < end && buf[e] != '\r' && buf[e] != '\n') ++e; + if (e >= end) break; // 라인 미완 → 다음 read 대기 + std::string line(reinterpret_cast(&buf[p]), e - p); + size_t semi = line.find(';'); + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 10000, "수신기 RTCM 수용: %s", + semi == std::string::npos ? line.c_str() : line.c_str() + semi + 1); + p = e; + } + } + void handle_bestnav(const uint8_t *f) { int32_t pos_type = rd(f, OFF_POSTYPE); if (fixed_only_ && !is_rtk_fixed(pos_type)) return; @@ -242,6 +437,7 @@ class Um982Driver : public rclcpp::Node { float undu = rd(f, OFF_UNDU); m.latitude = lat; m.longitude = lon; + if (pos_type != POS_NONE) { rover_lat_.store(lat); rover_lon_.store(lon); } m.altitude = hgt + undu; // 타원체고 (백엔드 ENU 입력) m.height_msl = hgt; @@ -288,15 +484,42 @@ class Um982Driver : public rclcpp::Node { // ── NTRIP ─────────────────────────────────────────────────────────────── void ntrip_loop() { - // 첫 유효 GGA 대기(VRS 부트스트랩) - while (running_ && rclcpp::ok() && latest_gga().empty()) - std::this_thread::sleep_for(std::chrono::milliseconds(200)); + const bool auto_mp = (ntrip_mp_ == "AUTO" || ntrip_mp_ == "auto"); + const bool send_gga = (gga_period_ > 0.0); + + // GGA 대기가 필요한 경우는 둘뿐: + // · VRS/네트워크 마운트포인트 — GGA 를 보내야 스트림이 시작된다 + // · AUTO — 가장 가까운 기준국을 고르려면 내 위치를 알아야 한다 + // 단일기준국(nmea=0)에 고정 마운트포인트면 GGA 없이 즉시 붙어야 한다. 안 그러면 + // 수신기가 자력으로 fix 를 얻기 전까지 RTCM 이 한 바이트도 안 들어간다. + if (send_gga || auto_mp) { + RCLCPP_INFO(get_logger(), "NTRIP: 첫 GGA 대기 중%s", auto_mp ? " (AUTO 기준국 선택용)" : ""); + while (running_ && rclcpp::ok() && latest_gga().empty()) + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + } else { + RCLCPP_INFO(get_logger(), "NTRIP: GGA 업링크 없음(단일기준국) → 즉시 접속"); + } + if (!running_ || !rclcpp::ok()) return; + + if (auto_mp && !resolve_auto_mountpoint()) + RCLCPP_ERROR(get_logger(), "AUTO 기준국 선택 실패 — 재시도한다"); auto last_send = std::chrono::steady_clock::now(); + auto last_summary = std::chrono::steady_clock::now(); while (running_ && rclcpp::ok()) { if (ntrip_fd_ < 0) { - if (!ntrip_connect()) { std::this_thread::sleep_for(std::chrono::seconds(2)); continue; } - RCLCPP_INFO(get_logger(), "NTRIP 접속: %s:%d/%s", ntrip_host_.c_str(), ntrip_port_, ntrip_mp_.c_str()); + if (auto_mp && active_mp_.empty() && !resolve_auto_mountpoint()) { + std::this_thread::sleep_for(std::chrono::seconds(5)); + continue; + } + if (!ntrip_connect()) { + // AUTO 면 다음 후보로 넘어간다(해당 관측소 점검/장애 대비) + if (auto_mp && !next_candidate()) + RCLCPP_WARN(get_logger(), "후보 소진 → 소스테이블 재조회"); + std::this_thread::sleep_for(std::chrono::seconds(2)); + continue; + } + RCLCPP_INFO(get_logger(), "NTRIP 접속: %s:%d/%s", ntrip_host_.c_str(), ntrip_port_, active_mp_.c_str()); last_send = std::chrono::steady_clock::now(); } uint8_t buf[4096]; @@ -304,16 +527,19 @@ class Um982Driver : public rclcpp::Node { if (n > 0) { ::write(serial_fd_, buf, n); // RTCM 주입 ntrip_bytes_total_ += n; - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, - "NTRIP 수신 누적 %ld bytes (최근 recv %zd bytes)", - ntrip_bytes_total_, n); + feed_rtcm(buf, static_cast(n)); // 주입과 병행해 스트림 검증 } else if (n == 0) { RCLCPP_WARN(get_logger(), "NTRIP 연결 종료 → 재접속"); ::close(ntrip_fd_); ntrip_fd_ = -1; continue; } - // VRS 유지용 GGA 주기 재전송 auto now = std::chrono::steady_clock::now(); - if (std::chrono::duration(now - last_send).count() > gga_period_) { + // 무엇이 실제로 들어오는지 30초마다 1줄 요약 (위성군 보정 누락 진단의 핵심) + if (std::chrono::duration(now - last_summary).count() > 30.0) { + log_rtcm_summary(); + last_summary = now; + } + // VRS 유지용 GGA 주기 재전송 + if (send_gga && std::chrono::duration(now - last_send).count() > gga_period_) { std::string g = latest_gga(); if (!g.empty()) ::send(ntrip_fd_, g.data(), g.size(), MSG_NOSIGNAL); last_send = now; @@ -321,6 +547,152 @@ class Um982Driver : public rclcpp::Node { } } + // ── AUTO: 소스테이블 → 최근접 단일기준국 ──────────────────────────────── + // "주변에 있는 진짜 기지국을 우선으로 잡는다" 를 그대로 구현. solution 필드가 0 인 + // STR(=단일기준국)만 후보로 두고, 소스테이블의 위경도로 거리순 정렬한다. + bool resolve_auto_mountpoint() { + double rlat = rover_lat_.load(), rlon = rover_lon_.load(); + if (std::isnan(rlat)) { + RCLCPP_WARN(get_logger(), "AUTO: 로버 위치 미확보 — 기준국 선택 보류"); + return false; + } + std::string table; + if (!fetch_sourcetable(table)) { + RCLCPP_ERROR(get_logger(), "AUTO: 소스테이블 조회 실패"); + return false; + } + std::vector> cand; + std::istringstream ss(table); + std::string line; + while (std::getline(ss, line)) { + if (line.rfind("STR;", 0) != 0) continue; + std::vector f; + std::string tok; + std::istringstream ls(line); + while (std::getline(ls, tok, ';')) f.push_back(tok); + if (f.size() < 13) continue; + if (f[12] != "0") continue; // solution=0 → 단일기준국만 + if (!auto_format_.empty() && f[3] != auto_format_) continue; + double blat, blon; + try { blat = std::stod(f[9]); blon = std::stod(f[10]); } catch (...) { continue; } + if (blat == 0.0 && blon == 0.0) continue; + cand.emplace_back(haversine_km(rlat, rlon, blat, blon), f[1]); + } + if (cand.empty()) { + RCLCPP_ERROR(get_logger(), "AUTO: 조건(solution=0, format=%s)에 맞는 기준국 없음", auto_format_.c_str()); + return false; + } + std::sort(cand.begin(), cand.end()); + candidates_.clear(); + for (size_t i = 0; i < cand.size() && i < 5; ++i) candidates_.push_back(cand[i].second); + cand_idx_ = 0; + active_mp_ = candidates_.front(); + std::string others; + for (size_t i = 1; i < candidates_.size(); ++i) others += " " + candidates_[i]; + RCLCPP_INFO(get_logger(), "AUTO 기준국 선택: %s (%.1f km) | 예비:%s", + active_mp_.c_str(), cand[0].first, others.empty() ? " 없음" : others.c_str()); + RCLCPP_INFO(get_logger(), " ※ 소스테이블 좌표는 반올림/오기입이 있다. 실제 거리는 아래 1005/1006 로그로 확인할 것"); + return true; + } + + bool next_candidate() { + if (candidates_.empty() || cand_idx_ + 1 >= candidates_.size()) { active_mp_.clear(); return false; } + active_mp_ = candidates_[++cand_idx_]; + RCLCPP_WARN(get_logger(), "AUTO: 다음 후보로 전환 → %s", active_mp_.c_str()); + return true; + } + + bool fetch_sourcetable(std::string &out) { + addrinfo hints{}, *res = nullptr; + hints.ai_family = AF_INET; hints.ai_socktype = SOCK_STREAM; + if (getaddrinfo(ntrip_host_.c_str(), std::to_string(ntrip_port_).c_str(), &hints, &res) != 0) return false; + int fd = ::socket(res->ai_family, res->ai_socktype, res->ai_protocol); + timeval tv{10, 0}; + setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); + bool ok = (::connect(fd, res->ai_addr, res->ai_addrlen) == 0); + freeaddrinfo(res); + if (!ok) { ::close(fd); return false; } + // 소스테이블 조회는 인증 불필요 + std::string req = "GET / HTTP/1.0\r\nUser-Agent: NTRIP um982_driver/1.0\r\n" + "Accept: */*\r\nConnection: close\r\n\r\n"; + ::send(fd, req.data(), req.size(), MSG_NOSIGNAL); + out.clear(); + char buf[4096]; + ssize_t r; + while ((r = ::recv(fd, buf, sizeof(buf), 0)) > 0) { + out.append(buf, static_cast(r)); + if (out.size() > (1u << 22)) break; // 4MB 안전장치 + } + ::close(fd); + return out.find("STR;") != std::string::npos; + } + + // ── RTCM 스니핑 ───────────────────────────────────────────────────────── + void feed_rtcm(const uint8_t *data, size_t n) { + rtcm_buf_.insert(rtcm_buf_.end(), data, data + n); + extract_rtcm_frames(rtcm_buf_, [this](int mt, const uint8_t *pl, size_t len) { + ++rtcm_types_[mt]; + if (mt == 1005 || mt == 1006) { + BasePos bp; + if (parse_base_pos(pl, len, bp)) note_base(bp); + } + }); + if (rtcm_buf_.size() > (1u << 16)) rtcm_buf_.clear(); // 안전장치 + } + + void note_base(const BasePos &bp) { + bool moved = base_known_ && (std::fabs(bp.x - base_x_) > 0.5 || + std::fabs(bp.y - base_y_) > 0.5 || + std::fabs(bp.z - base_z_) > 0.5); + if (base_known_ && !moved) return; + double dx = bp.x - base_x_, dy = bp.y - base_y_, dz = bp.z - base_z_; + double move_m = moved ? std::sqrt(dx * dx + dy * dy + dz * dz) : 0.0; + base_x_ = bp.x; base_y_ = bp.y; base_z_ = bp.z; base_known_ = true; + + double blat, blon, balt; + ecef_to_lla(bp.x, bp.y, bp.z, blat, blon, balt); + double rlat = rover_lat_.load(), rlon = rover_lon_.load(); + double km = std::isnan(rlat) ? -1.0 : haversine_km(rlat, rlon, blat, blon); + + if (moved) { + // 실제 관측소는 움직이지 않는다. 이게 뜨면 가상 기지국(VRS)이다. + RCLCPP_WARN(get_logger(), "⚠️ 기지국 좌표 이동 %.1fm → 가상 기지국(VRS) 스트림. id=%d", move_m, bp.id); + } + if (km >= 0.0) { + RCLCPP_INFO(get_logger(), "기지국 id=%d %.7f, %.7f, %.2fm%s | 베이스라인 %.2f km", + bp.id, blat, blon, balt, bp.has_ant_h ? " (안테나고 포함)" : "", km); + if (km > max_baseline_km_) + RCLCPP_WARN(get_logger(), "⚠️ 베이스라인 %.1f km > %.1f km — 단일기준국은 거리가 멀수록 Fixed 가 어려워진다", + km, max_baseline_km_); + } else { + RCLCPP_INFO(get_logger(), "기지국 id=%d %.7f, %.7f, %.2fm", bp.id, blat, blon, balt); + } + } + + // 30초마다: 어떤 RTCM 이 오는지 + 수신기가 쓸 수 있는 형태인지 + void log_rtcm_summary() { + if (rtcm_types_.empty()) { + RCLCPP_WARN(get_logger(), "RTCM 미수신 (누적 %ld bytes)", ntrip_bytes_total_); + return; + } + std::ostringstream all, net, undoc, sats; + for (const auto &kv : rtcm_types_) { + all << " " << kv.first << "x" << kv.second; + if (const char *nm = network_msg_name(kv.first)) net << " " << kv.first << "(" << nm << ")"; + if (const char *c = msm_constellation(kv.first)) { + sats << " " << c; + if (!rtcm_documented(kv.first)) undoc << " " << kv.first << "(" << c << ")"; + } + } + RCLCPP_INFO(get_logger(), "RTCM 누적 %ld bytes | 메시지:%s", ntrip_bytes_total_, all.str().c_str()); + if (!sats.str().empty()) + RCLCPP_INFO(get_logger(), " 보정 오는 위성군:%s", sats.str().c_str()); + if (!net.str().empty()) + RCLCPP_WARN(get_logger(), " 네트워크 RTK 메시지 존재 → 단일기준국 아님:%s", net.str().c_str()); + if (!undoc.str().empty()) + RCLCPP_WARN(get_logger(), " ⚠️ 매뉴얼 미기재 메시지:%s — 이 위성군은 RTK 에 안 쓰일 수 있다", undoc.str().c_str()); + } + bool ntrip_connect() { addrinfo hints{}, *res = nullptr; hints.ai_family = AF_INET; hints.ai_socktype = SOCK_STREAM; @@ -333,16 +705,29 @@ class Um982Driver : public rclcpp::Node { if (!ok) { ::close(fd); return false; } std::string auth = base64(ntrip_user_ + ":" + ntrip_pass_); - std::string req = "GET /" + ntrip_mp_ + " HTTP/1.0\r\n" + std::string req = "GET /" + active_mp_ + " HTTP/1.0\r\n" "User-Agent: NTRIP um982_driver/1.0\r\nAccept: */*\r\n" "Connection: close\r\nAuthorization: Basic " + auth + "\r\n\r\n"; ::send(fd, req.data(), req.size(), MSG_NOSIGNAL); char resp[1024]; ssize_t r = ::recv(fd, resp, sizeof(resp) - 1, 0); if (r <= 0) { ::close(fd); return false; } resp[r] = 0; - if (!strstr(resp, "200 OK") && !strstr(resp, "ICY 200 OK")) { ::close(fd); return false; } - std::string g = latest_gga(); - if (!g.empty()) ::send(fd, g.data(), g.size(), MSG_NOSIGNAL); + if (!strstr(resp, "200 OK") && !strstr(resp, "ICY 200 OK")) { + // 401(계정 문제)과 404(마운트포인트 오타/미개방)를 구분해줘야 삽질이 없다 + std::string head(resp, std::min(static_cast(r), 120)); + size_t eol = head.find('\r'); + RCLCPP_ERROR(get_logger(), "NTRIP 거부 [%s] ← %s:%d/%s", + head.substr(0, eol == std::string::npos ? head.size() : eol).c_str(), + ntrip_host_.c_str(), ntrip_port_, active_mp_.c_str()); + ::close(fd); + return false; + } + // 단일기준국(nmea=0)은 GGA 를 안 보내도 스트림이 나온다 + if (gga_period_ > 0.0) { + std::string g = latest_gga(); + if (!g.empty()) ::send(fd, g.data(), g.size(), MSG_NOSIGNAL); + } + rtcm_buf_.clear(); ntrip_fd_ = fd; return true; } @@ -351,9 +736,10 @@ class Um982Driver : public rclcpp::Node { // 파라미터 std::string serial_port_, gps_topic_, ntrip_host_, ntrip_user_, ntrip_pass_, ntrip_mp_; + std::string auto_format_; int serial_baud_{}, ntrip_port_{}; bool use_ntrip_{}, fixed_only_{}, inflate_cov_{}; - double gga_period_{}, max_corr_age_{}; + double gga_period_{}, max_corr_age_{}, max_baseline_km_{}; rclcpp::Publisher::SharedPtr pub_; int serial_fd_{-1}; @@ -364,6 +750,19 @@ class Um982Driver : public rclcpp::Node { std::mutex gga_mtx_; std::string latest_gga_; int32_t last_type_{-999}; + + // 로버 위치 — AUTO 기준국 선택과 베이스라인 계산용 (시리얼 스레드가 쓰고 NTRIP 스레드가 읽음) + std::atomic rover_lat_{std::numeric_limits::quiet_NaN()}; + std::atomic rover_lon_{std::numeric_limits::quiet_NaN()}; + + // NTRIP 스레드 전용 (락 불필요) + std::string active_mp_; // 실제로 접속 중인 마운트포인트 + std::vector candidates_; // AUTO 후보(거리순) + size_t cand_idx_{0}; + std::vector rtcm_buf_; + std::map rtcm_types_; + bool base_known_{false}; + double base_x_{0}, base_y_{0}, base_z_{0}; }; int main(int argc, char **argv) { diff --git a/tools/ntrip_probe.py b/tools/ntrip_probe.py new file mode 100644 index 0000000..373e130 --- /dev/null +++ b/tools/ntrip_probe.py @@ -0,0 +1,400 @@ +#!/usr/bin/env python3 +"""NTRIP 캐스터 조사/검증 도구 — 마운트포인트가 '진짜 단일기준국'인지 판별한다. + +VRS(가상 기지국)와 단일기준국(실제 상시관측소)은 같은 RTCM3 스트림으로 오지만 +메시지 구성이 다르다. 이 도구는 스트림을 직접 뜯어서 구분한다. + + table 캐스터 소스테이블 출력 (인증 불필요) + near 내 좌표에서 가까운 기준국 마운트포인트 정렬 + sniff 마운트포인트에 접속해 RTCM 메시지 종류/기지국 좌표/베이스라인을 관찰 + → 기지국 좌표가 시간에 따라 움직이면 VRS, 고정이면 실제 기지국 + +사용 예: + python3 ntrip_probe.py table gnssdata.or.kr 2101 + python3 ntrip_probe.py near gnssdata.or.kr 2101 37.5665 126.9780 + python3 ntrip_probe.py sniff RTS1.ngii.go.kr 2101 VRS-RTCM34 \ + --user ID --pass PW --gga-from-latlon 37.5665 126.9780 --secs 60 +""" + +import argparse +import base64 +import math +import socket +import sys +import time +from collections import Counter + +# ── RTCM3 프레이밍 ────────────────────────────────────────────────────────── +# 0xD3 | 6bit reserved + 10bit length | payload | 3byte CRC-24Q + +_CRC24_TAB = [] + + +def _init_crc24(): + poly = 0x1864CFB + for i in range(256): + crc = i << 16 + for _ in range(8): + crc <<= 1 + if crc & 0x1000000: + crc ^= poly + _CRC24_TAB.append(crc & 0xFFFFFF) + + +_init_crc24() + + +def crc24q(data: bytes) -> int: + crc = 0 + for b in data: + crc = ((crc << 8) & 0xFFFFFF) ^ _CRC24_TAB[((crc >> 16) ^ b) & 0xFF] + return crc + + +class Bits: + """MSB-first 비트 리더 (RTCM DF 필드용).""" + + def __init__(self, buf: bytes): + self.buf = buf + self.pos = 0 + + def u(self, n: int) -> int: + v = 0 + for _ in range(n): + byte = self.buf[self.pos >> 3] + v = (v << 1) | ((byte >> (7 - (self.pos & 7))) & 1) + self.pos += 1 + return v + + def s(self, n: int) -> int: + v = self.u(n) + return v - (1 << n) if v & (1 << (n - 1)) else v + + +def parse_1005_1006(payload: bytes): + """1005/1006 → (station_id, X, Y, Z, antenna_height|None). 단위 m.""" + b = Bits(payload) + msg = b.u(12) + if msg not in (1005, 1006): + return None + sid = b.u(12) + b.u(6) # ITRF realization year + b.u(1); b.u(1); b.u(1) # GPS / GLONASS / Galileo indicator + b.u(1) # reference station indicator + x = b.s(38) * 1e-4 + b.u(1); b.u(1) # single receiver osc / reserved + y = b.s(38) * 1e-4 + b.u(2) # quarter cycle indicator + z = b.s(38) * 1e-4 + h = b.u(16) * 1e-4 if msg == 1006 else None + return sid, x, y, z, h + + +def ecef_to_lla(x, y, z): + a, f = 6378137.0, 1 / 298.257223563 + e2 = f * (2 - f) + lon = math.atan2(y, x) + p = math.hypot(x, y) + lat = math.atan2(z, p * (1 - e2)) + for _ in range(10): + n = a / math.sqrt(1 - e2 * math.sin(lat) ** 2) + alt = p / math.cos(lat) - n + lat = math.atan2(z, p * (1 - e2 * n / (n + alt))) + n = a / math.sqrt(1 - e2 * math.sin(lat) ** 2) + return math.degrees(lat), math.degrees(lon), p / math.cos(lat) - n + + +def haversine_km(lat1, lon1, lat2, lon2): + r = 6371.0 + dlat = math.radians(lat2 - lat1) + dlon = math.radians(lon2 - lon1) + h = (math.sin(dlat / 2) ** 2 + + math.cos(math.radians(lat1)) * math.cos(math.radians(lat2)) * math.sin(dlon / 2) ** 2) + return 2 * r * math.asin(math.sqrt(h)) + + +# 네트워크 RTK(=VRS/FKP/MAC) 임을 드러내는 메시지들 +NETWORK_MSGS = { + 1014: "Network Auxiliary Station Data (MAC)", + 1015: "GPS Ionospheric Correction Differences (MAC)", + 1016: "GPS Geometric Correction Differences (MAC)", + 1017: "GPS Combined Correction Differences (MAC)", + 1030: "GPS Network RTK Residual", + 1031: "GLONASS Network RTK Residual", + 1032: "Physical Reference Station Position (VRS)", + 1034: "GPS Network FKP Gradient", + 1035: "GLONASS Network FKP Gradient", +} + + +# ── NTRIP ─────────────────────────────────────────────────────────────────── +def ntrip_open(host, port, mount=None, user="", pw="", timeout=15): + s = socket.create_connection((host, port), timeout=timeout) + path = "/" + (mount or "") + req = f"GET {path} HTTP/1.0\r\nUser-Agent: NTRIP ntrip_probe/1.0\r\nAccept: */*\r\n" + if user or pw: + auth = base64.b64encode(f"{user}:{pw}".encode()).decode() + req += f"Authorization: Basic {auth}\r\n" + req += "Connection: close\r\n\r\n" + s.sendall(req.encode()) + return s + + +def read_header(s): + """응답 헤더를 소진하고 (상태줄, 남은 바이트) 반환.""" + buf = b"" + while b"\r\n\r\n" not in buf and b"\n\n" not in buf: + chunk = s.recv(4096) + if not chunk: + break + buf += chunk + if buf.startswith(b"ICY 200 OK") and len(buf) > 12: # NTRIP1 짧은 응답 + break + if len(buf) > 65536: + break + sep = b"\r\n\r\n" if b"\r\n\r\n" in buf else b"\n\n" + head, _, rest = buf.partition(sep) + status = head.split(b"\r\n")[0].decode("latin1", "replace") + return status, head, rest + + +def fetch_sourcetable(host, port): + s = ntrip_open(host, port) + data = b"" + try: + while True: + chunk = s.recv(8192) + if not chunk: + break + data += chunk + finally: + s.close() + return data.decode("latin1", "replace") + + +def parse_str_rows(text, host=""): + # 캐스터가 짧은 간격의 재요청을 조용히 끊는 일이 있다 — 빈 결과를 정상으로 오인하면 안 된다. + if "STR;" not in text: + print(f"# ⚠️ 소스테이블 응답에 STR 항목이 없음 ({len(text)} bytes)" + + (f" — {host} 재시도해볼 것" if host else ""), file=sys.stderr) + if text[:200].strip(): + print("# 응답 앞부분: " + repr(text[:200]), file=sys.stderr) + rows = [] + for line in text.splitlines(): + if not line.startswith("STR;"): + continue + f = line.split(";") + if len(f) < 13: + continue + try: + lat, lon = float(f[9]), float(f[10]) + except ValueError: + lat = lon = None + rows.append({ + "mount": f[1], "format": f[3], "details": f[4], "nav": f[6], + "network": f[7], "lat": lat, "lon": lon, + "nmea": f[11], "solution": f[12], + "generator": f[13] if len(f) > 13 else "", + "auth": f[15] if len(f) > 15 else "", + }) + return rows + + +def nmea_gga(lat, lon, alt=50.0): + def dm(v, deg_w): + d = int(abs(v)) + m = (abs(v) - d) * 60 + return f"{d:0{deg_w}d}{m:09.6f}" + t = time.gmtime() + body = (f"GPGGA,{t.tm_hour:02d}{t.tm_min:02d}{t.tm_sec:02d}.00," + f"{dm(lat,2)},{'N' if lat>=0 else 'S'}," + f"{dm(lon,3)},{'E' if lon>=0 else 'W'}," + f"1,10,1.0,{alt:.1f},M,20.0,M,,") + ck = 0 + for c in body: + ck ^= ord(c) + return f"${body}*{ck:02X}\r\n".encode() + + +# ── 서브커맨드 ────────────────────────────────────────────────────────────── +def cmd_table(a): + text = fetch_sourcetable(a.host, a.port) + rows = parse_str_rows(text, a.host) + print(f"# {a.host}:{a.port} — STR {len(rows)}개") + print(f"{'MOUNTPOINT':<18} {'FORMAT':<9} {'NAV':<34} {'NET':<12} {'NMEA':<5} {'SOL':<4} {'종류'}") + for r in rows: + if a.filter and a.filter.upper() not in r["mount"].upper(): + continue + kind = "네트워크(VRS/FKP/MAC)" if r["solution"] == "1" else "단일기준국" + print(f"{r['mount']:<18} {r['format']:<9} {r['nav'][:34]:<34} " + f"{r['network'][:12]:<12} {r['nmea']:<5} {r['solution']:<4} {kind}") + net = sum(1 for r in rows if r["solution"] == "1") + print(f"\n요약: 단일기준국 {len(rows)-net}개 / 네트워크 {net}개") + print(" · SOL=0 → 실제 기준국 1곳의 관측치(단일기준국)") + print(" · SOL=1 → 망 보간 해(VRS/FKP/MAC). NMEA=1 이면 GGA 업링크 필요") + + +def cmd_near(a): + rows = parse_str_rows(fetch_sourcetable(a.host, a.port), a.host) + cand = [] + for r in rows: + if r["lat"] is None or (r["lat"] == 0 and r["lon"] == 0): + continue + if a.format and a.format.upper() not in r["format"].upper().replace(" ", ""): + continue + cand.append((haversine_km(a.lat, a.lon, r["lat"], r["lon"]), r)) + cand.sort(key=lambda t: t[0]) + print(f"# 기준 위치 {a.lat:.5f}, {a.lon:.5f} — 가까운 순 {a.n}개" + + (f" (포맷 필터: {a.format})" if a.format else "")) + print(f"{'거리km':>8} {'MOUNTPOINT':<18} {'FORMAT':<9} {'NAV':<34} {'SOL'}") + for d, r in cand[:a.n]: + print(f"{d:8.1f} {r['mount']:<18} {r['format']:<9} {r['nav'][:34]:<34} {r['solution']}") + print("\n주의: 소스테이블 좌표는 반올림/오기입이 흔하다(0.1도 = 약 11km).") + print(" 최종 확인은 sniff 로 1005/1006 기지국 실좌표를 봐야 한다.") + + +def cmd_sniff(a): + s = ntrip_open(a.host, a.port, a.mount, a.user, getattr(a, "pw")) + status, head, rest = read_header(s) + print(f"# 접속: {a.host}:{a.port}/{a.mount}") + print(f"# 응답: {status}") + if "200" not in status: + print(head.decode("latin1", "replace")) + return 1 + + gga = None + if a.gga_from_latlon: + gga = nmea_gga(a.gga_from_latlon[0], a.gga_from_latlon[1]) + s.sendall(gga) + print(f"# GGA 업링크: {gga.decode().strip()}") + + s.settimeout(5) + buf = bytearray(rest) + types = Counter() + bases = {} # station_id -> 마지막 좌표 + base_moves = [] # (t, sid, 이동거리 m) + total = 0 + t0 = time.time() + last_gga = t0 + + while time.time() - t0 < a.secs: + try: + chunk = s.recv(4096) + except socket.timeout: + print(" (수신 없음 5초)") + continue + if not chunk: + print("# 스트림 종료") + break + total += len(chunk) + buf += chunk + + i = 0 + while True: + j = buf.find(b"\xd3", i) + if j < 0 or len(buf) - j < 3: + break + # 길이 상위 바이트의 reserved 6bit 는 항상 0. 이걸 안 보면 페이로드 안의 + # 우연한 0xD3 을 프레임 시작으로 착각해 최대 1023B 동안 파서가 멈춘다. + if buf[j + 1] & 0xFC: + i = j + 1 + continue + length = ((buf[j + 1] & 0x03) << 8) | buf[j + 2] + frame_len = 3 + length + 3 + if len(buf) - j < frame_len: + break + frame = bytes(buf[j:j + frame_len]) + got = int.from_bytes(frame[-3:], "big") + if crc24q(frame[:-3]) != got: + i = j + 1 # 오정렬 → 다음 0xD3 부터 + continue + payload = frame[3:3 + length] + if length >= 2: + mt = (payload[0] << 4) | (payload[1] >> 4) + types[mt] += 1 + if mt in (1005, 1006): + p = parse_1005_1006(payload) + if p: + sid, x, y, z, h = p + prev = bases.get(sid) + if prev and (abs(prev[0] - x) > 0.5 or abs(prev[1] - y) > 0.5 + or abs(prev[2] - z) > 0.5): + d = math.dist(prev[:3], (x, y, z)) + base_moves.append((time.time() - t0, sid, d)) + print(f" ⚠️ 기지국 좌표 이동: id={sid} {d:.1f} m (t+{time.time()-t0:.0f}s)") + bases[sid] = (x, y, z, h) + i = j + frame_len + del buf[:i] + + if gga and time.time() - last_gga > 10: + s.sendall(gga) + last_gga = time.time() + + s.close() + + dur = time.time() - t0 + print(f"\n## 결과 ({dur:.0f}초, {total} bytes, {total/max(dur,1):.0f} B/s)") + print("\n### RTCM 메시지 종류") + for mt, c in sorted(types.items()): + tag = f" ← {NETWORK_MSGS[mt]}" if mt in NETWORK_MSGS else "" + print(f" {mt:<5} x{c:<6}{tag}") + + print("\n### 기지국(1005/1006) 좌표") + if not bases: + print(" 없음 — 1005/1006 이 안 왔다. 관측 시간을 늘려보라(권장 60초 이상).") + for sid, (x, y, z, h) in bases.items(): + lat, lon, alt = ecef_to_lla(x, y, z) + line = f" id={sid} {lat:.7f}, {lon:.7f}, {alt:.2f}m" + if h is not None: + line += f" (안테나고 {h:.3f}m)" + print(line) + if a.gga_from_latlon: + d = haversine_km(a.gga_from_latlon[0], a.gga_from_latlon[1], lat, lon) + print(f" 베이스라인 {d:.2f} km") + + print("\n### 판정") + net_found = sorted(set(types) & set(NETWORK_MSGS)) + if net_found: + print(f" ❌ 네트워크 RTK 스트림 — 네트워크 전용 메시지 {net_found} 존재") + elif base_moves: + print(f" ❌ 기지국 좌표가 {len(base_moves)}회 이동 — VRS(가상 기지국)") + elif bases: + print(" ✅ 단일기준국으로 보임 — 네트워크 메시지 없음 + 기지국 좌표 고정") + else: + print(" ? 판정 불가 — 1005/1006 미수신") + print("\n ※ VRS 는 GGA 를 보낸 위치 근처에 가상 기지국을 만든다. 정지 상태로 짧게 보면") + print(" 좌표가 고정으로 보일 수 있으므로, 네트워크 메시지 유무를 1차 근거로 삼을 것.") + return 0 + + +def main(): + ap = argparse.ArgumentParser(description=__doc__, + formatter_class=argparse.RawDescriptionHelpFormatter) + sub = ap.add_subparsers(dest="cmd", required=True) + + p = sub.add_parser("table", help="소스테이블 출력") + p.add_argument("host"); p.add_argument("port", type=int) + p.add_argument("--filter", help="마운트포인트 이름 부분일치 필터") + p.set_defaults(func=cmd_table) + + p = sub.add_parser("near", help="내 위치에서 가까운 기준국 정렬") + p.add_argument("host"); p.add_argument("port", type=int) + p.add_argument("lat", type=float); p.add_argument("lon", type=float) + p.add_argument("-n", type=int, default=10) + p.add_argument("--format", help="포맷 필터 (예: RTCM3.2)") + p.set_defaults(func=cmd_near) + + p = sub.add_parser("sniff", help="스트림을 뜯어 VRS/단일기준국 판정") + p.add_argument("host"); p.add_argument("port", type=int); p.add_argument("mount") + p.add_argument("--user", default=""); p.add_argument("--pass", dest="pw", default="") + p.add_argument("--gga-from-latlon", nargs=2, type=float, metavar=("LAT", "LON"), + help="GGA 업링크 (VRS/nmea=1 마운트포인트에 필요)") + p.add_argument("--secs", type=int, default=60) + p.set_defaults(func=cmd_sniff) + + a = ap.parse_args() + sys.exit(a.func(a) or 0) + + +if __name__ == "__main__": + main()