diff --git a/mapillary_tools/geotag/gpmf_parser.py b/mapillary_tools/geotag/gpmf_parser.py index da4e9d3b..f0916fd0 100644 --- a/mapillary_tools/geotag/gpmf_parser.py +++ b/mapillary_tools/geotag/gpmf_parser.py @@ -3,6 +3,8 @@ import itertools import pathlib import typing as T +import datetime + import construct as C @@ -137,6 +139,14 @@ class TelemetryData: magn: T.List[imu.MagnetometerData] +def _gps5_timestamp_to_epoch_time(dtstr: str): + # yymmddhhmmss.sss + dt = datetime.datetime.strptime(dtstr, "%y%m%d%H%M%S.%f").replace( + tzinfo=datetime.timezone.utc + ) + return dt.timestamp() + + # A GPS5 stream example: # key = b'STRM' type = b'\x00' structure_size = 1 repeat = 400 # data = ListContainer: @@ -197,6 +207,12 @@ def gps5_from_stream( else: gpsf_value = None + gpsu = indexed.get(b"GPSU") + if gpsu is not None: + epoch_time = _gps5_timestamp_to_epoch_time(gpsu[0][0].decode("utf-8")) + else: + epoch_time = None + gpsp = indexed.get(b"GPSP") if gpsp is not None: gpsp_value = gpsp[0][0] @@ -213,7 +229,7 @@ def gps5_from_stream( lat=lat, lon=lon, alt=alt, - epoch_time=None, + epoch_time=epoch_time, fix=gpsf_value, precision=gpsp_value, ground_speed=ground_speed, @@ -221,6 +237,19 @@ def gps5_from_stream( ) +_EPOCH_TIME_IN_2000 = datetime.datetime( + 2000, 1, 1, tzinfo=datetime.timezone.utc +).timestamp() + + +def _gps9_timestamp_to_epoch_time( + days_since_2000: int, secs_since_midnight: float +) -> float: + epoch_time = _EPOCH_TIME_IN_2000 + days_since_2000 * 24 * 60 * 60 + epoch_time += secs_since_midnight + return epoch_time + + def gps9_from_stream( stream: T.Sequence[KLVDict], ) -> T.Generator[GPSPoint, None, None]: @@ -260,19 +289,21 @@ def gps9_from_stream( alt, speed_2d, _speed_3d, - _days_since_2000, - _secs_since_midnight, + days_since_2000, + secs_since_midnight, dop, gps_fix, ) = [v / s for v, s in zip(sample_data, scal_values)] + epoch_time = _gps9_timestamp_to_epoch_time(days_since_2000, secs_since_midnight) + yield GPSPoint( # will figure out the actual timestamp later time=0, lat=lat, lon=lon, alt=alt, - epoch_time=None, + epoch_time=epoch_time, fix=GPSFix(gps_fix), precision=dop * 100, ground_speed=speed_2d, diff --git a/tests/cli/gpmf_parser.py b/tests/cli/gpmf_parser.py index cb7d0eeb..d271a588 100644 --- a/tests/cli/gpmf_parser.py +++ b/tests/cli/gpmf_parser.py @@ -43,19 +43,23 @@ def _convert_points_to_gpx_track_segment( { "distance_between": distance, "speed_between": speed, - "ground_speed": point.gps_ground_speed, + "ground_speed": point.ground_speed, } ) + if point.epoch_time is not None: + epoch_time = point.epoch_time + else: + epoch_time = point.time gpxp = gpxpy.gpx.GPXTrackPoint( point.lat, point.lon, elevation=point.alt, - time=datetime.datetime.utcfromtimestamp(point.time), - position_dilution=point.gps_precision, + time=datetime.datetime.utcfromtimestamp(epoch_time), + position_dilution=point.precision, comment=comment, ) - if point.gps_fix is not None: - gpxp.type_of_gpx_fix = gps_fix_map.get(point.gps_fix) + if point.fix is not None: + gpxp.type_of_gpx_fix = gps_fix_map.get(point.fix) gpx_segment.points.append(gpxp) return gpx_segment @@ -86,10 +90,10 @@ def _convert_geojson(path: pathlib.Path): geomtry = {"type": "Point", "coordinates": [p.lon, p.lat]} properties = { "alt": p.alt, - "fix": p.gps_fix.value if p.gps_fix is not None else None, + "fix": p.fix.value if p.fix is not None else None, "index": idx, "name": path.name, - "precision": p.gps_precision, + "precision": p.precision, "time": p.time, } features.append(