sensord: add support for Quectel raw gps (#24027)
* connecting to rawgps * dumping rawfps packets * this works, but it might be useless * fix parsing * parsing 0x1476 * compare * compare better * refactoring * parsing and broadcasting GPS packet * glonass support * compare using submaster * fix compare for glonass * update cereal * make linter happy * other linter issue * last mypy complaints * add support for publishing gpsLocationExternal * set flags to 1 to match ubloxd * ready to ship * qcomdiag * use unused variables * last one Co-authored-by: Comma Device <device@comma.ai> Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>pull/24042/head
George Hotz
3 years ago
committed by
GitHub
parent
ea74a90ca0
commit
1df3c86999
5 changed files with 595 additions and 0 deletions
@ -0,0 +1,66 @@ |
|||||||
|
#!/usr/bin/env python3 |
||||||
|
import cereal.messaging as messaging |
||||||
|
from laika import constants |
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
sm = messaging.SubMaster(['ubloxGnss', 'qcomGnss']) |
||||||
|
|
||||||
|
meas = None |
||||||
|
while 1: |
||||||
|
sm.update() |
||||||
|
if sm['ubloxGnss'].which() == "measurementReport": |
||||||
|
meas = sm['ubloxGnss'].measurementReport.measurements |
||||||
|
if not sm.updated['qcomGnss'] or meas is None: |
||||||
|
continue |
||||||
|
report = sm['qcomGnss'].measurementReport |
||||||
|
if report.source not in [0, 1]: |
||||||
|
continue |
||||||
|
GLONASS = report.source == 1 |
||||||
|
recv_time = report.milliseconds / 1000 |
||||||
|
|
||||||
|
car = [] |
||||||
|
print("qcom has ", list(sorted([x.svId for x in report.sv]))) |
||||||
|
print("ublox has", list(sorted([x.svId for x in meas if x.gnssId == (6 if GLONASS else 0)]))) |
||||||
|
for i in report.sv: |
||||||
|
# match to ublox |
||||||
|
tm = None |
||||||
|
for m in meas: |
||||||
|
if i.svId == m.svId and m.gnssId == 0 and m.sigId == 0 and not GLONASS: |
||||||
|
tm = m |
||||||
|
if (i.svId-64) == m.svId and m.gnssId == 6 and m.sigId == 0 and GLONASS: |
||||||
|
tm = m |
||||||
|
if tm is None: |
||||||
|
continue |
||||||
|
|
||||||
|
if not i.measurementStatus.measurementNotUsable and i.measurementStatus.satelliteTimeIsKnown: |
||||||
|
sat_time = (i.unfilteredMeasurementIntegral + i.unfilteredMeasurementFraction + i.latency) / 1000 |
||||||
|
ublox_psuedorange = tm.pseudorange |
||||||
|
qcom_psuedorange = (recv_time - sat_time)*constants.SPEED_OF_LIGHT |
||||||
|
if GLONASS: |
||||||
|
glonass_freq = tm.glonassFrequencyIndex - 7 |
||||||
|
ublox_speed = -(constants.SPEED_OF_LIGHT / (constants.GLONASS_L1 + glonass_freq*constants.GLONASS_L1_DELTA)) * (tm.doppler) |
||||||
|
else: |
||||||
|
ublox_speed = -(constants.SPEED_OF_LIGHT / constants.GPS_L1) * tm.doppler |
||||||
|
qcom_speed = i.unfilteredSpeed |
||||||
|
car.append((i.svId, tm.pseudorange, ublox_speed, qcom_psuedorange, qcom_speed, tm.cno)) |
||||||
|
|
||||||
|
if len(car) == 0: |
||||||
|
print("nothing to compare") |
||||||
|
continue |
||||||
|
|
||||||
|
pr_err, speed_err = 0., 0. |
||||||
|
for c in car: |
||||||
|
ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed = c[1:5] |
||||||
|
pr_err += ublox_psuedorange - qcom_psuedorange |
||||||
|
speed_err += ublox_speed - qcom_speed |
||||||
|
pr_err /= len(car) |
||||||
|
speed_err /= len(car) |
||||||
|
print("avg psuedorange err %f avg speed err %f" % (pr_err, speed_err)) |
||||||
|
for c in sorted(car, key=lambda x: abs(x[1] - x[3] - pr_err)): |
||||||
|
svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed, cno = c |
||||||
|
print("svid: %3d pseudorange: %10.2f m speed: %8.2f m/s meas: %12.2f speed: %10.2f meas_err: %10.3f speed_err: %8.3f cno: %d" % |
||||||
|
(svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed, |
||||||
|
ublox_psuedorange - qcom_psuedorange - pr_err, ublox_speed - qcom_speed - speed_err, cno)) |
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -0,0 +1,101 @@ |
|||||||
|
import os |
||||||
|
import time |
||||||
|
from serial import Serial |
||||||
|
from crcmod import mkCrcFun |
||||||
|
from struct import pack, unpack_from, calcsize |
||||||
|
|
||||||
|
class ModemDiag: |
||||||
|
def __init__(self): |
||||||
|
self.serial = self.open_serial() |
||||||
|
|
||||||
|
def open_serial(self): |
||||||
|
def op(): |
||||||
|
return Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True) |
||||||
|
try: |
||||||
|
serial = op() |
||||||
|
except Exception: |
||||||
|
# TODO: this is a hack to get around modemmanager's exclusive open |
||||||
|
print("unlocking serial...") |
||||||
|
os.system('sudo su -c \'echo "1-1.1:1.0" > /sys/bus/usb/drivers/option/unbind\'') |
||||||
|
os.system('sudo su -c \'echo "1-1.1:1.0" > /sys/bus/usb/drivers/option/bind\'') |
||||||
|
time.sleep(0.5) |
||||||
|
os.system("sudo chmod 666 /dev/ttyUSB0") |
||||||
|
serial = op() |
||||||
|
serial.flush() |
||||||
|
return serial |
||||||
|
|
||||||
|
ccitt_crc16 = mkCrcFun(0x11021, initCrc=0, xorOut=0xffff) |
||||||
|
ESCAPE_CHAR = b'\x7d' |
||||||
|
TRAILER_CHAR = b'\x7e' |
||||||
|
|
||||||
|
def hdlc_encapsulate(self, payload): |
||||||
|
payload += pack('<H', ModemDiag.ccitt_crc16(payload)) |
||||||
|
payload = payload.replace(self.ESCAPE_CHAR, bytes([self.ESCAPE_CHAR[0], self.ESCAPE_CHAR[0] ^ 0x20])) |
||||||
|
payload = payload.replace(self.TRAILER_CHAR, bytes([self.ESCAPE_CHAR[0], self.TRAILER_CHAR[0] ^ 0x20])) |
||||||
|
payload += self.TRAILER_CHAR |
||||||
|
return payload |
||||||
|
|
||||||
|
def hdlc_decapsulate(self, payload): |
||||||
|
assert len(payload) >= 3 |
||||||
|
assert payload[-1:] == self.TRAILER_CHAR |
||||||
|
payload = payload[:-1] |
||||||
|
payload = payload.replace(bytes([self.ESCAPE_CHAR[0], self.TRAILER_CHAR[0] ^ 0x20]), self.TRAILER_CHAR) |
||||||
|
payload = payload.replace(bytes([self.ESCAPE_CHAR[0], self.ESCAPE_CHAR[0] ^ 0x20]), self.ESCAPE_CHAR) |
||||||
|
assert payload[-2:] == pack('<H', ModemDiag.ccitt_crc16(payload[:-2])) |
||||||
|
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 |
||||||
|
raw_payload = b''.join(raw_payload) |
||||||
|
unframed_message = self.hdlc_decapsulate(raw_payload) |
||||||
|
return unframed_message[0], unframed_message[1:] |
||||||
|
|
||||||
|
def send(self, packet_type, packet_payload): |
||||||
|
self.serial.write(self.hdlc_encapsulate(bytes([packet_type]) + packet_payload)) |
||||||
|
|
||||||
|
# *** end class *** |
||||||
|
|
||||||
|
DIAG_LOG_F = 16 |
||||||
|
DIAG_LOG_CONFIG_F = 115 |
||||||
|
LOG_CONFIG_RETRIEVE_ID_RANGES_OP = 1 |
||||||
|
LOG_CONFIG_SET_MASK_OP = 3 |
||||||
|
LOG_CONFIG_SUCCESS_S = 0 |
||||||
|
|
||||||
|
def send_recv(diag, packet_type, packet_payload): |
||||||
|
diag.send(packet_type, packet_payload) |
||||||
|
while 1: |
||||||
|
opcode, payload = diag.recv() |
||||||
|
if opcode != DIAG_LOG_F: |
||||||
|
break |
||||||
|
return opcode, payload |
||||||
|
|
||||||
|
def setup_logs(diag, types_to_log): |
||||||
|
opcode, payload = send_recv(diag, DIAG_LOG_CONFIG_F, pack('<3xI', LOG_CONFIG_RETRIEVE_ID_RANGES_OP)) |
||||||
|
|
||||||
|
header_spec = '<3xII' |
||||||
|
operation, status = unpack_from(header_spec, payload) |
||||||
|
assert operation == LOG_CONFIG_RETRIEVE_ID_RANGES_OP |
||||||
|
assert status == LOG_CONFIG_SUCCESS_S |
||||||
|
|
||||||
|
log_masks = unpack_from('<16I', payload, calcsize(header_spec)) |
||||||
|
|
||||||
|
for log_type, log_mask_bitsize in enumerate(log_masks): |
||||||
|
if log_mask_bitsize: |
||||||
|
log_mask = [0] * ((log_mask_bitsize+7)//8) |
||||||
|
for i in range(log_mask_bitsize): |
||||||
|
if ((log_type<<12)|i) in types_to_log: |
||||||
|
log_mask[i//8] |= 1 << (i%8) |
||||||
|
opcode, payload = send_recv(diag, DIAG_LOG_CONFIG_F, pack('<3xIII', |
||||||
|
LOG_CONFIG_SET_MASK_OP, |
||||||
|
log_type, |
||||||
|
log_mask_bitsize |
||||||
|
) + bytes(log_mask)) |
||||||
|
assert opcode == DIAG_LOG_CONFIG_F |
||||||
|
operation, status = unpack_from(header_spec, payload) |
||||||
|
assert operation == LOG_CONFIG_SET_MASK_OP |
||||||
|
assert status == LOG_CONFIG_SUCCESS_S |
||||||
@ -0,0 +1,201 @@ |
|||||||
|
#!/usr/bin/env python3 |
||||||
|
import os |
||||||
|
import sys |
||||||
|
import signal |
||||||
|
import itertools |
||||||
|
import math |
||||||
|
from typing import NoReturn |
||||||
|
from struct import unpack_from, calcsize |
||||||
|
|
||||||
|
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.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 LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT |
||||||
|
from selfdrive.sensord.rawgps.structs import position_report, LOG_GNSS_POSITION_REPORT |
||||||
|
|
||||||
|
miscStatusFields = { |
||||||
|
"multipathEstimateIsValid": 0, |
||||||
|
"directionIsValid": 1, |
||||||
|
} |
||||||
|
|
||||||
|
measurementStatusFields = { |
||||||
|
"subMillisecondIsValid": 0, |
||||||
|
"subBitTimeIsKnown": 1, |
||||||
|
"satelliteTimeIsKnown": 2, |
||||||
|
"bitEdgeConfirmedFromSignal": 3, |
||||||
|
"measuredVelocity": 4, |
||||||
|
"fineOrCoarseVelocity": 5, |
||||||
|
"lockPointValid": 6, |
||||||
|
"lockPointPositive": 7, |
||||||
|
|
||||||
|
"lastUpdateFromDifference": 9, |
||||||
|
"lastUpdateFromVelocityDifference": 10, |
||||||
|
"strongIndicationOfCrossCorelation": 11, |
||||||
|
"tentativeMeasurement": 12, |
||||||
|
"measurementNotUsable": 13, |
||||||
|
"sirCheckIsNeeded": 14, |
||||||
|
"probationMode": 15, |
||||||
|
|
||||||
|
"multipathIndicator": 24, |
||||||
|
"imdJammingIndicator": 25, |
||||||
|
"lteB13TxJammingIndicator": 26, |
||||||
|
"freshMeasurementIndicator": 27, |
||||||
|
} |
||||||
|
|
||||||
|
measurementStatusGPSFields = { |
||||||
|
"gpsRoundRobinRxDiversity": 18, |
||||||
|
"gpsRxDiversity": 19, |
||||||
|
"gpsLowBandwidthRxDiversityCombined": 20, |
||||||
|
"gpsHighBandwidthNu4": 21, |
||||||
|
"gpsHighBandwidthNu8": 22, |
||||||
|
"gpsHighBandwidthUniform": 23, |
||||||
|
} |
||||||
|
|
||||||
|
measurementStatusGlonassFields = { |
||||||
|
"glonassMeanderBitEdgeValid": 16, |
||||||
|
"glonassTimeMarkValid": 17 |
||||||
|
} |
||||||
|
|
||||||
|
def main() -> NoReturn: |
||||||
|
unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True) |
||||||
|
unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(gps_measurement_report_sv, True) |
||||||
|
|
||||||
|
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) |
||||||
|
|
||||||
|
log_types = [ |
||||||
|
LOG_GNSS_GPS_MEASUREMENT_REPORT, |
||||||
|
LOG_GNSS_GLONASS_MEASUREMENT_REPORT, |
||||||
|
] |
||||||
|
pub_types = ['qcomGnss'] |
||||||
|
if int(os.getenv("PUBLISH_EXTERNAL", "0")) == 1: |
||||||
|
unpack_position, _ = dict_unpacker(position_report) |
||||||
|
log_types.append(LOG_GNSS_POSITION_REPORT) |
||||||
|
pub_types.append("gpsLocationExternal") |
||||||
|
|
||||||
|
os.system("mmcli -m 0 --location-enable-gps-raw --location-enable-gps-nmea") |
||||||
|
diag = ModemDiag() |
||||||
|
|
||||||
|
def try_setup_logs(diag, log_types): |
||||||
|
for _ in range(5): |
||||||
|
try: |
||||||
|
setup_logs(diag, log_types) |
||||||
|
break |
||||||
|
except Exception: |
||||||
|
pass |
||||||
|
|
||||||
|
def disable_logs(sig, frame): |
||||||
|
os.system("mmcli -m 0 --location-disable-gps-raw --location-disable-gps-nmea") |
||||||
|
cloudlog.warning("rawgpsd: shutting down") |
||||||
|
try_setup_logs(diag, []) |
||||||
|
cloudlog.warning("rawgpsd: logs disabled") |
||||||
|
sys.exit(0) |
||||||
|
signal.signal(signal.SIGINT, disable_logs) |
||||||
|
try_setup_logs(diag, log_types) |
||||||
|
cloudlog.warning("rawgpsd: setup logs done") |
||||||
|
|
||||||
|
pm = messaging.PubMaster(pub_types) |
||||||
|
|
||||||
|
while 1: |
||||||
|
opcode, payload = diag.recv() |
||||||
|
assert opcode == DIAG_LOG_F |
||||||
|
(pending_msgs, log_outer_length), inner_log_packet = unpack_from('<BH', payload), payload[calcsize('<BH'):] |
||||||
|
if pending_msgs > 0: |
||||||
|
cloudlog.debug("have %d pending messages" % pending_msgs) |
||||||
|
assert log_outer_length == len(inner_log_packet) |
||||||
|
(log_inner_length, log_type, log_time), log_payload = unpack_from('<HHQ', inner_log_packet), inner_log_packet[calcsize('<HHQ'):] |
||||||
|
assert log_inner_length == len(inner_log_packet) |
||||||
|
if log_type not in log_types: |
||||||
|
continue |
||||||
|
#print("got log: %x" % log_type) |
||||||
|
|
||||||
|
if log_type == LOG_GNSS_POSITION_REPORT: |
||||||
|
report = unpack_position(log_payload) |
||||||
|
if report["u_PosSource"] != 2: |
||||||
|
continue |
||||||
|
vNED = [report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"], -report["q_FltVelEnuMps[2]"]] |
||||||
|
vNEDsigma = [report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"], -report["q_FltVelSigmaMps[2]"]] |
||||||
|
|
||||||
|
msg = messaging.new_message('gpsLocationExternal') |
||||||
|
gps = msg.gpsLocationExternal |
||||||
|
gps.flags = 1 |
||||||
|
gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180/math.pi |
||||||
|
gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180/math.pi |
||||||
|
gps.altitude = report["q_FltFinalPosAlt"] |
||||||
|
gps.speed = math.sqrt(sum([x**2 for x in vNED])) |
||||||
|
gps.bearingDeg = report["q_FltHeadingRad"] * 180/math.pi |
||||||
|
# TODO: this probably isn't right, use laika for this |
||||||
|
gps.timestamp = report['w_GpsWeekNumber']*604800*1000 + report['q_GpsFixTimeMs'] |
||||||
|
gps.source = log.GpsLocationData.SensorSource.qcomdiag |
||||||
|
gps.vNED = vNED |
||||||
|
gps.verticalAccuracy = report["q_FltVdop"] |
||||||
|
gps.bearingAccuracyDeg = report["q_FltHeadingUncRad"] * 180/math.pi |
||||||
|
gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma])) |
||||||
|
|
||||||
|
pm.send('gpsLocationExternal', msg) |
||||||
|
|
||||||
|
if log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: |
||||||
|
msg = messaging.new_message('qcomGnss') |
||||||
|
|
||||||
|
gnss = msg.qcomGnss |
||||||
|
gnss.logTs = log_time |
||||||
|
gnss.init('measurementReport') |
||||||
|
report = gnss.measurementReport |
||||||
|
|
||||||
|
if log_type == LOG_GNSS_GPS_MEASUREMENT_REPORT: |
||||||
|
dat = unpack_gps_meas(log_payload) |
||||||
|
sats = log_payload[size_gps_meas:] |
||||||
|
unpack_meas_sv, size_meas_sv = unpack_gps_meas_sv, size_gps_meas_sv |
||||||
|
report.source = 0 # gps |
||||||
|
measurement_status_fields = (measurementStatusFields.items(), measurementStatusGPSFields.items()) |
||||||
|
elif log_type == LOG_GNSS_GLONASS_MEASUREMENT_REPORT: |
||||||
|
dat = unpack_glonass_meas(log_payload) |
||||||
|
sats = log_payload[size_glonass_meas:] |
||||||
|
unpack_meas_sv, size_meas_sv = unpack_glonass_meas_sv, size_glonass_meas_sv |
||||||
|
report.source = 1 # glonass |
||||||
|
measurement_status_fields = (measurementStatusFields.items(), measurementStatusGlonassFields.items()) |
||||||
|
else: |
||||||
|
assert False |
||||||
|
|
||||||
|
for k,v in dat.items(): |
||||||
|
if k == "version": |
||||||
|
assert v == 0 |
||||||
|
elif k == "week": |
||||||
|
report.gpsWeek = v |
||||||
|
elif k == "svCount": |
||||||
|
pass |
||||||
|
else: |
||||||
|
setattr(report, k, v) |
||||||
|
assert len(sats)//dat['svCount'] == size_meas_sv |
||||||
|
report.init('sv', dat['svCount']) |
||||||
|
for i in range(dat['svCount']): |
||||||
|
sv = report.sv[i] |
||||||
|
sv.init('measurementStatus') |
||||||
|
sat = unpack_meas_sv(sats[size_meas_sv*i:size_meas_sv*(i+1)]) |
||||||
|
for k,v in sat.items(): |
||||||
|
if k == "parityErrorCount": |
||||||
|
sv.gpsParityErrorCount = v |
||||||
|
elif k == "frequencyIndex": |
||||||
|
sv.glonassFrequencyIndex = v |
||||||
|
elif k == "hemmingErrorCount": |
||||||
|
sv.glonassHemmingErrorCount = v |
||||||
|
elif k == "measurementStatus": |
||||||
|
for kk,vv in itertools.chain(*measurement_status_fields): |
||||||
|
setattr(sv.measurementStatus, kk, bool(v & (1<<vv))) |
||||||
|
elif k == "miscStatus": |
||||||
|
for kk,vv in miscStatusFields.items(): |
||||||
|
setattr(sv.measurementStatus, kk, bool(v & (1<<vv))) |
||||||
|
elif k == "pad": |
||||||
|
pass |
||||||
|
else: |
||||||
|
setattr(sv, k, v) |
||||||
|
|
||||||
|
pm.send('qcomGnss', msg) |
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
main() |
||||||
@ -0,0 +1,224 @@ |
|||||||
|
from struct import unpack_from, calcsize |
||||||
|
|
||||||
|
LOG_GNSS_POSITION_REPORT = 0x1476 |
||||||
|
LOG_GNSS_GPS_MEASUREMENT_REPORT = 0x1477 |
||||||
|
LOG_GNSS_CLOCK_REPORT = 0x1478 |
||||||
|
LOG_GNSS_GLONASS_MEASUREMENT_REPORT = 0x1480 |
||||||
|
LOG_GNSS_BDS_MEASUREMENT_REPORT = 0x1756 |
||||||
|
LOG_GNSS_GAL_MEASUREMENT_REPORT = 0x1886 |
||||||
|
|
||||||
|
LOG_GNSS_OEMDRE_MEASUREMENT_REPORT = 0x14DE |
||||||
|
LOG_GNSS_OEMDRE_SVPOLY_REPORT = 0x14E1 |
||||||
|
|
||||||
|
LOG_GNSS_ME_DPO_STATUS = 0x1838 |
||||||
|
LOG_GNSS_CD_DB_REPORT = 0x147B |
||||||
|
LOG_GNSS_PRX_RF_HW_STATUS_REPORT = 0x147E |
||||||
|
LOG_CGPS_SLOW_CLOCK_CLIB_REPORT = 0x1488 |
||||||
|
LOG_GNSS_CONFIGURATION_STATE = 0x1516 |
||||||
|
|
||||||
|
glonass_measurement_report = """ |
||||||
|
uint8_t version; |
||||||
|
uint32_t f_count; |
||||||
|
uint8_t glonass_cycle_number; |
||||||
|
uint16_t glonass_number_of_days; |
||||||
|
uint32_t milliseconds; |
||||||
|
float time_bias; |
||||||
|
float clock_time_uncertainty; |
||||||
|
float clock_frequency_bias; |
||||||
|
float clock_frequency_uncertainty; |
||||||
|
uint8_t sv_count; |
||||||
|
""" |
||||||
|
|
||||||
|
glonass_measurement_report_sv = """ |
||||||
|
uint8_t sv_id; |
||||||
|
int8_t frequency_index; |
||||||
|
uint8_t observation_state; // SVObservationStates |
||||||
|
uint8_t observations; |
||||||
|
uint8_t good_observations; |
||||||
|
uint8_t hemming_error_count; |
||||||
|
uint8_t filter_stages; |
||||||
|
uint16_t carrier_noise; |
||||||
|
int16_t latency; |
||||||
|
uint8_t predetect_interval; |
||||||
|
uint16_t postdetections; |
||||||
|
uint32_t unfiltered_measurement_integral; |
||||||
|
float unfiltered_measurement_fraction; |
||||||
|
float unfiltered_time_uncertainty; |
||||||
|
float unfiltered_speed; |
||||||
|
float unfiltered_speed_uncertainty; |
||||||
|
uint32_t measurement_status; |
||||||
|
uint8_t misc_status; |
||||||
|
uint32_t multipath_estimate; |
||||||
|
float azimuth; |
||||||
|
float elevation; |
||||||
|
int32_t carrier_phase_cycles_integral; |
||||||
|
uint16_t carrier_phase_cycles_fraction; |
||||||
|
float fine_speed; |
||||||
|
float fine_speed_uncertainty; |
||||||
|
uint8_t cycle_slip_count; |
||||||
|
uint32_t pad; |
||||||
|
""" |
||||||
|
|
||||||
|
gps_measurement_report = """ |
||||||
|
uint8_t version; |
||||||
|
uint32_t f_count; |
||||||
|
uint16_t week; |
||||||
|
uint32_t milliseconds; |
||||||
|
float time_bias; |
||||||
|
float clock_time_uncertainty; |
||||||
|
float clock_frequency_bias; |
||||||
|
float clock_frequency_uncertainty; |
||||||
|
uint8_t sv_count; |
||||||
|
""" |
||||||
|
|
||||||
|
gps_measurement_report_sv = """ |
||||||
|
uint8_t sv_id; |
||||||
|
uint8_t observation_state; // SVObservationStates |
||||||
|
uint8_t observations; |
||||||
|
uint8_t good_observations; |
||||||
|
uint16_t parity_error_count; |
||||||
|
uint8_t filter_stages; |
||||||
|
uint16_t carrier_noise; |
||||||
|
int16_t latency; |
||||||
|
uint8_t predetect_interval; |
||||||
|
uint16_t postdetections; |
||||||
|
uint32_t unfiltered_measurement_integral; |
||||||
|
float unfiltered_measurement_fraction; |
||||||
|
float unfiltered_time_uncertainty; |
||||||
|
float unfiltered_speed; |
||||||
|
float unfiltered_speed_uncertainty; |
||||||
|
uint32_t measurement_status; |
||||||
|
uint8_t misc_status; |
||||||
|
uint32_t multipath_estimate; |
||||||
|
float azimuth; |
||||||
|
float elevation; |
||||||
|
int32_t carrier_phase_cycles_integral; |
||||||
|
uint16_t carrier_phase_cycles_fraction; |
||||||
|
float fine_speed; |
||||||
|
float fine_speed_uncertainty; |
||||||
|
uint8_t cycle_slip_count; |
||||||
|
uint32_t pad; |
||||||
|
""" |
||||||
|
|
||||||
|
position_report = """ |
||||||
|
uint8 u_Version; /* Version number of DM log */ |
||||||
|
uint32 q_Fcount; /* Local millisecond counter */ |
||||||
|
uint8 u_PosSource; /* Source of position information */ /* 0: None 1: Weighted least-squares 2: Kalman filter 3: Externally injected 4: Internal database */ |
||||||
|
uint32 q_Reserved1; /* Reserved memory field */ |
||||||
|
uint16 w_PosVelFlag; /* Position velocity bit field: (see DM log 0x1476 documentation) */ |
||||||
|
uint32 q_PosVelFlag2; /* Position velocity 2 bit field: (see DM log 0x1476 documentation) */ |
||||||
|
uint8 u_FailureCode; /* Failure code: (see DM log 0x1476 documentation) */ |
||||||
|
uint16 w_FixEvents; /* Fix events bit field: (see DM log 0x1476 documentation) */ |
||||||
|
uint32 _fake_align_week_number; |
||||||
|
uint16 w_GpsWeekNumber; /* GPS week number of position */ |
||||||
|
uint32 q_GpsFixTimeMs; /* GPS fix time of week of in milliseconds */ |
||||||
|
uint8 u_GloNumFourYear; /* Number of Glonass four year cycles */ |
||||||
|
uint16 w_GloNumDaysInFourYear; /* Glonass calendar day in four year cycle */ |
||||||
|
uint32 q_GloFixTimeMs; /* Glonass fix time of day in milliseconds */ |
||||||
|
uint32 q_PosCount; /* Integer count of the number of unique positions reported */ |
||||||
|
uint64 t_DblFinalPosLatLon[2]; /* Final latitude and longitude of position in radians */ |
||||||
|
uint32 q_FltFinalPosAlt; /* Final height-above-ellipsoid altitude of position */ |
||||||
|
uint32 q_FltHeadingRad; /* User heading in radians */ |
||||||
|
uint32 q_FltHeadingUncRad; /* User heading uncertainty in radians */ |
||||||
|
uint32 q_FltVelEnuMps[3]; /* User velocity in east, north, up coordinate frame. In meters per second. */ |
||||||
|
uint32 q_FltVelSigmaMps[3]; /* Gaussian 1-sigma value for east, north, up components of user velocity */ |
||||||
|
uint32 q_FltClockBiasMeters; /* Receiver clock bias in meters */ |
||||||
|
uint32 q_FltClockBiasSigmaMeters; /* Gaussian 1-sigma value for receiver clock bias in meters */ |
||||||
|
uint32 q_FltGGTBMeters; /* GPS to Glonass time bias in meters */ |
||||||
|
uint32 q_FltGGTBSigmaMeters; /* Gaussian 1-sigma value for GPS to Glonass time bias uncertainty in meters */ |
||||||
|
uint32 q_FltGBTBMeters; /* GPS to BeiDou time bias in meters */ |
||||||
|
uint32 q_FltGBTBSigmaMeters; /* Gaussian 1-sigma value for GPS to BeiDou time bias uncertainty in meters */ |
||||||
|
uint32 q_FltBGTBMeters; /* BeiDou to Glonass time bias in meters */ |
||||||
|
uint32 q_FltBGTBSigmaMeters; /* Gaussian 1-sigma value for BeiDou to Glonass time bias uncertainty in meters */ |
||||||
|
uint32 q_FltFiltGGTBMeters; /* Filtered GPS to Glonass time bias in meters */ |
||||||
|
uint32 q_FltFiltGGTBSigmaMeters; /* Filtered Gaussian 1-sigma value for GPS to Glonass time bias uncertainty in meters */ |
||||||
|
uint32 q_FltFiltGBTBMeters; /* Filtered GPS to BeiDou time bias in meters */ |
||||||
|
uint32 q_FltFiltGBTBSigmaMeters; /* Filtered Gaussian 1-sigma value for GPS to BeiDou time bias uncertainty in meters */ |
||||||
|
uint32 q_FltFiltBGTBMeters; /* Filtered BeiDou to Glonass time bias in meters */ |
||||||
|
uint32 q_FltFiltBGTBSigmaMeters; /* Filtered Gaussian 1-sigma value for BeiDou to Glonass time bias uncertainty in meters */ |
||||||
|
uint32 q_FltSftOffsetSec; /* SFT offset as computed by WLS in seconds */ |
||||||
|
uint32 q_FltSftOffsetSigmaSec; /* Gaussian 1-sigma value for SFT offset in seconds */ |
||||||
|
uint32 q_FltClockDriftMps; /* Clock drift (clock frequency bias) in meters per second */ |
||||||
|
uint32 q_FltClockDriftSigmaMps; /* Gaussian 1-sigma value for clock drift in meters per second */ |
||||||
|
uint32 q_FltFilteredAlt; /* Filtered height-above-ellipsoid altitude in meters as computed by WLS */ |
||||||
|
uint32 q_FltFilteredAltSigma; /* Gaussian 1-sigma value for filtered height-above-ellipsoid altitude in meters */ |
||||||
|
uint32 q_FltRawAlt; /* Raw height-above-ellipsoid altitude in meters as computed by WLS */ |
||||||
|
uint32 q_FltRawAltSigma; /* Gaussian 1-sigma value for raw height-above-ellipsoid altitude in meters */ |
||||||
|
uint32 align_Flt[14]; |
||||||
|
uint32 q_FltPdop; /* 3D position dilution of precision as computed from the unweighted |
||||||
|
uint32 q_FltHdop; /* Horizontal position dilution of precision as computed from the unweighted least-squares covariance matrix */ |
||||||
|
uint32 q_FltVdop; /* Vertical position dilution of precision as computed from the unweighted least-squares covariance matrix */ |
||||||
|
uint8 u_EllipseConfidence; /* Statistical measure of the confidence (percentage) associated with the uncertainty ellipse values */ |
||||||
|
uint32 q_FltEllipseAngle; /* Angle of semimajor axis with respect to true North, with increasing angles moving clockwise from North. In units of degrees. */ |
||||||
|
uint32 q_FltEllipseSemimajorAxis; /* Semimajor axis of final horizontal position uncertainty error ellipse. In units of meters. */ |
||||||
|
uint32 q_FltEllipseSemiminorAxis; /* Semiminor axis of final horizontal position uncertainty error ellipse. In units of meters. */ |
||||||
|
uint32 q_FltPosSigmaVertical; /* Gaussian 1-sigma value for final position height-above-ellipsoid altitude in meters */ |
||||||
|
uint8 u_HorizontalReliability; /* Horizontal position reliability 0: Not set 1: Very Low 2: Low 3: Medium 4: High */ |
||||||
|
uint8 u_VerticalReliability; /* Vertical position reliability */ |
||||||
|
uint16 w_Reserved2; /* Reserved memory field */ |
||||||
|
uint32 q_FltGnssHeadingRad; /* User heading in radians derived from GNSS only solution */ |
||||||
|
uint32 q_FltGnssHeadingUncRad; /* User heading uncertainty in radians derived from GNSS only solution */ |
||||||
|
uint32 q_SensorDataUsageMask; /* Denotes which additional sensor data were used to compute this position fix. BIT[0] 0x00000001 <96> Accelerometer BIT[1] 0x00000002 <96> Gyro 0x0000FFFC - Reserved A bit set to 1 indicates that certain fields as defined by the SENSOR_AIDING_MASK were aided with sensor data*/ |
||||||
|
uint32 q_SensorAidMask; /* Denotes which component of the position report was assisted with additional sensors defined in SENSOR_DATA_USAGE_MASK BIT[0] 0x00000001 <96> Heading aided with sensor data BIT[1] 0x00000002 <96> Speed aided with sensor data BIT[2] 0x00000004 <96> Position aided with sensor data BIT[3] 0x00000008 <96> Velocity aided with sensor data 0xFFFFFFF0 <96> Reserved */ |
||||||
|
uint8 u_NumGpsSvsUsed; /* The number of GPS SVs used in the fix */ |
||||||
|
uint8 u_TotalGpsSvs; /* Total number of GPS SVs detected by searcher, including ones not used in position calculation */ |
||||||
|
uint8 u_NumGloSvsUsed; /* The number of Glonass SVs used in the fix */ |
||||||
|
uint8 u_TotalGloSvs; /* Total number of Glonass SVs detected by searcher, including ones not used in position calculation */ |
||||||
|
uint8 u_NumBdsSvsUsed; /* The number of BeiDou SVs used in the fix */ |
||||||
|
uint8 u_TotalBdsSvs; /* Total number of BeiDou SVs detected by searcher, including ones not used in position calculation */ |
||||||
|
""" |
||||||
|
|
||||||
|
def name_to_camelcase(nam): |
||||||
|
ret = [] |
||||||
|
i = 0 |
||||||
|
while i < len(nam): |
||||||
|
if nam[i] == "_": |
||||||
|
ret.append(nam[i+1].upper()) |
||||||
|
i += 2 |
||||||
|
else: |
||||||
|
ret.append(nam[i]) |
||||||
|
i += 1 |
||||||
|
return ''.join(ret) |
||||||
|
|
||||||
|
def parse_struct(ss): |
||||||
|
st = "<" |
||||||
|
nams = [] |
||||||
|
for l in ss.strip().split("\n"): |
||||||
|
typ, nam = l.split(";")[0].split() |
||||||
|
#print(typ, nam) |
||||||
|
if typ == "float" or '_Flt' in nam: |
||||||
|
st += "f" |
||||||
|
elif typ == "double" or '_Dbl' in nam: |
||||||
|
st += "d" |
||||||
|
elif typ in ["uint8", "uint8_t"]: |
||||||
|
st += "B" |
||||||
|
elif typ in ["int8", "int8_t"]: |
||||||
|
st += "b" |
||||||
|
elif typ in ["uint32", "uint32_t"]: |
||||||
|
st += "I" |
||||||
|
elif typ in ["int32", "int32_t"]: |
||||||
|
st += "i" |
||||||
|
elif typ in ["uint16", "uint16_t"]: |
||||||
|
st += "H" |
||||||
|
elif typ in ["int16", "int16_t"]: |
||||||
|
st += "h" |
||||||
|
elif typ == "uint64": |
||||||
|
st += "Q" |
||||||
|
else: |
||||||
|
print("unknown type", typ) |
||||||
|
assert False |
||||||
|
if '[' in nam: |
||||||
|
cnt = int(nam.split("[")[1].split("]")[0]) |
||||||
|
st += st[-1]*(cnt-1) |
||||||
|
for i in range(cnt): |
||||||
|
nams.append("%s[%d]" % (nam.split("[")[0], i)) |
||||||
|
else: |
||||||
|
nams.append(nam) |
||||||
|
return st, nams |
||||||
|
|
||||||
|
def dict_unpacker(ss, camelcase = False): |
||||||
|
st, nams = parse_struct(ss) |
||||||
|
if camelcase: |
||||||
|
nams = [name_to_camelcase(x) for x in nams] |
||||||
|
sz = calcsize(st) |
||||||
|
return lambda x: dict(zip(nams, unpack_from(st, x))), sz |
||||||
Loading…
Reference in new issue