Skip to content

Commit 1626cd3

Browse files
ptptptpt
authored
improve: handle NMEA GGA/GLL/RMC in blackvue videos (#757)
* improve: handle NMEA GGA/GLL/RMC * tests * tests
1 parent 13aecd1 commit 1626cd3

File tree

3 files changed

+82
-47
lines changed

3 files changed

+82
-47
lines changed

‎mapillary_tools/blackvue_parser.py‎

Lines changed: 45 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -17,11 +17,11 @@
1717
NMEA_LINE_REGEX = re.compile(
1818
rb"""
1919
^\s*
20-
\[(\d+)\] # timestamp
20+
\[(\d+)\] # Timestamp
2121
\s*
22-
(\$\w{5}.*) # nmea line
22+
(\$\w{5}.*) # NMEA message
2323
\s*
24-
(\[\d+\])? # strange timestamp
24+
(\[\d+\])? # Strange timestamp
2525
\s*$
2626
""",
2727
re.X,
@@ -118,8 +118,16 @@ def _parse_gps_box(gps_data: bytes) -> T.Generator[geo.Point, None, None]:
118118
"""
119119
>>> list(_parse_gps_box(b"[1623057074211]$GPGGA,202530.00,5109.0262,N,11401.8407,W,5,40,0.5,1097.36,M,-17.00,M,18,TSTR*61"))
120120
[Point(time=1623057074211, lat=51.150436666666664, lon=-114.03067833333333, alt=1097.36, angle=None)]
121+
121122
>>> list(_parse_gps_box(b"[1629874404069]$GNGGA,175322.00,3244.53126,N,11710.97811,W,1,12,0.84,17.4,M,-34.0,M,,*45"))
122123
[Point(time=1629874404069, lat=32.742187666666666, lon=-117.1829685, alt=17.4, angle=None)]
124+
125+
>>> list(_parse_gps_box(b"[1629874404069]$GNGLL,4404.14012,N,12118.85993,W,001037.00,A,A*67"))
126+
[Point(time=1629874404069, lat=44.069002, lon=-121.31433216666667, alt=None, angle=None)]
127+
128+
>>> list(_parse_gps_box(b"[1629874404069]$GNRMC,001031.00,A,4404.13993,N,12118.86023,W,0.146,,100117,,,A*7B"))
129+
[Point(time=1629874404069, lat=44.06899883333333, lon=-121.31433716666666, alt=None, angle=None)]
130+
123131
>>> list(_parse_gps_box(b"[1623057074211]$GPVTG,,T,,M,0.078,N,0.144,K,D*28[1623057075215]"))
124132
[]
125133
"""
@@ -128,24 +136,43 @@ def _parse_gps_box(gps_data: bytes) -> T.Generator[geo.Point, None, None]:
128136
if match is None:
129137
continue
130138
nmea_line_bytes = match.group(2)
131-
if nmea_line_bytes.startswith(b"$GPGGA") or nmea_line_bytes.startswith(
132-
b"$GNGGA"
133-
):
134-
try:
135-
nmea_line = nmea_line_bytes.decode("utf8")
136-
except UnicodeDecodeError:
137-
continue
138-
try:
139-
nmea = pynmea2.parse(nmea_line)
140-
except pynmea2.nmea.ParseError:
139+
140+
if not nmea_line_bytes:
141+
continue
142+
143+
try:
144+
nmea_line = nmea_line_bytes.decode("utf8")
145+
except UnicodeDecodeError:
146+
continue
147+
148+
if not nmea_line:
149+
continue
150+
151+
try:
152+
message = pynmea2.parse(nmea_line)
153+
except pynmea2.nmea.ParseError:
154+
continue
155+
156+
epoch_ms = int(match.group(1))
157+
158+
# https://tavotech.com/gps-nmea-sentence-structure/
159+
if message.sentence_type in ["GGA"]:
160+
if not message.is_valid:
141161
continue
142-
if not nmea.is_valid:
162+
yield geo.Point(
163+
time=epoch_ms,
164+
lat=message.latitude,
165+
lon=message.longitude,
166+
alt=message.altitude,
167+
angle=None,
168+
)
169+
elif message.sentence_type in ["RMC", "GLL"]:
170+
if not message.is_valid:
143171
continue
144-
epoch_ms = int(match.group(1))
145172
yield geo.Point(
146173
time=epoch_ms,
147-
lat=nmea.latitude,
148-
lon=nmea.longitude,
149-
alt=nmea.altitude,
174+
lat=message.latitude,
175+
lon=message.longitude,
176+
alt=None,
150177
angle=None,
151178
)

‎mapillary_tools/process_sequence_properties.py‎

Lines changed: 7 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -24,24 +24,21 @@ def split_sequence_by(
2424
"""
2525
output_sequences: list[list[SeqItem]] = []
2626

27-
seq = iter(sequence)
28-
29-
prev = next(seq, None)
30-
if prev is None:
31-
return output_sequences
32-
33-
output_sequences.append([prev])
27+
if sequence:
28+
output_sequences.append([sequence[0]])
3429

35-
for cur in seq:
30+
for prev, cur in geo.pairwise(sequence):
3631
# invariant: prev is processed
3732
if should_split(prev, cur):
3833
output_sequences.append([cur])
3934
else:
4035
output_sequences[-1].append(cur)
41-
prev = cur
4236
# invariant: cur is processed
4337

44-
assert sum(len(s) for s in output_sequences) == len(sequence)
38+
assert sum(len(s) for s in output_sequences) == len(sequence), (
39+
output_sequences,
40+
sequence,
41+
)
4542

4643
return output_sequences
4744

‎tests/unit/test_blackvue_parser.py‎

Lines changed: 30 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -7,36 +7,33 @@
77

88
def test_parse_points():
99
gps_data = b"""
10-
# GGA
11-
[1623057130221]$GPGGA,201205.00,3853.16949,N,07659.54604,W,2,10,0.82,7.7,M,-34.7,M,,0000*6F
10+
[1623057130221]$GPGGA,201205.00,3853.16949,N,07659.54604,W,2,10,0.82,7.7,M,-34.7,M,,0000*6F
1211
13-
# GGA
14-
[1623057129253]$GPGGA,201204.00,3853.16945,N,07659.54371,W,2,10,0.99,10.2,M,-34.7,M,,0000*5C
12+
[1623057129253]$GPGGA,201204.00,3853.16945,N,07659.54371,W,2,10,0.99,10.2,M,-34.7,M,,0000*5C
1513
16-
[1623057129253]$GPGSA,A,3,19,02,06,12,17,09,05,20,04,25,,,1.83,0.99,1.54*0C
14+
[1623057129253]$GPGSA,A,3,19,02,06,12,17,09,05,20,04,25,,,1.83,0.99,1.54*0C
1715
18-
[1623057129253]$GPGSV,3,1,12,02,67,331,39,04,08,040,21,05,28,214,30,06,53,047,31*71
16+
[1623057129253]$GPGSV,3,1,12,02,67,331,39,04,08,040,21,05,28,214,30,06,53,047,31*71
1917
20-
[1623057129253]$GPGSV,3,2,12,09,23,071,28,12,48,268,41,17,17,124,26,19,38,117,35*78
18+
[1623057129253]$GPGSV,3,2,12,09,23,071,28,12,48,268,41,17,17,124,26,19,38,117,35*78
2119
22-
[1623057129253]$GPGSV,3,3,12,20,23,221,35,25,26,307,39,46,20,244,35,51,35,223,40*72
20+
[1623057129253]$GPGSV,3,3,12,20,23,221,35,25,26,307,39,46,20,244,35,51,35,223,40*72
2321
24-
[1623057129253]$GPGLL,3853.16945,N,07659.54371,W,201204.00,A,D*70
22+
[1623057129255]$GPGLL,3853.16945,N,07659.54371,W,201204.00,A,D*70
2523
26-
[1623057129253]$GPRMC,201205.00,A,3853.16949,N,07659.54604,W,5.849,284.43,070621,,,D*76
24+
[1623057129256]$GPRMC,201205.00,A,3853.16949,N,07659.54604,W,5.849,284.43,070621,,,D*76
2725
28-
[1623057129253]$GPVTG,284.43,T,,M,5.849,N,10.833,K,D*08[1623057130221]
26+
[1623057129257]$GPVTG,284.43,T,,M,5.849,N,10.833,K,D*08[1623057130221]
2927
30-
# GGA
31-
[1623057130221]$GPGGA,201205.00,3853.16949,N,07659.54604,W,2,10,0.82,7.7,M,-34.7,M,,0000*6F
28+
[1623057130258]$GPGGA,201205.00,3853.16949,N,07659.54604,W,2,10,0.82,7.7,M,-34.7,M,,0000*6F
3229
33-
# invalid line
34-
[1623057130221]$GPGGA,**&^%$%$&(&(*(&&(^^*^*^^*&^&*))))
30+
# invalid line
31+
[1623057130221]$GPGGA,**&^%$%$&(&(*(&&(^^*^*^^*&^&*))))
3532
36-
# invalid line
37-
[1623057130221]$GPGGA,\x00\x00\x1c\xff
33+
# invalid line
34+
[1623057130221]$GPGGA,\x00\x00\x1c\xff
3835
39-
[1623057130221]$GPGSA,A,3,19,02,06,12,17,09,05,20,04,25,,,1.65,0.82,1.43*08
36+
[1623057130221]$GPGSA,A,3,19,02,06,12,17,09,05,20,04,25,,,1.65,0.82,1.43*08
4037
"""
4138

4239
box = {"type": b"free", "data": [{"type": b"gps ", "data": gps_data}]}
@@ -48,9 +45,23 @@ def test_parse_points():
4845
time=0.0, lat=38.8861575, lon=-76.99239516666667, alt=10.2, angle=None
4946
),
5047
geo.Point(
51-
time=0.968, lat=38.88615816666667, lon=-76.992434, alt=7.7, angle=None
48+
time=0.002,
49+
lat=38.8861575,
50+
lon=-76.99239516666667,
51+
alt=None,
52+
angle=None,
53+
),
54+
geo.Point(
55+
time=0.003,
56+
lat=38.88615816666667,
57+
lon=-76.992434,
58+
alt=None,
59+
angle=None,
5260
),
5361
geo.Point(
5462
time=0.968, lat=38.88615816666667, lon=-76.992434, alt=7.7, angle=None
5563
),
64+
geo.Point(
65+
time=1.005, lat=38.88615816666667, lon=-76.992434, alt=7.7, angle=None
66+
),
5667
] == list(info.gps or [])

0 commit comments

Comments
 (0)