Rm laika deps in rawgpsd (#30452)

* Rm more laikad references

* rawgpsd usage of laika

* bugfix
pull/30458/head
Harald Schäfer 1 year ago committed by GitHub
parent 95c6d5140a
commit c0942dbf9f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
  1. 25
      system/sensord/rawgps/rawgpsd.py

@ -8,7 +8,7 @@ import time
import pycurl import pycurl
import shutil import shutil
import subprocess import subprocess
from datetime import datetime import datetime
from multiprocessing import Process, Event from multiprocessing import Process, Event
from typing import NoReturn, Optional from typing import NoReturn, Optional
from struct import unpack_from, calcsize, pack from struct import unpack_from, calcsize, pack
@ -16,9 +16,6 @@ from struct import unpack_from, calcsize, pack
from cereal import log from cereal import log
import cereal.messaging as messaging import cereal.messaging as messaging
from openpilot.common.gpio import gpio_init, gpio_set 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.hardware.tici.pins import GPIO
from openpilot.system.swaglog import cloudlog from openpilot.system.swaglog import cloudlog
from openpilot.system.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv 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() inject_assistance()
os.remove(ASSIST_DATA_FILE) os.remove(ASSIST_DATA_FILE)
#at_cmd("AT+QGPSXTRADATA?") #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(f"AT+QGPSXTRATIME=0,\"{time_str}\",1,1,1000")
at_cmd("AT+QGPSCFG=\"outport\",\"usbnmea\"") at_cmd("AT+QGPSCFG=\"outport\",\"usbnmea\"")
@ -293,7 +290,6 @@ def main() -> NoReturn:
diag = ModemDiag() diag = ModemDiag()
r = setup_quectel(diag) r = setup_quectel(diag)
want_assistance = not r want_assistance = not r
current_gps_time = utc_to_gpst(GPSTime.from_datetime(datetime.utcnow()))
cloudlog.warning("quectel setup done") cloudlog.warning("quectel setup done")
gpio_init(GPIO.GNSS_PWR_EN, True) gpio_init(GPIO.GNSS_PWR_EN, True)
gpio_set(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))) setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
else: else:
setattr(sv, k, v) 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) pm.send('qcomGnss', msg)
elif log_type == LOG_GNSS_POSITION_REPORT: elif log_type == LOG_GNSS_POSITION_REPORT:
report = unpack_position(log_payload) report = unpack_position(log_payload)
@ -383,8 +377,12 @@ def main() -> NoReturn:
gps.altitude = report["q_FltFinalPosAlt"] gps.altitude = report["q_FltFinalPosAlt"]
gps.speed = math.sqrt(sum([x**2 for x in vNED])) gps.speed = math.sqrt(sum([x**2 for x in vNED]))
gps.bearingDeg = report["q_FltHeadingRad"] * 180/math.pi 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.source = log.GpsLocationData.SensorSource.qcomdiag
gps.vNED = vNED gps.vNED = vNED
gps.verticalAccuracy = report["q_FltVdop"] gps.verticalAccuracy = report["q_FltVdop"]
@ -395,8 +393,6 @@ def main() -> NoReturn:
if gps.flags: if gps.flags:
want_assistance = False want_assistance = False
stop_download_event.set() stop_download_event.set()
pm.send('gpsLocation', msg) pm.send('gpsLocation', msg)
elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT: elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT:
@ -415,6 +411,10 @@ def main() -> NoReturn:
else: else:
setattr(poly, k, v) 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) prn = get_prn_from_nmea_id(poly.svId)
if prn[0] == 'R': 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)) 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.gpsWeek = epoch.week
poly.gpsTow = epoch.tow poly.gpsTow = epoch.tow
'''
pm.send('qcomGnss', msg) pm.send('qcomGnss', msg)
elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]:

Loading…
Cancel
Save