wip

system/qcomgpsd/qcomgpsd.py

@@ -15,6 +15,7 @@ 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 openpilot.common.retry import retry
 from openpilot.system.hardware.tici.pins import GPIO
@@ -280,8 +281,8 @@ def main() -> NoReturn:
     if log_type not in LOG_TYPES:
       continue
 
-    if DEBUG:
-      print("%.4f: got log: %x len %d" % (time.time(), log_type, len(log_payload)))
+    # if DEBUG:
+      # print("%.4f: got log: %x len %d" % (time.time(), log_type, len(log_payload)))
 
     if log_type == LOG_GNSS_OEMDRE_MEASUREMENT_REPORT:
       msg = messaging.new_message('qcomGnss', valid=True)
@@ -341,6 +342,9 @@ def main() -> NoReturn:
       gps.speed = math.sqrt(sum([x**2 for x in vNED]))
       gps.bearingDeg = report["q_FltHeadingRad"] * 180/math.pi
 
+      if DEBUG:
+        print("%.4f: lat: %.4f lng: %.4f speed: %.4f" % (time.time(), gps.latitude, gps.longitude, gps.speed))
+
       # TODO needs update if there is another leap second, after june 2024?
       dt_timestamp = (datetime.datetime(1980, 1, 6, 0, 0, 0, 0, datetime.timezone.utc) +
                       datetime.timedelta(weeks=report['w_GpsWeekNumber']) +