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