sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
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>
pull/24092/head
George Hotz 3 years ago committed by GitHub
parent d14791525d
commit 5af52627de
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 189 additions and 16 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 20
      selfdrive/sensord/rawgps/modemdiag.py
  2. 94
      selfdrive/sensord/rawgps/rawgpsd.py
  3. 91
      selfdrive/sensord/rawgps/structs.py

20
selfdrive/sensord/rawgps/modemdiag.py
Unescape 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
Unescape 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
Unescape 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)

Write Preview
Loading…
Cancel
Save