mawei
/
openpilot0
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
openpilot0/system/qcomgpsd/structs.py

354 lines
15 KiB
Python
Raw Blame History

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
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_svpoly_report = """
uint8_t version;
uint16_t sv_id;
int8_t frequency_index;
uint8_t flags;
uint16_t iode;
double t0;
double xyz0[3];
double xyzN[9];
float other[4];
float position_uncertainty;
float iono_delay;
float iono_dot;
float sbas_iono_delay;
float sbas_iono_dot;
float tropo_delay;
float elevation;
float elevation_dot;
float elevation_uncertainty;
double velocity_coeff[12];
"""
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;
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; // SV PRN
uint8_t observation_state; // SV Observation state
uint8_t observations; // Count of all observation (both success and failure)
uint8_t good_observations; // Count of Good observations
uint16_t parity_error_count; // Carrier to Code filtering N count
uint8_t filter_stages; // Pre-Detection (Coherent) Interval (msecs)
uint16_t carrier_noise; // CNo. Units of 0.1 dB
int16_t latency; // Age of the measurement in msecs (+ve meas Meas precedes ref time)
uint8_t predetect_interval; // Pre-Detection (Coherent) Interval (msecs)
uint16_t postdetections; // Num Post-Detections (uints of PreInts
uint32_t unfiltered_measurement_integral; // Range of 0 thru (WEEK_MSECS-1) [msecs]
float unfiltered_measurement_fraction; // Range of 0 thru 0.99999 [msecs]
float unfiltered_time_uncertainty; // Time uncertainty (msec)
float unfiltered_speed; // Speed estimate (meters/sec)
float unfiltered_speed_uncertainty; // Speed uncertainty estimate (meters/sec)
uint32_t measurement_status;
uint8_t misc_status;
uint32_t multipath_estimate;
float azimuth; // Azimuth (radians)
float elevation; // Elevation (radians)
int32_t carrier_phase_cycles_integral;
uint16_t carrier_phase_cycles_fraction;
float fine_speed; // Carrier phase derived speed
float fine_speed_uncertainty; // Carrier phase derived speed UNC
uint8_t cycle_slip_count; // Increments when a CSlip is detected
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 */
""" # noqa: E501
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"):
if len(l.strip()) == 0:
continue
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 in ["uint64", "uint64_t"]:
st += "Q"
else:
raise RuntimeError(f"unknown type {typ}")
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), strict=True)), sz
def relist(dat):
list_keys = set()
for key in dat.keys():
if '[' in key:
list_keys.add(key.split('[')[0])
list_dict = {}
for list_key in list_keys:
list_dict[list_key] = []
i = 0
while True:
key = list_key + f'[{i}]'
if key not in dat:
break
list_dict[list_key].append(dat[key])
del dat[key]
i += 1
return {**dat, **list_dict}
Reference in New Issue View Git Blame Copy Permalink