azmo/fhd_rtk_ws
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

upload

Browse Source
This commit is contained in:
Dongubak
2026-08-22 02:54:26 +09:00
parent be9e83d2f4
commit 11fa444ce7
6 changed files with 1281 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/NTRIP-단일기준국-전환-조사.md
+238
View File
@@ -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 기준 `<MsgID>,<MsgNum>,<BaseID>,<SatsNum>,<L1~L6>`.
**③과 ④를 대조하면 "캐스터는 보냈는데 수신기가 안 쓴" 위성군이 그대로 드러난다** — §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 <ID> --pass <PW> --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 <PW> --secs 60
```
`401` 이면 별도 가입 필요. 그동안의 대안으로 RTS1 `RTK-RTCM32` 정체도 확인:
```bash
python3 tools/ntrip_probe.py sniff RTS1.ngii.go.kr 2101 RTK-RTCM32 \
--user hyuk6578 --pass <PW> --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 |
src/um982_driver/README.md
+15 -3
View File
@@ -12,11 +12,16 @@ UM982(Unicore) → `gnss_comm/msg/GnssPVTSolnMsg` 드라이버 노드. FAST-LIVO
같은 노드 안에 있다: 같은 노드 안에 있다:
``` ```
NTRIP(VRS) ──RTCM3──▶ UM982 ──BESTNAV(binary)+GGA(ascii)──▶ um982_driver ──GnssPVTSolnMsg──▶ 백엔드 NTRIP(단일기준국) ──RTCM3──▶ UM982 ──BESTNAV(binary)+GGA(ascii)──▶ um982_driver ──GnssPVTSolnMsg──▶ 백엔드
▲ │ ▲ │
└────────────────────── GGA(VRS 유지) ───────────────────┘ └──────────── GGA (VRS 쓸 때만; 단일기준국은 불필요) ──────────┘
``` ```
> VRS(가상 기지국) 대신 실제 상시관측소를 쓴다. 배경·근거는
> [`docs/NTRIP-단일기준국-전환-조사.md`](../../docs/NTRIP-단일기준국-전환-조사.md).
> 드라이버가 주입 중인 RTCM 을 스니핑해 기지국 좌표·베이스라인·위성군 구성을 로깅하므로,
> 붙은 곳이 진짜 단일기준국인지 로그로 바로 확인된다.
## 수신기 사전 설정 ## 수신기 사전 설정
드라이버 실행 전에 UM982 가 아래를 출력하도록 저장돼 있어야 한다(리포 루트 `rtk/config_all.py`): 드라이버 실행 전에 UM982 가 아래를 출력하도록 저장돼 있어야 한다(리포 루트 `rtk/config_all.py`):
``` ```
@@ -55,7 +60,14 @@ ros2 run um982_driver um982_driver_node --ros-args \
## 파라미터 (config/um982.yaml) ## 파라미터 (config/um982.yaml)
- `serial_port`, `serial_baud`(460800), `gps_topic` - `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) - `publish_fixed_only`(기본 false), `inflate_cov_when_not_fixed`(기본 true)
> 백엔드는 fix 품질을 거르지 않으므로, RTK Fixed 가 아닌 해는 `inflate_cov_when_not_fixed` > 백엔드는 fix 품질을 거르지 않으므로, RTK Fixed 가 아닌 해는 `inflate_cov_when_not_fixed`
src/um982_driver/RTK-녹화-명세.md
+200
View File
@@ -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:=<id> -p ntrip_pass:=<pw>
```
콘솔에 `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 에 존재
```
src/um982_driver/config/um982.yaml
+13 -4
View File
@@ -7,14 +7,23 @@ um982_driver:
# ── 발행 토픽 (백엔드 gps.gps_topic 와 일치) ── # ── 발행 토픽 (백엔드 gps.gps_topic 와 일치) ──
gps_topic: "/ublox_driver/receiver_pvt" 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 use_ntrip: true
ntrip_host: "RTS1.ngii.go.kr" ntrip_host: "gnssdata.or.kr"
ntrip_port: 2101 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_user: "" # ⚠️ 여기에 채우거나 launch 인자/환경변수로 주입(리포에 평문 저장 지양)
ntrip_pass: "" 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 품질 필터가 없음) ── # ── 품질 처리 (백엔드에 fix 품질 필터가 없음) ──
publish_fixed_only: false # true = RTK Fixed(carr_soln=2) 해만 발행 publish_fixed_only: false # true = RTK Fixed(carr_soln=2) 해만 발행
src/um982_driver/src/um982_driver_node.cpp
+410 -11
View File
@@ -24,10 +24,14 @@
#include <arpa/inet.h> #include <arpa/inet.h>
#include <algorithm> #include <algorithm>
#include <array>
#include <atomic> #include <atomic>
#include <cmath> #include <cmath>
#include <cstring> #include <cstring>
#include <limits>
#include <map>
#include <mutex> #include <mutex>
#include <sstream>
#include <string> #include <string>
#include <thread> #include <thread>
#include <vector> #include <vector>
@@ -75,6 +79,163 @@ uint32_t block_crc32(const uint8_t *buf, size_t n) {
return crc; return crc;
} }
// ── RTCM3 스니핑 (주입 스트림 검증) ─────────────────────────────────────────
// 프레임: 0xD3 | 6bit reserved + 10bit len | payload | CRC-24Q 3B
// 목적 두 가지:
// (1) 붙은 마운트포인트가 실제 단일기준국인지 — 1005/1006 좌표 고정 + 네트워크 메시지 부재
// (2) UM982 가 실제로 쓸 수 있는 위성군 보정이 오는지 (아래 rtcm_documented 참고)
const std::array<uint32_t, 256> &crc24q_table() {
static const std::array<uint32_t, 256> t = [] {
std::array<uint32_t, 256> a{};
for (int i = 0; i < 256; ++i) {
uint32_t crc = static_cast<uint32_t>(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<int64_t>(v) - (1LL << n) : static_cast<int64_t>(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<int>(b.u(12));
if (msg != 1005 && msg != 1006) return false;
if (msg == 1006 && len < 21) return false;
out.id = static_cast<int>(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<double>(b.s(38)) * 1e-4;
b.u(1); b.u(1); // single receiver osc, reserved
out.y = static_cast<double>(b.s(38)) * 1e-4;
b.u(2); // quarter cycle indicator
out.z = static_cast<double>(b.s(38)) * 1e-4;
out.has_ant_h = (msg == 1006);
out.ant_h = out.has_ant_h ? static_cast<double>(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 <typename F>
void extract_rtcm_frames(std::vector<uint8_t> &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<size_t>(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<uint32_t>(f[total - 3]) << 16)
| (static_cast<uint32_t>(f[total - 2]) << 8)
| static_cast<uint32_t>(f[total - 1]);
if (crc24q(f, total - 3) != got) { i = j + 1; continue; } // 오정렬 → 다음 0xD3 부터
if (len >= 2) on_frame((static_cast<int>(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) { speed_t to_speed(int baud) {
switch (baud) { switch (baud) {
case 9600: return B9600; case 19200: return B19200; case 9600: return B9600; case 19200: return B19200;
@@ -117,6 +278,12 @@ class Um982Driver : public rclcpp::Node {
inflate_cov_ = declare_parameter<bool>("inflate_cov_when_not_fixed", true); inflate_cov_ = declare_parameter<bool>("inflate_cov_when_not_fixed", true);
max_corr_age_ = declare_parameter<double>("max_corr_age", 30.0); // 보정 나이 초과 시 코스팅으로 간주 max_corr_age_ = declare_parameter<double>("max_corr_age", 30.0); // 보정 나이 초과 시 코스팅으로 간주
gga_period_ = declare_parameter<double>("gga_send_period", 10.0); gga_period_ = declare_parameter<double>("gga_send_period", 10.0);
// ntrip_mountpoint 가 "AUTO" 면 소스테이블을 받아 현재 위치에서 가장 가까운
// 단일기준국(solution=0)을 고른다. 아래는 그 선택 기준.
auto_format_ = declare_parameter<std::string>("ntrip_auto_format", "RTCM 3.2");
max_baseline_km_ = declare_parameter<double>("max_baseline_km", 30.0); // 초과 시 경고만
if (ntrip_mp_ != "AUTO" && ntrip_mp_ != "auto") active_mp_ = ntrip_mp_;
pub_ = create_publisher<GnssPVTSolnMsg>(gps_topic_, rclcpp::QoS(rclcpp::KeepLast(2000))); pub_ = create_publisher<GnssPVTSolnMsg>(gps_topic_, rclcpp::QoS(rclcpp::KeepLast(2000)));
@@ -127,6 +294,13 @@ class Um982Driver : public rclcpp::Node {
RCLCPP_INFO(get_logger(), "UM982 %s @ %d → 발행 %s", RCLCPP_INFO(get_logger(), "UM982 %s @ %d → 발행 %s",
serial_port_.c_str(), serial_baud_, gps_topic_.c_str()); serial_port_.c_str(), serial_baud_, gps_topic_.c_str());
// RAM 에만 적용되는 로그 활성화(SAVECONFIG 안 하므로 재부팅 시 원복). 수신기가 어떤 RTCM 을
// 실제로 받아들이는지 보려면 이게 켜져 있어야 한다.
if (declare_parameter<bool>("enable_rtcm_status_log", true)) {
static constexpr char kCmd[] = "RTCMSTATUSA ONCHANGED\r\n";
::write(serial_fd_, kCmd, sizeof(kCmd) - 1);
}
running_ = true; running_ = true;
serial_thread_ = std::thread(&Um982Driver::serial_loop, this); serial_thread_ = std::thread(&Um982Driver::serial_loop, this);
if (use_ntrip_) ntrip_thread_ = std::thread(&Um982Driver::ntrip_loop, this); if (use_ntrip_) ntrip_thread_ = std::thread(&Um982Driver::ntrip_loop, this);
@@ -179,6 +353,7 @@ class Um982Driver : public rclcpp::Node {
// 동기 이전(head)에서 GGA 추출 // 동기 이전(head)에서 GGA 추출
size_t head_end = (sync == std::string::npos) ? buf.size() : sync; size_t head_end = (sync == std::string::npos) ? buf.size() : sync;
scan_gga(buf, search, head_end); scan_gga(buf, search, head_end);
scan_rtcm_status(buf, search, head_end);
if (sync == std::string::npos) { if (sync == std::string::npos) {
if (buf.size() > 2) buf.erase(buf.begin(), buf.end() - 2); // 동기 경계 보존 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,...;<MsgID>,<MsgNum>,<BaseID>,<SatsNum>,<L1..L6>*<CRC>
void scan_rtcm_status(const std::vector<uint8_t> &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<const char *>(&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) { void handle_bestnav(const uint8_t *f) {
int32_t pos_type = rd<int32_t>(f, OFF_POSTYPE); int32_t pos_type = rd<int32_t>(f, OFF_POSTYPE);
if (fixed_only_ && !is_rtk_fixed(pos_type)) return; if (fixed_only_ && !is_rtk_fixed(pos_type)) return;
@@ -242,6 +437,7 @@ class Um982Driver : public rclcpp::Node {
float undu = rd<float>(f, OFF_UNDU); float undu = rd<float>(f, OFF_UNDU);
m.latitude = lat; m.latitude = lat;
m.longitude = lon; m.longitude = lon;
if (pos_type != POS_NONE) { rover_lat_.store(lat); rover_lon_.store(lon); }
m.altitude = hgt + undu; // 타원체고 (백엔드 ENU 입력) m.altitude = hgt + undu; // 타원체고 (백엔드 ENU 입력)
m.height_msl = hgt; m.height_msl = hgt;
@@ -288,15 +484,42 @@ class Um982Driver : public rclcpp::Node {
// ── NTRIP ─────────────────────────────────────────────────────────────── // ── NTRIP ───────────────────────────────────────────────────────────────
void ntrip_loop() { void ntrip_loop() {
// 첫 유효 GGA 대기(VRS 부트스트랩) 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()) while (running_ && rclcpp::ok() && latest_gga().empty())
std::this_thread::sleep_for(std::chrono::milliseconds(200)); 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_send = std::chrono::steady_clock::now();
auto last_summary = std::chrono::steady_clock::now();
while (running_ && rclcpp::ok()) { while (running_ && rclcpp::ok()) {
if (ntrip_fd_ < 0) { if (ntrip_fd_ < 0) {
if (!ntrip_connect()) { std::this_thread::sleep_for(std::chrono::seconds(2)); continue; } if (auto_mp && active_mp_.empty() && !resolve_auto_mountpoint()) {
RCLCPP_INFO(get_logger(), "NTRIP 접속: %s:%d/%s", ntrip_host_.c_str(), ntrip_port_, ntrip_mp_.c_str()); 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(); last_send = std::chrono::steady_clock::now();
} }
uint8_t buf[4096]; uint8_t buf[4096];
@@ -304,16 +527,19 @@ class Um982Driver : public rclcpp::Node {
if (n > 0) { if (n > 0) {
::write(serial_fd_, buf, n); // RTCM 주입 ::write(serial_fd_, buf, n); // RTCM 주입
ntrip_bytes_total_ += n; ntrip_bytes_total_ += n;
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, feed_rtcm(buf, static_cast<size_t>(n)); // 주입과 병행해 스트림 검증
"NTRIP 수신 누적 %ld bytes (최근 recv %zd bytes)",
ntrip_bytes_total_, n);
} else if (n == 0) { } else if (n == 0) {
RCLCPP_WARN(get_logger(), "NTRIP 연결 종료 → 재접속"); RCLCPP_WARN(get_logger(), "NTRIP 연결 종료 → 재접속");
::close(ntrip_fd_); ntrip_fd_ = -1; continue; ::close(ntrip_fd_); ntrip_fd_ = -1; continue;
} }
// VRS 유지용 GGA 주기 재전송
auto now = std::chrono::steady_clock::now(); auto now = std::chrono::steady_clock::now();
if (std::chrono::duration<double>(now - last_send).count() > gga_period_) { // 무엇이 실제로 들어오는지 30초마다 1줄 요약 (위성군 보정 누락 진단의 핵심)
if (std::chrono::duration<double>(now - last_summary).count() > 30.0) {
log_rtcm_summary();
last_summary = now;
}
// VRS 유지용 GGA 주기 재전송
if (send_gga && std::chrono::duration<double>(now - last_send).count() > gga_period_) {
std::string g = latest_gga(); std::string g = latest_gga();
if (!g.empty()) ::send(ntrip_fd_, g.data(), g.size(), MSG_NOSIGNAL); if (!g.empty()) ::send(ntrip_fd_, g.data(), g.size(), MSG_NOSIGNAL);
last_send = now; 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<std::pair<double, std::string>> cand;
std::istringstream ss(table);
std::string line;
while (std::getline(ss, line)) {
if (line.rfind("STR;", 0) != 0) continue;
std::vector<std::string> 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<size_t>(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() { bool ntrip_connect() {
addrinfo hints{}, *res = nullptr; addrinfo hints{}, *res = nullptr;
hints.ai_family = AF_INET; hints.ai_socktype = SOCK_STREAM; 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; } if (!ok) { ::close(fd); return false; }
std::string auth = base64(ntrip_user_ + ":" + ntrip_pass_); 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" "User-Agent: NTRIP um982_driver/1.0\r\nAccept: */*\r\n"
"Connection: close\r\nAuthorization: Basic " + auth + "\r\n\r\n"; "Connection: close\r\nAuthorization: Basic " + auth + "\r\n\r\n";
::send(fd, req.data(), req.size(), MSG_NOSIGNAL); ::send(fd, req.data(), req.size(), MSG_NOSIGNAL);
char resp[1024]; ssize_t r = ::recv(fd, resp, sizeof(resp) - 1, 0); char resp[1024]; ssize_t r = ::recv(fd, resp, sizeof(resp) - 1, 0);
if (r <= 0) { ::close(fd); return false; } if (r <= 0) { ::close(fd); return false; }
resp[r] = 0; resp[r] = 0;
if (!strstr(resp, "200 OK") && !strstr(resp, "ICY 200 OK")) { ::close(fd); return false; } if (!strstr(resp, "200 OK") && !strstr(resp, "ICY 200 OK")) {
// 401(계정 문제)과 404(마운트포인트 오타/미개방)를 구분해줘야 삽질이 없다
std::string head(resp, std::min<size_t>(static_cast<size_t>(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(); std::string g = latest_gga();
if (!g.empty()) ::send(fd, g.data(), g.size(), MSG_NOSIGNAL); if (!g.empty()) ::send(fd, g.data(), g.size(), MSG_NOSIGNAL);
}
rtcm_buf_.clear();
ntrip_fd_ = fd; ntrip_fd_ = fd;
return true; 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 serial_port_, gps_topic_, ntrip_host_, ntrip_user_, ntrip_pass_, ntrip_mp_;
std::string auto_format_;
int serial_baud_{}, ntrip_port_{}; int serial_baud_{}, ntrip_port_{};
bool use_ntrip_{}, fixed_only_{}, inflate_cov_{}; bool use_ntrip_{}, fixed_only_{}, inflate_cov_{};
double gga_period_{}, max_corr_age_{}; double gga_period_{}, max_corr_age_{}, max_baseline_km_{};
rclcpp::Publisher<GnssPVTSolnMsg>::SharedPtr pub_; rclcpp::Publisher<GnssPVTSolnMsg>::SharedPtr pub_;
int serial_fd_{-1}; int serial_fd_{-1};
@@ -364,6 +750,19 @@ class Um982Driver : public rclcpp::Node {
std::mutex gga_mtx_; std::mutex gga_mtx_;
std::string latest_gga_; std::string latest_gga_;
int32_t last_type_{-999}; int32_t last_type_{-999};
// 로버 위치 — AUTO 기준국 선택과 베이스라인 계산용 (시리얼 스레드가 쓰고 NTRIP 스레드가 읽음)
std::atomic<double> rover_lat_{std::numeric_limits<double>::quiet_NaN()};
std::atomic<double> rover_lon_{std::numeric_limits<double>::quiet_NaN()};
// NTRIP 스레드 전용 (락 불필요)
std::string active_mp_; // 실제로 접속 중인 마운트포인트
std::vector<std::string> candidates_; // AUTO 후보(거리순)
size_t cand_idx_{0};
std::vector<uint8_t> rtcm_buf_;
std::map<int, long> rtcm_types_;
bool base_known_{false};
double base_x_{0}, base_y_{0}, base_z_{0};
}; };
int main(int argc, char **argv) { int main(int argc, char **argv) {
tools/ntrip_probe.py
+400
View File
@@ -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()