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

rawgps: add oemdre support (#24059)

Browse Source 
* oemdre works

* fast receive

* parse oemdre

* parsing dre

* parsing oemdre

* unused import

* remove unparsed msgs

* import, don't redefine

Co-authored-by: Comma Device <device@comma.ai>
This commit is contained in:
George Hotz
2022-03-30 16:12:24 -07:00
committed by GitHub
parent d14791525d
commit 5af52627de
3 changed files with 189 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
selfdrive/sensord/rawgps/modemdiag.py
View File

@@ -1,5 +1,6 @@
import os
import time
import select
from serial import Serial
from crcmod import mkCrcFun
from struct import pack, unpack_from, calcsize
@@ -7,10 +8,11 @@ from struct import pack, unpack_from, calcsize
class ModemDiag:
def __init__(self):
self.serial = self.open_serial()
self.pend = b''
def open_serial(self):
def op():
return Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True)
return Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True, timeout=0)
try:
serial = op()
except Exception:
@@ -22,6 +24,8 @@ class ModemDiag:
os.system("sudo chmod 666 /dev/ttyUSB0")
serial = op()
serial.flush()
serial.reset_input_buffer()
serial.reset_output_buffer()
return serial
ccitt_crc16 = mkCrcFun(0x11021, initCrc=0, xorOut=0xffff)
@@ -45,13 +49,15 @@ class ModemDiag:
return payload[:-2]
def recv(self):
raw_payload = []
while 1:
char_read = self.serial.read()
raw_payload.append(char_read)
if char_read.endswith(self.TRAILER_CHAR):
break
# self.serial.read_until makes tons of syscalls!
raw_payload = [self.pend]
while self.TRAILER_CHAR not in raw_payload[-1]:
select.select([self.serial.fd], [], [])
raw = self.serial.read(0x10000)
raw_payload.append(raw)
raw_payload = b''.join(raw_payload)
raw_payload, self.pend = raw_payload.split(self.TRAILER_CHAR, 1)
raw_payload += self.TRAILER_CHAR
unframed_message = self.hdlc_decapsulate(raw_payload)
return unframed_message[0], unframed_message[1:]

94
selfdrive/sensord/rawgps/rawgpsd.py
View File

@@ -6,18 +6,19 @@ import itertools
import math
import time
from typing import NoReturn
from struct import unpack_from, calcsize
from struct import unpack_from, calcsize, pack
import cereal.messaging as messaging
from cereal import log
from selfdrive.swaglog import cloudlog
from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs
from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
from selfdrive.sensord.rawgps.structs import dict_unpacker
from selfdrive.sensord.rawgps.structs import gps_measurement_report, gps_measurement_report_sv
from selfdrive.sensord.rawgps.structs import glonass_measurement_report, glonass_measurement_report_sv
from selfdrive.sensord.rawgps.structs import oemdre_measurement_report, oemdre_measurement_report_sv
from selfdrive.sensord.rawgps.structs import LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT
from selfdrive.sensord.rawgps.structs import position_report, LOG_GNSS_POSITION_REPORT
from selfdrive.sensord.rawgps.structs import position_report, LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT
DEBUG = int(os.getenv("DEBUG", "0"))==1
@@ -71,9 +72,13 @@ def main() -> NoReturn:
unpack_glonass_meas, size_glonass_meas = dict_unpacker(glonass_measurement_report, True)
unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(glonass_measurement_report_sv, True)
unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True)
unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True)
log_types = [
LOG_GNSS_GPS_MEASUREMENT_REPORT,
LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
]
pub_types = ['qcomGnss']
if int(os.getenv("PUBLISH_EXTERNAL", "0")) == 1:
@@ -81,11 +86,18 @@ def main() -> NoReturn:
log_types.append(LOG_GNSS_POSITION_REPORT)
pub_types.append("gpsLocationExternal")
# disable DPO power savings for more accuracy
os.system("mmcli -m 0 --command='AT+QGPSCFG=\"dpoenable\",0'")
os.system("mmcli -m 0 --location-enable-gps-raw --location-enable-gps-nmea")
# connect to modem
diag = ModemDiag()
# NV enable OEMDRE
# TODO: it has to reboot for this to take effect
DIAG_NV_READ_F = 38
DIAG_NV_WRITE_F = 39
NV_GNSS_OEM_FEATURE_MASK = 7165
opcode, payload = send_recv(diag, DIAG_NV_WRITE_F, pack('<HI', NV_GNSS_OEM_FEATURE_MASK, 1))
opcode, payload = send_recv(diag, DIAG_NV_READ_F, pack('<H', NV_GNSS_OEM_FEATURE_MASK))
def try_setup_logs(diag, log_types):
for _ in range(5):
try:
@@ -104,6 +116,29 @@ def main() -> NoReturn:
try_setup_logs(diag, log_types)
cloudlog.warning("rawgpsd: setup logs done")
# disable DPO power savings for more accuracy
os.system("mmcli -m 0 --command='AT+QGPSCFG=\"dpoenable\",0'")
os.system("mmcli -m 0 --location-enable-gps-raw --location-enable-gps-nmea")
# enable OEMDRE mode
DIAG_SUBSYS_CMD_F = 75
DIAG_SUBSYS_GPS = 13
CGPS_DIAG_PDAPI_CMD = 0x64
CGPS_OEM_CONTROL = 202
GPSDIAG_OEMFEATURE_DRE = 1
GPSDIAG_OEM_DRE_ON = 1
# gpsdiag_OemControlReqType
opcode, payload = send_recv(diag, DIAG_SUBSYS_CMD_F, pack('<BHBBIIII',
DIAG_SUBSYS_GPS, # Subsystem Id
CGPS_DIAG_PDAPI_CMD, # Subsystem Command Code
CGPS_OEM_CONTROL, # CGPS Command Code
0, # Version
GPSDIAG_OEMFEATURE_DRE,
GPSDIAG_OEM_DRE_ON,
0,0
))
pm = messaging.PubMaster(pub_types)
while 1:
@@ -118,8 +153,51 @@ def main() -> NoReturn:
if log_type not in log_types:
continue
if DEBUG:
print("%.2f: got log: %x len %d" % (time.time(), log_type, len(log_payload)))
if log_type == LOG_GNSS_POSITION_REPORT:
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')
gnss = msg.qcomGnss
gnss.logTs = log_time
gnss.init('drMeasurementReport')
report = gnss.drMeasurementReport
dat = unpack_oemdre_meas(log_payload)
for k,v in dat.items():
if k in ["gpsTimeBias", "gpsClockTimeUncertainty"]:
k += "Ms"
if k == "version":
assert v == 2
elif k == "svCount" or k.startswith("cdmaClockInfo["):
# TODO: should we save cdmaClockInfo?
pass
elif k == "systemRtcValid":
setattr(report, k, bool(v))
else:
setattr(report, k, v)
report.init('sv', dat['svCount'])
sats = log_payload[size_oemdre_meas:]
for i in range(dat['svCount']):
sat = unpack_oemdre_meas_sv(sats[size_oemdre_meas_sv*i:size_oemdre_meas_sv*(i+1)])
sv = report.sv[i]
sv.init('measurementStatus')
for k,v in sat.items():
if k in ["unkn", "measurementStatus2"]:
pass
elif k == "multipathEstimateValid":
sv.measurementStatus.multipathEstimateIsValid = bool(v)
elif k == "directionValid":
sv.measurementStatus.directionIsValid = bool(v)
elif k == "goodParity":
setattr(sv, k, bool(v))
elif k == "measurementStatus":
for kk,vv in measurementStatusFields.items():
setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
else:
setattr(sv, k, v)
pm.send('qcomGnss', msg)
elif log_type == LOG_GNSS_POSITION_REPORT:
report = unpack_position(log_payload)
if report["u_PosSource"] != 2:
continue

91
selfdrive/sensord/rawgps/structs.py
View File

@@ -16,6 +16,93 @@ LOG_GNSS_PRX_RF_HW_STATUS_REPORT = 0x147E
LOG_CGPS_SLOW_CLOCK_CLIB_REPORT = 0x1488
LOG_GNSS_CONFIGURATION_STATE = 0x1516
oemdre_measurement_report = """
uint8_t version;
uint8_t reason;
uint8_t sv_count;
uint8_t seq_num;
uint8_t seq_max;
uint16_t rf_loss;
uint8_t system_rtc_valid;
uint32_t f_count;
uint32_t clock_resets;
uint64_t system_rtc_time;
uint8_t gps_leap_seconds;
uint8_t gps_leap_seconds_uncertainty;
float gps_to_glonass_time_bias_milliseconds;
float gps_to_glonass_time_bias_milliseconds_uncertainty;
uint16_t gps_week;
uint32_t gps_milliseconds;
uint32_t gps_time_bias;
uint32_t gps_clock_time_uncertainty;
uint8_t gps_clock_source;
uint8_t glonass_clock_source;
uint8_t glonass_year;
uint16_t glonass_day;
uint32_t glonass_milliseconds;
float glonass_time_bias;
float glonass_clock_time_uncertainty;
float clock_frequency_bias;
float clock_frequency_uncertainty;
uint8_t frequency_source;
uint32_t cdma_clock_info[5];
uint8_t source;
"""
oemdre_measurement_report_sv = """
uint8_t sv_id;
uint8_t unkn;
int8_t glonass_frequency_index;
uint32_t observation_state;
uint8_t observations;
uint8_t good_observations;
uint8_t filter_stages;
uint8_t predetect_interval;
uint8_t cycle_slip_count;
uint16_t postdetections;
uint32_t measurement_status;
uint32_t measurement_status2;
uint16_t carrier_noise;
uint16_t rf_loss;
int16_t latency;
float filtered_measurement_fraction;
uint32_t filtered_measurement_integral;
float filtered_time_uncertainty;
float filtered_speed;
float filtered_speed_uncertainty;
float unfiltered_measurement_fraction;
uint32_t unfiltered_measurement_integral;
float unfiltered_time_uncertainty;
float unfiltered_speed;
float unfiltered_speed_uncertainty;
uint8_t multipath_estimate_valid;
uint32_t multipath_estimate;
uint8_t direction_valid;
float azimuth;
float elevation;
float doppler_acceleration;
float fine_speed;
float fine_speed_uncertainty;
uint64_t carrier_phase;
uint32_t f_count;
uint16_t parity_error_count;
uint8_t good_parity;
"""
glonass_measurement_report = """
uint8_t version;
uint32_t f_count;
@@ -184,6 +271,8 @@ def parse_struct(ss):
st = "<"
nams = []
for l in ss.strip().split("\n"):
if len(l.strip()) == 0:
continue
typ, nam = l.split(";")[0].split()
#print(typ, nam)
if typ == "float" or '_Flt' in nam:
@@ -202,7 +291,7 @@ def parse_struct(ss):
st += "H"
elif typ in ["int16", "int16_t"]:
st += "h"
elif typ == "uint64":
elif typ in ["uint64", "uint64_t"]:
st += "Q"
else:
print("unknown type", typ)