mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-19 05:24:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rm laika deps in rawgpsd (#30452)

Browse Source 
* Rm more laikad references

* rawgpsd usage of laika

* bugfix
This commit is contained in:
Harald Schäfer
2023-11-13 23:38:24 -05:00
committed by GitHub
parent 95c6d5140a
commit c0942dbf9f
1 changed files with 13 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
system/sensord/rawgps/rawgpsd.py
View File

@@ -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]: