From c0942dbf9fd0d6be501bf4eec99eba003c3a3e0c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= <harald.the.engineer@gmail.com>
Date: Mon, 13 Nov 2023 23:38:24 -0500
Subject: [PATCH] Rm laika deps in rawgpsd (#30452)

* Rm more laikad references

* rawgpsd usage of laika

* bugfix
---
 system/sensord/rawgps/rawgpsd.py | 25 +++++++++++++------------
 1 file changed, 13 insertions(+), 12 deletions(-)

diff --git a/system/sensord/rawgps/rawgpsd.py b/system/sensord/rawgps/rawgpsd.py
index b947c54872..787a9316d3 100755
--- a/system/sensord/rawgps/rawgpsd.py
+++ b/system/sensord/rawgps/rawgpsd.py
@@ -8,7 +8,7 @@ import time
 import pycurl
 import shutil
 import subprocess
-from datetime import datetime
+import datetime
 from multiprocessing import Process, Event
 from typing import NoReturn, Optional
 from struct import unpack_from, calcsize, pack
@@ -16,9 +16,6 @@ from struct import unpack_from, calcsize, pack
 from cereal import log
 import cereal.messaging as messaging
 from openpilot.common.gpio import gpio_init, gpio_set
-from laika.gps_time import GPSTime, utc_to_gpst, get_leap_seconds
-from laika.helpers import get_prn_from_nmea_id
-from laika.constants import SECS_IN_HR, SECS_IN_DAY, SECS_IN_WEEK
 from openpilot.system.hardware.tici.pins import GPIO
 from openpilot.system.swaglog import cloudlog
 from openpilot.system.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
@@ -211,7 +208,7 @@ def setup_quectel(diag: ModemDiag) -> bool:
     inject_assistance()
     os.remove(ASSIST_DATA_FILE)
   #at_cmd("AT+QGPSXTRADATA?")
-  time_str = datetime.utcnow().strftime("%Y/%m/%d,%H:%M:%S")
+  time_str = datetime.datetime.utcnow().strftime("%Y/%m/%d,%H:%M:%S")
   at_cmd(f"AT+QGPSXTRATIME=0,\"{time_str}\",1,1,1000")
 
   at_cmd("AT+QGPSCFG=\"outport\",\"usbnmea\"")
@@ -293,7 +290,6 @@ def main() -> NoReturn:
   diag = ModemDiag()
   r = setup_quectel(diag)
   want_assistance = not r
-  current_gps_time = utc_to_gpst(GPSTime.from_datetime(datetime.utcnow()))
   cloudlog.warning("quectel setup done")
   gpio_init(GPIO.GNSS_PWR_EN, True)
   gpio_set(GPIO.GNSS_PWR_EN, True)
@@ -366,8 +362,6 @@ def main() -> NoReturn:
               setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
           else:
             setattr(sv, k, v)
-      if report.source == log.QcomGnss.MeasurementSource.gps:
-        current_gps_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0)
       pm.send('qcomGnss', msg)
     elif log_type == LOG_GNSS_POSITION_REPORT:
       report = unpack_position(log_payload)
@@ -383,8 +377,12 @@ def main() -> NoReturn:
       gps.altitude = report["q_FltFinalPosAlt"]
       gps.speed = math.sqrt(sum([x**2 for x in vNED]))
       gps.bearingDeg = report["q_FltHeadingRad"] * 180/math.pi
-      gps.unixTimestampMillis = GPSTime(report['w_GpsWeekNumber'],
-                                        1e-3*report['q_GpsFixTimeMs']).as_unix_timestamp()*1e3
+
+      # TODO needs update if there is another leap second, after june 2024?
+      dt_timestamp = (datetime.datetime(1980, 1, 6, 0, 0, 0, 0, None) +
+                      datetime.timedelta(weeks=report['w_GpsWeekNumber']) +
+                      datetime.timedelta(seconds=(1e-3*report['q_GpsFixTimeMs'] - 18)))
+      gps.unixTimestampMillis = dt_timestamp.timestamp()*1e3
       gps.source = log.GpsLocationData.SensorSource.qcomdiag
       gps.vNED = vNED
       gps.verticalAccuracy = report["q_FltVdop"]
@@ -395,8 +393,6 @@ def main() -> NoReturn:
       if gps.flags:
         want_assistance = False
         stop_download_event.set()
-
-
       pm.send('gpsLocation', msg)
 
     elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT:
@@ -415,6 +411,10 @@ def main() -> NoReturn:
         else:
           setattr(poly, k, v)
 
+      '''
+      # Timestamp glonass polys with GPSTime
+      from laika.gps_time import GPSTime, utc_to_gpst, get_leap_seconds
+      from laika.helpers import get_prn_from_nmea_id
       prn = get_prn_from_nmea_id(poly.svId)
       if prn[0] == 'R':
         epoch = GPSTime(current_gps_time.week, (poly.t0 - 3*SECS_IN_HR + SECS_IN_DAY) % (SECS_IN_WEEK) + get_leap_seconds(current_gps_time))
@@ -429,6 +429,7 @@ def main() -> NoReturn:
 
       poly.gpsWeek = epoch.week
       poly.gpsTow = epoch.tow
+      '''
       pm.send('qcomGnss', msg)
 
     elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: