sadmen
/
jyoung
mirror of https://github.com/jyoung8607/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
open source driving agent
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
496 Commits
101 Branches
34 Tags
1.6 GiB
 
 
 
 
 
 
Tag: Branch: Tree: d17745fb5e
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'd17745fb5e'
${ noResults }
jyoung/selfdrive/sensord/rawgps.cc

1189 lines
37 KiB
Raw Blame History

#include <cstdio>
#include <cstdlib>
#include <cassert>
#include <csignal>
#include <unistd.h>
#include <semaphore.h>
#include <time.h>
#include <pthread.h>
#include <zmq.h>
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "common/timing.h"
#include "common/util.h"
#include "common/swaglog.h"
#include "libdiag.h"
extern volatile int do_exit;
#define NV_GNSS_OEM_FEATURE_MASK 7165
# define NV_GNSS_OEM_FEATURE_MASK_OEMDRE 1
#define NV_CGPS_DPO_CONTROL 5596
#define DIAG_NV_READ_F 38
#define DIAG_NV_WRITE_F 39
#define DIAG_SUBSYS_CMD 75
#define DIAG_SUBSYS_CMD_VER_2 128
#define DIAG_SUBSYS_GPS 13
#define DIAG_SUBSYS_FS 19
#define CGPS_DIAG_PDAPI_CMD 100
#define CGPS_OEM_CONTROL 202
#define GPSDIAG_OEMFEATURE_DRE 1
#define GPSDIAG_OEM_DRE_ON 1
#define FEATURE_OEMDRE_NOT_SUPPORTED 1
#define FEATURE_OEMDRE_ON 2
#define FEATURE_OEMDRE_ALREADY_ON 4
#define TM_DIAG_NAV_CONFIG_CMD 0x6E
#define EFS2_DIAG_SYNC_NO_WAIT 48
struct __attribute__((packed)) NvPacket {
uint8_t cmd_code;
uint16_t nv_id;
uint8_t data[128];
uint16_t status;
};
enum NvStatus {
NV_DONE,
NV_BUSY,
NV_FULL,
NV_FAIL,
NV_NOTACTIVE,
NV_BADPARAM,
NV_READONLY,
NV_BADRG,
NV_NOMEM,
NV_NOTALLOC,
};
struct __attribute__((packed)) Efs2DiagSyncReq {
uint8_t cmd_code;
uint8_t subsys_id;
uint16_t subsys_cmd_code;
uint16_t sequence_num;
char path[8];
};
struct __attribute__((packed)) Efs2DiagSyncResp {
uint8_t cmd_code;
uint8_t subsys_id;
uint16_t subsys_cmd_code;
uint16_t sequence_num;
uint32_t sync_token;
int32_t diag_errno;
};
struct __attribute__((packed)) GpsOemControlReq {
uint8_t cmd_code;
uint8_t subsys_id;
uint16_t subsys_cmd_code;
uint8_t gps_cmd_code;
uint8_t version;
uint32_t oem_feature;
uint32_t oem_command;
uint32_t reserved[2];
};
struct __attribute__((packed)) GpsOemControlResp {
uint8_t cmd_code;
uint8_t subsys_id;
uint16_t subsys_cmd_code;
uint8_t gps_cmd_code;
uint8_t version;
uint32_t oem_feature;
uint32_t oem_command;
uint32_t resp_result;
uint32_t reserved[2];
};
struct __attribute__((packed)) GpsNavConfigReq {
uint8_t cmd_code;
uint8_t subsys_id;
uint16_t subsys_cmd_code;
uint32_t subsys_status;
uint16_t subsys_delayed_resp_id;
uint16_t subsys_rsp_cnt;
uint8_t desired_config;
};
struct __attribute__((packed)) GpsNavConfigResp {
uint8_t cmd_code;
uint8_t subsys_id;
uint16_t subsys_cmd_code;
uint32_t subsys_status;
uint16_t subsys_delayed_resp_id;
uint16_t subsys_rsp_cnt;
uint8_t supported_config;
uint8_t actual_config;
};
#define LOG_GNSS_POSITION_REPORT 0x1476
#define LOG_GNSS_GPS_MEASUREMENT_REPORT 0x1477
#define LOG_GNSS_CLOCK_REPORT 0x1478
#define LOG_GNSS_GLONASS_MEASUREMENT_REPORT 0x1480
#define LOG_GNSS_BDS_MEASUREMENT_REPORT 0x1756
#define LOG_GNSS_GAL_MEASUREMENT_REPORT 0x1886
#define LOG_GNSS_OEMDRE_MEASUREMENMT_REPORT 0x14DE
#define LOG_GNSS_OEMDRE_SVPOLY_REPORT 0x14E1
#define LOG_GNSS_ME_DPO_STATUS 0x1838
#define LOG_GNSS_CD_DB_REPORT 0x147B
#define LOG_GNSS_PRX_RF_HW_STATUS_REPORT 0x147E
#define LOG_CGPS_SLOW_CLOCK_CLIB_REPORT 0x1488
#define LOG_GNSS_CONFIGURATION_STATE 0x1516
struct __attribute__((packed)) log_header_type {
uint16_t len;
uint16_t code;
uint64_t ts;
};
enum SVObservationStates {
SV_IDLE,
SV_SEARCH,
SV_SEACH_VERIFY,
SV_BIT_EDGE,
SV_TRACK_VERIFY,
SV_TRACK,
SV_RESTART,
SV_DPO,
SV_GLO_10ms_BE,
SV_GLO_10ms_AT,
};
struct __attribute__((packed)) GNSSGpsMeasurementReportv0_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;
};
_Static_assert(sizeof(GNSSGpsMeasurementReportv0_SV) == 70, "error");
struct __attribute__((packed)) GNSSGpsMeasurementReportv0{
log_header_type header;
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;
GNSSGpsMeasurementReportv0_SV sv[];
};
struct __attribute__((packed)) GNSSGlonassMeasurementReportv0_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;
};
_Static_assert(sizeof(GNSSGlonassMeasurementReportv0_SV) == 70, "error");
struct __attribute__((packed)) GNSSGlonassMeasurementReportv0 {
log_header_type header;
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;
GNSSGlonassMeasurementReportv0_SV sv[];
};
struct __attribute__((packed)) GNSSClockReportv2 {
log_header_type header;
uint8_t version;
uint16_t valid_flags;
uint32_t f_count;
uint16_t gps_week;
uint32_t gps_milliseconds;
float gps_time_bias;
float gps_clock_time_uncertainty;
uint8_t gps_clock_source;
uint8_t glonass_year;
uint16_t glonass_day;
uint32_t glonass_milliseconds;
float glonass_time_bias;
float glonass_clock_time_uncertainty;
uint8_t glonass_clock_source;
uint16_t bds_week;
uint32_t bds_milliseconds;
float bds_time_bias;
float bds_clock_time_uncertainty;
uint8_t bds_clock_source;
uint16_t gal_week;
uint32_t gal_milliseconds;
float gal_time_bias;
float gal_clock_time_uncertainty;
uint8_t gal_clock_source;
float clock_frequency_bias;
float clock_frequency_uncertainty;
uint8_t frequency_source;
uint8_t gps_leap_seconds;
uint8_t gps_leap_seconds_uncertainty;
uint8_t gps_leap_seconds_source;
float gps_to_glonass_time_bias_milliseconds;
float gps_to_glonass_time_bias_milliseconds_uncertainty;
float gps_to_bds_time_bias_milliseconds;
float gps_to_bds_time_bias_milliseconds_uncertainty;
float bds_to_glo_time_bias_milliseconds;
float bds_to_glo_time_bias_milliseconds_uncertainty;
float gps_to_gal_time_bias_milliseconds;
float gps_to_gal_time_bias_milliseconds_uncertainty;
float gal_to_glo_time_bias_milliseconds;
float gal_to_glo_time_bias_milliseconds_uncertainty;
float gal_to_bds_time_bias_milliseconds;
float gal_to_bds_time_bias_milliseconds_uncertainty;
uint32_t system_rtc_time;
uint32_t f_count_offset;
uint32_t lpm_rtc_count;
uint32_t clock_resets;
uint32_t reserved[3];
};
enum GNSSMeasurementSource {
SOURCE_GPS,
SOURCE_GLONASS,
SOURCE_BEIDOU,
};
struct __attribute__((packed)) GNSSOemdreMeasurement {
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;
};
_Static_assert(sizeof(GNSSOemdreMeasurement) == 109, "error");
struct __attribute__((packed)) GNSSOemdreMeasurementReportv2 {
log_header_type header;
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;
GNSSOemdreMeasurement measurements[16];
};
_Static_assert(sizeof(GNSSOemdreMeasurementReportv2) == 1851, "error");
struct __attribute__((packed)) GNSSOemdreSVPolyReportv2 {
log_header_type header;
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];
};
_Static_assert(sizeof(GNSSOemdreSVPolyReportv2) == 271, "error");
static void* rawgps_context;
static void *rawgps_publisher;
static int client_id = 0;
static void hexdump(uint8_t* d, size_t len) {
for (int i=0; i<len; i++) {
printf("%02x ", d[i]);
}
printf("\n");
}
static void parse_measurement_status_common(uint32_t measurement_status,
cereal::QcomGnss::MeasurementStatus::Builder status) {
status.setSubMillisecondIsValid(measurement_status & (1 << 0));
status.setSubBitTimeIsKnown(measurement_status & (1 << 1));
status.setSatelliteTimeIsKnown(measurement_status & (1 << 2));
status.setBitEdgeConfirmedFromSignal(measurement_status & (1 << 3));
status.setMeasuredVelocity(measurement_status & (1 << 4));
status.setFineOrCoarseVelocity(measurement_status & (1 << 5));
status.setLockPointValid(measurement_status & (1 << 6));
status.setLockPointPositive(measurement_status & (1 << 7));
status.setLastUpdateFromDifference(measurement_status & (1 << 9));
status.setLastUpdateFromVelocityDifference(measurement_status & (1 << 10));
status.setStrongIndicationOfCrossCorelation(measurement_status & (1 << 11));
status.setTentativeMeasurement(measurement_status & (1 << 12));
status.setMeasurementNotUsable(measurement_status & (1 << 13));
status.setSirCheckIsNeeded(measurement_status & (1 << 14));
status.setProbationMode(measurement_status & (1 << 15));
status.setMultipathIndicator(measurement_status & (1 << 24));
status.setImdJammingIndicator(measurement_status & (1 << 25));
status.setLteB13TxJammingIndicator(measurement_status & (1 << 26));
status.setFreshMeasurementIndicator(measurement_status & (1 << 27));
}
static void handle_log(uint8_t *ptr, int len) {
assert(len >= sizeof(log_header_type)+1);
log_header_type* log_header = (log_header_type*)ptr;
uint8_t* log_data = ptr + sizeof(log_header_type);
#ifdef RAWGPS_TEST
printf("%04x\n", log_header->code);
#endif
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto qcomGnss = event.initQcomGnss();
qcomGnss.setLogTs(log_header->ts);
switch (log_header->code) {
case LOG_GNSS_CLOCK_REPORT: {
uint8_t version = log_data[0];
assert(version == 2);
assert(len >= sizeof(GNSSClockReportv2));
const GNSSClockReportv2* report = (const GNSSClockReportv2*)ptr;
auto lreport = qcomGnss.initClockReport();
lreport.setHasFCount(report->valid_flags & (1 << 0));
lreport.setFCount(report->f_count);
lreport.setHasGpsWeek(report->valid_flags & (1 << 2));
lreport.setGpsWeek(report->gps_week);
lreport.setHasGpsMilliseconds(report->valid_flags & (1 << 1));
lreport.setGpsMilliseconds(report->gps_milliseconds);
lreport.setGpsTimeBias(report->gps_time_bias);
lreport.setGpsClockTimeUncertainty(report->gps_clock_time_uncertainty);
lreport.setGpsClockSource(report->gps_clock_source);
lreport.setHasGlonassYear(report->valid_flags & (1 << 6));
lreport.setGlonassYear(report->glonass_year);
lreport.setHasGlonassDay(report->valid_flags & (1 << 5));
lreport.setGlonassDay(report->glonass_day);
lreport.setHasGlonassMilliseconds(report->valid_flags & (1 << 4));
lreport.setGlonassMilliseconds(report->glonass_milliseconds);
lreport.setGlonassTimeBias(report->glonass_time_bias);
lreport.setGlonassClockTimeUncertainty(report->glonass_clock_time_uncertainty);
lreport.setGlonassClockSource(report->glonass_clock_source);
lreport.setBdsWeek(report->bds_week);
lreport.setBdsMilliseconds(report->bds_milliseconds);
lreport.setBdsTimeBias(report->bds_time_bias);
lreport.setBdsClockTimeUncertainty(report->bds_clock_time_uncertainty);
lreport.setBdsClockSource(report->bds_clock_source);
lreport.setGalWeek(report->gal_week);
lreport.setGalMilliseconds(report->gal_milliseconds);
lreport.setGalTimeBias(report->gal_time_bias);
lreport.setGalClockTimeUncertainty(report->gal_clock_time_uncertainty);
lreport.setGalClockSource(report->gal_clock_source);
lreport.setClockFrequencyBias(report->clock_frequency_bias);
lreport.setClockFrequencyUncertainty(report->clock_frequency_uncertainty);
lreport.setFrequencySource(report->frequency_source);
lreport.setGpsLeapSeconds(report->gps_leap_seconds);
lreport.setGpsLeapSecondsUncertainty(report->gps_leap_seconds_uncertainty);
lreport.setGpsLeapSecondsSource(report->gps_leap_seconds_source);
lreport.setGpsToGlonassTimeBiasMilliseconds(report->gps_to_glonass_time_bias_milliseconds);
lreport.setGpsToGlonassTimeBiasMillisecondsUncertainty(report->gps_to_glonass_time_bias_milliseconds_uncertainty);
lreport.setGpsToBdsTimeBiasMilliseconds(report->gps_to_bds_time_bias_milliseconds);
lreport.setGpsToBdsTimeBiasMillisecondsUncertainty(report->gps_to_bds_time_bias_milliseconds_uncertainty);
lreport.setBdsToGloTimeBiasMilliseconds(report->bds_to_glo_time_bias_milliseconds);
lreport.setBdsToGloTimeBiasMillisecondsUncertainty(report->bds_to_glo_time_bias_milliseconds_uncertainty);
lreport.setGpsToGalTimeBiasMilliseconds(report->gps_to_gal_time_bias_milliseconds);
lreport.setGpsToGalTimeBiasMillisecondsUncertainty(report->gps_to_gal_time_bias_milliseconds_uncertainty);
lreport.setGalToGloTimeBiasMilliseconds(report->gal_to_glo_time_bias_milliseconds);
lreport.setGalToGloTimeBiasMillisecondsUncertainty(report->gal_to_glo_time_bias_milliseconds_uncertainty);
lreport.setGalToBdsTimeBiasMilliseconds(report->gal_to_bds_time_bias_milliseconds);
lreport.setGalToBdsTimeBiasMillisecondsUncertainty(report->gal_to_bds_time_bias_milliseconds_uncertainty);
lreport.setHasRtcTime(report->valid_flags & (1 << 3));
lreport.setSystemRtcTime(report->system_rtc_time);
lreport.setFCountOffset(report->f_count_offset);
lreport.setLpmRtcCount(report->lpm_rtc_count);
lreport.setClockResets(report->clock_resets);
break;
}
case LOG_GNSS_GPS_MEASUREMENT_REPORT: {
uint8_t version = log_data[0];
assert(version == 0);
assert(len >= sizeof(GNSSGpsMeasurementReportv0));
const GNSSGpsMeasurementReportv0* report = (const GNSSGpsMeasurementReportv0*)ptr;
assert(len >= sizeof(sizeof(GNSSGpsMeasurementReportv0))+sizeof(GNSSGpsMeasurementReportv0_SV) * report->sv_count);
auto lreport = qcomGnss.initMeasurementReport();
lreport.setSource(cereal::QcomGnss::MeasurementSource::GPS);
lreport.setFCount(report->f_count);
lreport.setGpsWeek(report->week);
lreport.setMilliseconds(report->milliseconds);
lreport.setTimeBias(report->time_bias);
lreport.setClockTimeUncertainty(report->clock_time_uncertainty);
lreport.setClockFrequencyBias(report->clock_frequency_bias);
lreport.setClockFrequencyUncertainty(report->clock_frequency_uncertainty);
auto lsvs = lreport.initSv(report->sv_count);
for (int i=0; i<report->sv_count; i++) {
auto lsv = lsvs[i];
const GNSSGpsMeasurementReportv0_SV *sv = &report->sv[i];
lsv.setSvId(sv->sv_id);
lsv.setObservationState(cereal::QcomGnss::SVObservationState(sv->observation_state));
lsv.setObservations(sv->observations);
lsv.setGoodObservations(sv->good_observations);
lsv.setGpsParityErrorCount(sv->parity_error_count);
lsv.setFilterStages(sv->filter_stages);
lsv.setCarrierNoise(sv->carrier_noise);
lsv.setLatency(sv->latency);
lsv.setPredetectInterval(sv->predetect_interval);
lsv.setPostdetections(sv->postdetections);
lsv.setUnfilteredMeasurementIntegral(sv->unfiltered_measurement_integral);
lsv.setUnfilteredMeasurementFraction(sv->unfiltered_measurement_fraction);
lsv.setUnfilteredTimeUncertainty(sv->unfiltered_time_uncertainty);
lsv.setUnfilteredSpeed(sv->unfiltered_speed);
lsv.setUnfilteredSpeedUncertainty(sv->unfiltered_speed_uncertainty);
lsv.setMultipathEstimate(sv->multipath_estimate);
lsv.setAzimuth(sv->azimuth);
lsv.setElevation(sv->elevation);
lsv.setCarrierPhaseCyclesIntegral(sv->carrier_phase_cycles_integral);
lsv.setCarrierPhaseCyclesFraction(sv->carrier_phase_cycles_fraction);
lsv.setFineSpeed(sv->fine_speed);
lsv.setFineSpeedUncertainty(sv->fine_speed_uncertainty);
lsv.setCycleSlipCount(sv->cycle_slip_count);
auto status = lsv.initMeasurementStatus();
parse_measurement_status_common(sv->measurement_status, status);
status.setGpsRoundRobinRxDiversity(sv->measurement_status & (1 << 18));
status.setGpsRxDiversity(sv->measurement_status & (1 << 19));
status.setGpsLowBandwidthRxDiversityCombined(sv->measurement_status & (1 << 20));
status.setGpsHighBandwidthNu4(sv->measurement_status & (1 << 21));
status.setGpsHighBandwidthNu8(sv->measurement_status & (1 << 22));
status.setGpsHighBandwidthUniform(sv->measurement_status & (1 << 23));
status.setMultipathEstimateIsValid(sv->misc_status & (1 << 0));
status.setDirectionIsValid(sv->misc_status & (1 << 1));
#ifdef RAWGPS_TEST
// if (sv->measurement_status & (1 << 27)) printf("%d\n", sv->unfiltered_measurement_integral);
printf("GPS %03d %d %d 0x%08X o: %02x go: %02x fs: %02x cn: %02x pd: %02x cs: %02x po: %02x ms: %08x ms2: %08x me: %08x az: %08x el: %08x fc: %08x\n",
sv->sv_id,
!!(sv->measurement_status & (1 << 27)),
sv->unfiltered_measurement_integral, sv->unfiltered_measurement_integral,
sv->observations,
sv->good_observations,
sv->filter_stages,
sv->carrier_noise,
sv->predetect_interval,
sv->cycle_slip_count,
sv->postdetections,
sv->measurement_status,
sv->misc_status,
sv->multipath_estimate,
*(uint32_t*)&sv->azimuth,
*(uint32_t*)&sv->elevation,
report->f_count
);
#endif
}
break;
}
case LOG_GNSS_GLONASS_MEASUREMENT_REPORT: {
uint8_t version = log_data[0];
assert(version == 0);
assert(len >= sizeof(GNSSGlonassMeasurementReportv0));
const GNSSGlonassMeasurementReportv0* report = (const GNSSGlonassMeasurementReportv0*)ptr;
auto lreport = qcomGnss.initMeasurementReport();
lreport.setSource(cereal::QcomGnss::MeasurementSource::GLONASS);
lreport.setFCount(report->f_count);
lreport.setGlonassCycleNumber(report->glonass_cycle_number);
lreport.setGlonassNumberOfDays(report->glonass_number_of_days);
lreport.setMilliseconds(report->milliseconds);
lreport.setTimeBias(report->time_bias);
lreport.setClockTimeUncertainty(report->clock_time_uncertainty);
lreport.setClockFrequencyBias(report->clock_frequency_bias);
lreport.setClockFrequencyUncertainty(report->clock_frequency_uncertainty);
auto lsvs = lreport.initSv(report->sv_count);
for (int i=0; i<report->sv_count; i++) {
auto lsv = lsvs[i];
const GNSSGlonassMeasurementReportv0_SV *sv = &report->sv[i];
lsv.setSvId(sv->sv_id);
lsv.setObservationState(cereal::QcomGnss::SVObservationState(sv->observation_state));
lsv.setObservations(sv->observations);
lsv.setGoodObservations(sv->good_observations);
lsv.setGlonassFrequencyIndex(sv->frequency_index);
lsv.setGlonassHemmingErrorCount(sv->hemming_error_count);
lsv.setFilterStages(sv->filter_stages);
lsv.setCarrierNoise(sv->carrier_noise);
lsv.setLatency(sv->latency);
lsv.setPredetectInterval(sv->predetect_interval);
lsv.setPostdetections(sv->postdetections);
lsv.setUnfilteredMeasurementIntegral(sv->unfiltered_measurement_integral);
lsv.setUnfilteredMeasurementFraction(sv->unfiltered_measurement_fraction);
lsv.setUnfilteredTimeUncertainty(sv->unfiltered_time_uncertainty);
lsv.setUnfilteredSpeed(sv->unfiltered_speed);
lsv.setUnfilteredSpeedUncertainty(sv->unfiltered_speed_uncertainty);
lsv.setMultipathEstimate(sv->multipath_estimate);
lsv.setAzimuth(sv->azimuth);
lsv.setElevation(sv->elevation);
lsv.setCarrierPhaseCyclesIntegral(sv->carrier_phase_cycles_integral);
lsv.setCarrierPhaseCyclesFraction(sv->carrier_phase_cycles_fraction);
lsv.setFineSpeed(sv->fine_speed);
lsv.setFineSpeedUncertainty(sv->fine_speed_uncertainty);
lsv.setCycleSlipCount(sv->cycle_slip_count);
auto status = lsv.initMeasurementStatus();
parse_measurement_status_common(sv->measurement_status, status);
status.setGlonassMeanderBitEdgeValid(sv->measurement_status & (1 << 16));
status.setGlonassTimeMarkValid(sv->measurement_status & (1 << 17));
status.setMultipathEstimateIsValid(sv->misc_status & (1 << 0));
status.setDirectionIsValid(sv->misc_status & (1 << 1));
#ifdef RAWGPS_TEST
// if (sv->measurement_status & (1 << 27)) printf("%d\n", sv->unfiltered_measurement_integral);
printf("GLO %03d %02x %d %d 0x%08X o: %02x go: %02x fs: %02x cn: %02x pd: %02x cs: %02x po: %02x ms: %08x ms2: %08x me: %08x az: %08x el: %08x fc: %08x\n",
sv->sv_id, sv->frequency_index & 0xff,
!!(sv->measurement_status & (1 << 27)),
sv->unfiltered_measurement_integral, sv->unfiltered_measurement_integral,
sv->observations,
sv->good_observations,
sv->filter_stages,
sv->carrier_noise,
sv->predetect_interval,
sv->cycle_slip_count,
sv->postdetections,
sv->measurement_status,
sv->misc_status,
sv->multipath_estimate,
*(uint32_t*)&sv->azimuth,
*(uint32_t*)&sv->elevation,
report->f_count
);
#endif
}
break;
}
case LOG_GNSS_OEMDRE_MEASUREMENMT_REPORT: {
// hexdump(ptr, len);
uint8_t version = log_data[0];
assert(version == 2);
assert(len >= sizeof(GNSSOemdreMeasurementReportv2));
const GNSSOemdreMeasurementReportv2* report = (const GNSSOemdreMeasurementReportv2*)ptr;
auto lreport = qcomGnss.initDrMeasurementReport();
lreport.setReason(report->reason);
lreport.setSeqNum(report->seq_num);
lreport.setSeqMax(report->seq_max);
lreport.setRfLoss(report->rf_loss);
lreport.setSystemRtcValid(report->system_rtc_valid);
lreport.setFCount(report->f_count);
lreport.setClockResets(report->clock_resets);
lreport.setSystemRtcTime(report->system_rtc_time);
lreport.setGpsLeapSeconds(report->gps_leap_seconds);
lreport.setGpsLeapSecondsUncertainty(report->gps_leap_seconds_uncertainty);
lreport.setGpsToGlonassTimeBiasMilliseconds(report->gps_to_glonass_time_bias_milliseconds);
lreport.setGpsToGlonassTimeBiasMillisecondsUncertainty(report->gps_to_glonass_time_bias_milliseconds_uncertainty);
lreport.setGpsWeek(report->gps_week);
lreport.setGpsMilliseconds(report->gps_milliseconds);
lreport.setGpsTimeBiasMs(report->gps_time_bias);
lreport.setGpsClockTimeUncertaintyMs(report->gps_clock_time_uncertainty);
lreport.setGpsClockSource(report->gps_clock_source);
lreport.setGlonassClockSource(report->glonass_clock_source);
lreport.setGlonassYear(report->glonass_year);
lreport.setGlonassDay(report->glonass_day);
lreport.setGlonassMilliseconds(report->glonass_milliseconds);
lreport.setGlonassTimeBias(report->glonass_time_bias);
lreport.setGlonassClockTimeUncertainty(report->glonass_clock_time_uncertainty);
lreport.setClockFrequencyBias(report->clock_frequency_bias);
lreport.setClockFrequencyUncertainty(report->clock_frequency_uncertainty);
lreport.setFrequencySource(report->frequency_source);
lreport.setSource(cereal::QcomGnss::MeasurementSource(report->source));
auto lsvs = lreport.initSv(report->sv_count);
// for (int i=0; i<report->sv_count; i++) {
// GNSSOemdreMeasurement *sv = &report->gps[i];
// if (!(sv->measurement_status & (1 << 27))) continue;
// printf("oemdre %03d %d %f\n", sv->sv_id, sv->unfiltered_measurement_integral, sv->unfiltered_measurement_fraction);
// }
for (int i=0; i<report->sv_count; i++) {
auto lsv = lsvs[i];
const GNSSOemdreMeasurement *sv = &report->measurements[i];
lsv.setSvId(sv->sv_id);
lsv.setGlonassFrequencyIndex(sv->glonass_frequency_index);
lsv.setObservationState(cereal::QcomGnss::SVObservationState(sv->observation_state));
lsv.setObservations(sv->observations);
lsv.setGoodObservations(sv->good_observations);
lsv.setFilterStages(sv->filter_stages);
lsv.setPredetectInterval(sv->predetect_interval);
lsv.setCycleSlipCount(sv->cycle_slip_count);
lsv.setPostdetections(sv->postdetections);
auto status = lsv.initMeasurementStatus();
parse_measurement_status_common(sv->measurement_status, status);
status.setMultipathEstimateIsValid(sv->multipath_estimate_valid);
status.setDirectionIsValid(sv->direction_valid);
lsv.setCarrierNoise(sv->carrier_noise);
lsv.setRfLoss(sv->rf_loss);
lsv.setLatency(sv->latency);
lsv.setFilteredMeasurementFraction(sv->filtered_measurement_fraction);
lsv.setFilteredMeasurementIntegral(sv->filtered_measurement_integral);
lsv.setFilteredTimeUncertainty(sv->filtered_time_uncertainty);
lsv.setFilteredSpeed(sv->filtered_speed);
lsv.setFilteredSpeedUncertainty(sv->filtered_speed_uncertainty);
lsv.setUnfilteredMeasurementFraction(sv->unfiltered_measurement_fraction);
lsv.setUnfilteredMeasurementIntegral(sv->unfiltered_measurement_integral);
lsv.setUnfilteredTimeUncertainty(sv->unfiltered_time_uncertainty);
lsv.setUnfilteredSpeed(sv->unfiltered_speed);
lsv.setUnfilteredSpeedUncertainty(sv->unfiltered_speed_uncertainty);
lsv.setMultipathEstimate(sv->multipath_estimate);
lsv.setAzimuth(sv->azimuth);
lsv.setElevation(sv->elevation);
lsv.setDopplerAcceleration(sv->doppler_acceleration);
lsv.setFineSpeed(sv->fine_speed);
lsv.setFineSpeedUncertainty(sv->fine_speed_uncertainty);
lsv.setCarrierPhase(sv->carrier_phase);
lsv.setFCount(sv->f_count);
lsv.setParityErrorCount(sv->parity_error_count);
lsv.setGoodParity(sv->good_parity);
}
break;
}
case LOG_GNSS_OEMDRE_SVPOLY_REPORT: {
uint8_t version = log_data[0];
assert(version == 2);
assert(len >= sizeof(GNSSOemdreSVPolyReportv2));
const GNSSOemdreSVPolyReportv2* report = (const GNSSOemdreSVPolyReportv2*)ptr;
auto lreport = qcomGnss.initDrSvPoly();
lreport.setSvId(report->sv_id);
lreport.setFrequencyIndex(report->frequency_index);
lreport.setHasPosition(report->flags & 1);
lreport.setHasIono(report->flags & 2);
lreport.setHasTropo(report->flags & 4);
lreport.setHasElevation(report->flags & 8);
lreport.setPolyFromXtra(report->flags & 16);
lreport.setHasSbasIono(report->flags & 32);
lreport.setIode(report->iode);
lreport.setT0(report->t0);
kj::ArrayPtr<const double> xyz0(report->xyz0, 3);
lreport.setXyz0(xyz0);
kj::ArrayPtr<const double> xyzN(report->xyzN, 9);
lreport.setXyzN(xyzN);
kj::ArrayPtr<const float> other(report->other, 4);
lreport.setOther(other);
lreport.setPositionUncertainty(report->position_uncertainty);
lreport.setIonoDelay(report->iono_delay);
lreport.setIonoDot(report->iono_dot);
lreport.setSbasIonoDelay(report->sbas_iono_delay);
lreport.setSbasIonoDot(report->sbas_iono_dot);
lreport.setTropoDelay(report->tropo_delay);
lreport.setElevation(report->elevation);
lreport.setElevationDot(report->elevation_dot);
lreport.setElevationUncertainty(report->elevation_uncertainty);
kj::ArrayPtr<const double> velocity_coeff(report->velocity_coeff, 12);
lreport.setVelocityCoeff(velocity_coeff);
break;
}
default:
// printf("%04x\n", log_header->code);
// hexdump(ptr, len);
qcomGnss.setRawLog(kj::arrayPtr(ptr, len));
break;
}
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(rawgps_publisher, bytes.begin(), bytes.size(), 0);
}
static void handle_event(unsigned char *ptr, int len) {
// printf("EVENT\n");
}
static uint16_t log_codes[] = {
LOG_GNSS_CLOCK_REPORT,
LOG_GNSS_GPS_MEASUREMENT_REPORT,
LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
LOG_GNSS_OEMDRE_MEASUREMENMT_REPORT,
LOG_GNSS_OEMDRE_SVPOLY_REPORT,
// unparsed:
LOG_GNSS_POSITION_REPORT,
LOG_GNSS_BDS_MEASUREMENT_REPORT, // these are missing by might as well try to catch them anyway
LOG_GNSS_GAL_MEASUREMENT_REPORT,
LOG_GNSS_ME_DPO_STATUS,
LOG_GNSS_CD_DB_REPORT,
LOG_GNSS_PRX_RF_HW_STATUS_REPORT,
LOG_CGPS_SLOW_CLOCK_CLIB_REPORT,
LOG_GNSS_CONFIGURATION_STATE,
};
struct SendDiagSyncState {
sem_t sem;
int len;
};
static void diag_send_sync_cb(unsigned char *ptr, int len, void *data_ptr) {
SendDiagSyncState *s = (SendDiagSyncState*)data_ptr;
s->len = len;
sem_post(&s->sem);
}
static int diag_send_sync(int client_id, unsigned char* req_pkt, size_t pkt_len,
unsigned char* res_pkt, size_t res_pkt_size) {
SendDiagSyncState s = {0};
sem_init(&s.sem, 0, 0);
int err = diag_send_dci_async_req(client_id, req_pkt, pkt_len, res_pkt, res_pkt_size,
diag_send_sync_cb, &s);
assert(err == DIAG_DCI_NO_ERROR);
sem_wait(&s.sem);
return s.len;
}
static int oemdre_on(int client_id) {
// enable OEM DR
unsigned char res_pkt[DIAG_MAX_RX_PKT_SIZ];
GpsOemControlReq req_pkt = {
.cmd_code = DIAG_SUBSYS_CMD,
.subsys_id = DIAG_SUBSYS_GPS,
.subsys_cmd_code = CGPS_DIAG_PDAPI_CMD,
.gps_cmd_code = CGPS_OEM_CONTROL,
.version = 1,
.oem_feature = GPSDIAG_OEMFEATURE_DRE,
.oem_command = GPSDIAG_OEM_DRE_ON,
};
int res_len = diag_send_sync(client_id, (unsigned char*)&req_pkt, sizeof(req_pkt),
res_pkt, sizeof(res_pkt));
GpsOemControlResp *resp = (GpsOemControlResp*)res_pkt;
if (res_len != sizeof(GpsOemControlResp)
|| resp->cmd_code != DIAG_SUBSYS_CMD
|| resp->subsys_id != DIAG_SUBSYS_GPS
|| resp->subsys_cmd_code != CGPS_DIAG_PDAPI_CMD
|| resp->gps_cmd_code != CGPS_OEM_CONTROL
|| resp->oem_feature != GPSDIAG_OEMFEATURE_DRE
|| resp->oem_command != GPSDIAG_OEM_DRE_ON) {
LOGW("oemdre_on: bad response!");
return -1;
}
return resp->resp_result;
}
static void efs_sync(int client_id) {
unsigned char res_pkt[DIAG_MAX_RX_PKT_SIZ];
Efs2DiagSyncReq req_pkt = {
.cmd_code = DIAG_SUBSYS_CMD,
.subsys_id = DIAG_SUBSYS_FS,
.subsys_cmd_code = EFS2_DIAG_SYNC_NO_WAIT,
.sequence_num = (uint16_t)(rand() % 100),
};
req_pkt.path[0] = '/';
req_pkt.path[1] = 0;
int res_len = diag_send_sync(client_id, (unsigned char*)&req_pkt, sizeof(req_pkt),
res_pkt, sizeof(res_pkt));
Efs2DiagSyncResp *resp = (Efs2DiagSyncResp*)res_pkt;
if (res_len != sizeof(Efs2DiagSyncResp)
|| resp->cmd_code != DIAG_SUBSYS_CMD
|| resp->subsys_id != DIAG_SUBSYS_FS
|| resp->subsys_cmd_code != EFS2_DIAG_SYNC_NO_WAIT) {
LOGW("efs_sync: bad response!");
return;
}
if (resp->diag_errno != 0) {
LOGW("efs_sync: error %d", resp->diag_errno);
}
}
static uint32_t nv_read_u32(int client_id, uint16_t nv_id) {
NvPacket req = {
.cmd_code = DIAG_NV_READ_F,
.nv_id = nv_id,
};
NvPacket resp = {0};
int res_len = diag_send_sync(client_id, (unsigned char*)&req, sizeof(req),
(unsigned char*)&resp, sizeof(resp));
// hexdump((uint8_t*)&resp, res_len);
if (resp.cmd_code != DIAG_NV_READ_F
|| resp.nv_id != nv_id) {
LOGW("nv_read_u32: diag command failed");
return 0;
}
if (resp.status != NV_DONE) {
LOGW("nv_read_u32: read failed: %d", resp.status);
return 0;
}
return *(uint32_t*)resp.data;
}
static bool nv_write_u32(int client_id, uint16_t nv_id, uint32_t val) {
NvPacket req = {
.cmd_code = DIAG_NV_WRITE_F,
.nv_id = nv_id,
};
*(uint32_t*)req.data = val;
NvPacket resp = {0};
int res_len = diag_send_sync(client_id, (unsigned char*)&req, sizeof(req),
(unsigned char*)&resp, sizeof(resp));
// hexdump((uint8_t*)&resp, res_len);
if (resp.cmd_code != DIAG_NV_WRITE_F
|| resp.nv_id != nv_id) {
LOGW("nv_write_u32: diag command failed");
return false;
}
if (resp.status != NV_DONE) {
LOGW("nv_write_u32: write failed: %d", resp.status);
return false;
}
return true;
}
static void nav_config(int client_id, uint8_t config) {
unsigned char res_pkt[DIAG_MAX_RX_PKT_SIZ];
GpsNavConfigReq req_pkt = {
.cmd_code = DIAG_SUBSYS_CMD_VER_2,
.subsys_id = DIAG_SUBSYS_GPS,
.subsys_cmd_code = TM_DIAG_NAV_CONFIG_CMD,
.desired_config = config,
};
int res_len = diag_send_sync(client_id, (unsigned char*)&req_pkt, sizeof(req_pkt),
res_pkt, sizeof(res_pkt));
GpsNavConfigResp *resp = (GpsNavConfigResp*)res_pkt;
if (res_len != sizeof(GpsNavConfigResp)
|| resp->cmd_code != DIAG_SUBSYS_CMD_VER_2
|| resp->subsys_id != DIAG_SUBSYS_GPS
|| resp->subsys_cmd_code != TM_DIAG_NAV_CONFIG_CMD) {
LOGW("nav_config: bad response!");
return;
}
LOG("nav config: %04x %04x", resp->supported_config, resp->actual_config);
}
void rawgps_init() {
int err;
rawgps_context = zmq_ctx_new();
rawgps_publisher = zmq_socket(rawgps_context, ZMQ_PUB);
zmq_bind(rawgps_publisher, "tcp://*:8029");
bool init_success = Diag_LSM_Init(NULL);
assert(init_success);
uint16_t list = DIAG_CON_APSS | DIAG_CON_MPSS;
int signal_type = SIGCONT;
err = diag_register_dci_client(&client_id, &list, 0, &signal_type);
assert(err == DIAG_DCI_NO_ERROR);
{
uint32_t oem_features = nv_read_u32(client_id, NV_GNSS_OEM_FEATURE_MASK);
LOG("oem features: %08x", oem_features);
if (!(oem_features & NV_GNSS_OEM_FEATURE_MASK_OEMDRE)) {
LOG("OEMDRE feature disabled, enabling...");
nv_write_u32(client_id, NV_GNSS_OEM_FEATURE_MASK, NV_GNSS_OEM_FEATURE_MASK_OEMDRE);
efs_sync(client_id);
}
int oemdre_status = oemdre_on(client_id);
LOG("oemdre status: %d", oemdre_status);
}
{
// make sure GNSS duty cycling is off
uint32_t dpo = nv_read_u32(client_id, NV_CGPS_DPO_CONTROL);
LOG("dpo: %d", dpo);
if (dpo != 0) {
nv_write_u32(client_id, NV_CGPS_DPO_CONTROL, 0);
efs_sync(client_id);
}
}
// enable beidou
// nav_config(client_id, 0x13); // 0b10011
err = diag_register_dci_stream(handle_log, handle_event);
assert(err == DIAG_DCI_NO_ERROR);
err = diag_log_stream_config(client_id, true, log_codes, ARRAYSIZE(log_codes));
assert(err == DIAG_DCI_NO_ERROR);
}
void rawgps_destroy() {
int err;
err = diag_log_stream_config(client_id, false, log_codes, ARRAYSIZE(log_codes));
assert(err == DIAG_DCI_NO_ERROR);
err = diag_release_dci_client(&client_id);
assert(err == DIAG_DCI_NO_ERROR);
Diag_LSM_DeInit();
zmq_close(rawgps_publisher);
zmq_term(rawgps_context);
}
#ifdef RAWGPS_TEST
int main() {
int err = 0;
rawgps_init();
while(1) {
usleep(100000);
}
rawgps_destroy();
return 0;
}
#endif