From f8d22a91be7521ace57dbe9e06847302dbca28de Mon Sep 17 00:00:00 2001 From: George Hotz Date: Fri, 17 Jan 2020 11:17:58 -0800 Subject: [PATCH] selfdrive/sensord old-commit-hash: 5c9afcc785ac971c13662911707b4f685047a28c --- selfdrive/sensord/SConscript | 5 + selfdrive/sensord/gpsd | 3 + selfdrive/sensord/gpsd.cc | 201 ++++++ selfdrive/sensord/libdiag.h | 40 ++ selfdrive/sensord/rawgps.cc | 1184 ++++++++++++++++++++++++++++++++++ selfdrive/sensord/rawgps.h | 7 + selfdrive/sensord/sensord | 3 + selfdrive/sensord/sensors.cc | 245 +++++++ 8 files changed, 1688 insertions(+) create mode 100644 selfdrive/sensord/SConscript create mode 100755 selfdrive/sensord/gpsd create mode 100644 selfdrive/sensord/gpsd.cc create mode 100644 selfdrive/sensord/libdiag.h create mode 100644 selfdrive/sensord/rawgps.cc create mode 100644 selfdrive/sensord/rawgps.h create mode 100755 selfdrive/sensord/sensord create mode 100644 selfdrive/sensord/sensors.cc diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript new file mode 100644 index 0000000000..b285cf9754 --- /dev/null +++ b/selfdrive/sensord/SConscript @@ -0,0 +1,5 @@ +Import('env', 'common', 'messaging') +env.Program('_sensord', 'sensors.cc', LIBS=['hardware', common, 'json', messaging, 'capnp', 'zmq', 'kj']) +lenv = env.Clone() +lenv['LIBPATH'] += ['/system/vendor/lib64'] +lenv.Program('_gpsd', ['gpsd.cc', 'rawgps.cc'], LIBS=['hardware', common, 'diag', 'time_genoff', 'json', messaging, 'capnp', 'zmq', 'kj']) diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd new file mode 100755 index 0000000000..97d5a1467a --- /dev/null +++ b/selfdrive/sensord/gpsd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:35038a572308e61aa56a483b0edffbdc64322c04911a8422669c52a6fbe76a86 +size 79 diff --git a/selfdrive/sensord/gpsd.cc b/selfdrive/sensord/gpsd.cc new file mode 100644 index 0000000000..ae2ecec035 --- /dev/null +++ b/selfdrive/sensord/gpsd.cc @@ -0,0 +1,201 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include + +#include + +#include "messaging.hpp" +#include "common/timing.h" +#include "common/swaglog.h" + +#include "cereal/gen/cpp/log.capnp.h" + +#include "rawgps.h" + +volatile sig_atomic_t do_exit = 0; + +namespace { + +Context *gps_context; +PubSocket *gps_publisher; +PubSocket *gps_location_publisher; + +const GpsInterface* gGpsInterface = NULL; +const AGpsInterface* gAGpsInterface = NULL; + +void set_do_exit(int sig) { + do_exit = 1; +} + +void nmea_callback(GpsUtcTime timestamp, const char* nmea, int length) { + + uint64_t log_time = nanos_since_boot(); + uint64_t log_time_wall = nanos_since_epoch(); + + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(log_time); + + auto nmeaData = event.initGpsNMEA(); + nmeaData.setTimestamp(timestamp); + nmeaData.setLocalWallTime(log_time_wall); + nmeaData.setNmea(nmea); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + // printf("gps send %d\n", bytes.size()); + gps_publisher->send((char*)bytes.begin(), bytes.size()); +} + +void location_callback(GpsLocation* location) { + //printf("got location callback\n"); + uint64_t log_time = nanos_since_boot(); + + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(log_time); + + auto locationData = event.initGpsLocation(); + locationData.setFlags(location->flags); + locationData.setLatitude(location->latitude); + locationData.setLongitude(location->longitude); + locationData.setAltitude(location->altitude); + locationData.setSpeed(location->speed); + locationData.setBearing(location->bearing); + locationData.setAccuracy(location->accuracy); + locationData.setTimestamp(location->timestamp); + locationData.setSource(cereal::GpsLocationData::SensorSource::ANDROID); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + gps_location_publisher->send((char*)bytes.begin(), bytes.size()); +} + +pthread_t create_thread_callback(const char* name, void (*start)(void *), void* arg) { + LOG("creating thread: %s", name); + pthread_t thread; + pthread_attr_t attr; + int err; + + err = pthread_attr_init(&attr); + err = pthread_create(&thread, &attr, (void*(*)(void*))start, arg); + + return thread; +} + +GpsCallbacks gps_callbacks = { + sizeof(GpsCallbacks), + location_callback, + NULL, + NULL, + nmea_callback, + NULL, + NULL, + NULL, + create_thread_callback, +}; + +void agps_status_cb(AGpsStatus *status) { + switch (status->status) { + case GPS_REQUEST_AGPS_DATA_CONN: + fprintf(stdout, "*** data_conn_open\n"); + gAGpsInterface->data_conn_open("internet"); + break; + case GPS_RELEASE_AGPS_DATA_CONN: + fprintf(stdout, "*** data_conn_closed\n"); + gAGpsInterface->data_conn_closed(); + break; + } +} + +AGpsCallbacks agps_callbacks = { + agps_status_cb, + create_thread_callback, +}; + + + +void gps_init() { + LOG("*** init GPS"); + hw_module_t* module = NULL; + hw_get_module(GPS_HARDWARE_MODULE_ID, (hw_module_t const**)&module); + assert(module); + + static hw_device_t* device = NULL; + module->methods->open(module, GPS_HARDWARE_MODULE_ID, &device); + assert(device); + + // ** get gps interface ** + gps_device_t* gps_device = (gps_device_t *)device; + gGpsInterface = gps_device->get_gps_interface(gps_device); + assert(gGpsInterface); + + gAGpsInterface = (const AGpsInterface*)gGpsInterface->get_extension(AGPS_INTERFACE); + assert(gAGpsInterface); + + + gGpsInterface->init(&gps_callbacks); + gAGpsInterface->init(&agps_callbacks); + gAGpsInterface->set_server(AGPS_TYPE_SUPL, "supl.google.com", 7276); + + // gGpsInterface->delete_aiding_data(GPS_DELETE_ALL); + gGpsInterface->start(); + gGpsInterface->set_position_mode(GPS_POSITION_MODE_MS_BASED, + GPS_POSITION_RECURRENCE_PERIODIC, + 100, 0, 0); + + gps_context = Context::create(); + gps_publisher = PubSocket::create(gps_context, "gpsNMEA"); + gps_location_publisher = PubSocket::create(gps_context, "gpsLocation"); + + assert(gps_publisher != NULL); + assert(gps_location_publisher != NULL); +} + +void gps_destroy() { + gGpsInterface->stop(); + gGpsInterface->cleanup(); +} + + +int64_t arm_cntpct() { + int64_t v; + asm volatile("mrs %0, cntpct_el0" : "=r"(v)); + return v; +} + +} + +int main() { + int err = 0; + setpriority(PRIO_PROCESS, 0, -13); + + signal(SIGINT, (sighandler_t)set_do_exit); + signal(SIGTERM, (sighandler_t)set_do_exit); + + gps_init(); + + rawgps_init(); + + while(!do_exit) pause(); + + rawgps_destroy(); + + gps_destroy(); + + return 0; +} diff --git a/selfdrive/sensord/libdiag.h b/selfdrive/sensord/libdiag.h new file mode 100644 index 0000000000..ab3ee91b14 --- /dev/null +++ b/selfdrive/sensord/libdiag.h @@ -0,0 +1,40 @@ +#ifndef LIBDIAG_H +#define LIBDIAG_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define DIAG_MAX_RX_PKT_SIZ 4096 + +bool Diag_LSM_Init(uint8_t* pIEnv); +bool Diag_LSM_DeInit(void); + +// DCI + +#define DIAG_CON_APSS 0x001 +#define DIAG_CON_MPSS 0x002 +#define DIAG_CON_LPASS 0x004 +#define DIAG_CON_WCNSS 0x008 + +enum { + DIAG_DCI_NO_ERROR = 1001, +} diag_dci_error_type; + +int diag_register_dci_client(int*, uint16_t*, int, void*); +int diag_log_stream_config(int client_id, int set_mask, uint16_t log_codes_array[], int num_codes); +int diag_register_dci_stream(void (*func_ptr_logs)(unsigned char *ptr, int len), void (*func_ptr_events)(unsigned char *ptr, int len)); +int diag_release_dci_client(int*); + +int diag_send_dci_async_req(int client_id, unsigned char buf[], int bytes, unsigned char *rsp_ptr, int rsp_len, + void (*func_ptr)(unsigned char *ptr, int len, void *data_ptr), void *data_ptr); + + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/selfdrive/sensord/rawgps.cc b/selfdrive/sensord/rawgps.cc new file mode 100644 index 0000000000..3a0d1c8046 --- /dev/null +++ b/selfdrive/sensord/rawgps.cc @@ -0,0 +1,1184 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "cereal/gen/cpp/log.capnp.h" + +#include "messaging.hpp" +#include "common/timing.h" +#include "common/util.h" +#include "common/swaglog.h" + +#include "libdiag.h" + +#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 Context *rawgps_context; +static PubSocket *rawgps_publisher; +static int client_id = 0; + +static void hexdump(uint8_t* d, size_t len) { + for (int i=0; i= 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(); + 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; isv_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; isv_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; isv_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; isv_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 xyz0(report->xyz0, 3); + lreport.setXyz0(xyz0); + + kj::ArrayPtr xyzN(report->xyzN, 9); + lreport.setXyzN(xyzN); + + kj::ArrayPtr 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 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(); + rawgps_publisher->send((char*)bytes.begin(), bytes.size()); +} + +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 = Context::create(); + rawgps_publisher = PubSocket::create(rawgps_context, "qcomGnss"); + assert(rawgps_publisher != NULL); + + 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(); + + delete rawgps_publisher; + delete rawgps_context; +} + + +#ifdef RAWGPS_TEST +int main() { + int err = 0; + rawgps_init(); + + while(1) { + usleep(100000); + } + + rawgps_destroy(); + return 0; +} +#endif diff --git a/selfdrive/sensord/rawgps.h b/selfdrive/sensord/rawgps.h new file mode 100644 index 0000000000..13d23cd28c --- /dev/null +++ b/selfdrive/sensord/rawgps.h @@ -0,0 +1,7 @@ +#ifndef RAWGPS_H +#define RAWGPS_H + +void rawgps_init(); +void rawgps_destroy(); + +#endif diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord new file mode 100755 index 0000000000..71509d05e4 --- /dev/null +++ b/selfdrive/sensord/sensord @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ed52dac08364c5582ae5f7ab3ab8d541014d90a9acfeddd287817695ca0260b9 +size 82 diff --git a/selfdrive/sensord/sensors.cc b/selfdrive/sensord/sensors.cc new file mode 100644 index 0000000000..952a4863c2 --- /dev/null +++ b/selfdrive/sensord/sensors.cc @@ -0,0 +1,245 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include + +#include + +#include "messaging.hpp" +#include "common/timing.h" +#include "common/swaglog.h" + +#include "cereal/gen/cpp/log.capnp.h" + +#define SENSOR_ACCELEROMETER 1 +#define SENSOR_MAGNETOMETER 2 +#define SENSOR_GYRO 4 + +// ACCELEROMETER_UNCALIBRATED is only in Android O +// https://developer.android.com/reference/android/hardware/Sensor.html#STRING_TYPE_ACCELEROMETER_UNCALIBRATED +#define SENSOR_MAGNETOMETER_UNCALIBRATED 3 +#define SENSOR_GYRO_UNCALIBRATED 5 + +#define SENSOR_PROXIMITY 6 +#define SENSOR_LIGHT 7 + +volatile sig_atomic_t do_exit = 0; +volatile sig_atomic_t re_init_sensors = 0; + +namespace { + +void set_do_exit(int sig) { + do_exit = 1; +} + +void sigpipe_handler(int sig) { + LOGE("SIGPIPE received"); + re_init_sensors = true; +} + + +void sensor_loop() { + LOG("*** sensor loop"); + + + while (!do_exit) { + Context * c = Context::create(); + PubSocket * sensor_events_sock = PubSocket::create(c, "sensorEvents"); + assert(sensor_events_sock != NULL); + + struct sensors_poll_device_t* device; + struct sensors_module_t* module; + + hw_get_module(SENSORS_HARDWARE_MODULE_ID, (hw_module_t const**)&module); + sensors_open(&module->common, &device); + + // required + struct sensor_t const* list; + int count = module->get_sensors_list(module, &list); + LOG("%d sensors found", count); + + if (getenv("SENSOR_TEST")) { + exit(count); + } + + for (int i = 0; i < count; i++) { + LOGD("sensor %4d: %4d %60s %d-%ld us", i, list[i].handle, list[i].name, list[i].minDelay, list[i].maxDelay); + } + + device->activate(device, SENSOR_MAGNETOMETER_UNCALIBRATED, 0); + device->activate(device, SENSOR_GYRO_UNCALIBRATED, 0); + device->activate(device, SENSOR_ACCELEROMETER, 0); + device->activate(device, SENSOR_MAGNETOMETER, 0); + device->activate(device, SENSOR_GYRO, 0); + device->activate(device, SENSOR_PROXIMITY, 0); + device->activate(device, SENSOR_LIGHT, 0); + + device->activate(device, SENSOR_MAGNETOMETER_UNCALIBRATED, 1); + device->activate(device, SENSOR_GYRO_UNCALIBRATED, 1); + device->activate(device, SENSOR_ACCELEROMETER, 1); + device->activate(device, SENSOR_MAGNETOMETER, 1); + device->activate(device, SENSOR_GYRO, 1); + device->activate(device, SENSOR_PROXIMITY, 1); + device->activate(device, SENSOR_LIGHT, 1); + + device->setDelay(device, SENSOR_GYRO_UNCALIBRATED, ms2ns(10)); + device->setDelay(device, SENSOR_MAGNETOMETER_UNCALIBRATED, ms2ns(100)); + device->setDelay(device, SENSOR_ACCELEROMETER, ms2ns(10)); + device->setDelay(device, SENSOR_GYRO, ms2ns(10)); + device->setDelay(device, SENSOR_MAGNETOMETER, ms2ns(100)); + device->setDelay(device, SENSOR_PROXIMITY, ms2ns(100)); + device->setDelay(device, SENSOR_LIGHT, ms2ns(100)); + + static const size_t numEvents = 16; + sensors_event_t buffer[numEvents]; + + + while (!do_exit) { + int n = device->poll(device, buffer, numEvents); + if (n == 0) continue; + if (n < 0) { + LOG("sensor_loop poll failed: %d", n); + continue; + } + + int log_events = 0; + for (int i=0; i < n; i++) { + switch (buffer[i].type) { + case SENSOR_TYPE_ACCELEROMETER: + case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: + case SENSOR_TYPE_MAGNETIC_FIELD: + case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: + case SENSOR_TYPE_GYROSCOPE: + case SENSOR_TYPE_PROXIMITY: + case SENSOR_TYPE_LIGHT: + log_events++; + break; + default: + continue; + } + } + + uint64_t log_time = nanos_since_boot(); + + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(log_time); + + auto sensor_events = event.initSensorEvents(log_events); + + int log_i = 0; + for (int i = 0; i < n; i++) { + + const sensors_event_t& data = buffer[i]; + + switch (data.type) { + case SENSOR_TYPE_ACCELEROMETER: + case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: + case SENSOR_TYPE_MAGNETIC_FIELD: + case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: + case SENSOR_TYPE_GYROSCOPE: + case SENSOR_TYPE_PROXIMITY: + case SENSOR_TYPE_LIGHT: + break; + default: + continue; + } + + auto log_event = sensor_events[log_i]; + + log_event.setSource(cereal::SensorEventData::SensorSource::ANDROID); + log_event.setVersion(data.version); + log_event.setSensor(data.sensor); + log_event.setType(data.type); + log_event.setTimestamp(data.timestamp); + + switch (data.type) { + case SENSOR_TYPE_ACCELEROMETER: { + auto svec = log_event.initAcceleration(); + kj::ArrayPtr vs(&data.acceleration.v[0], 3); + svec.setV(vs); + svec.setStatus(data.acceleration.status); + break; + } + case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: { + auto svec = log_event.initMagneticUncalibrated(); + // assuming the uncalib and bias floats are contiguous in memory + kj::ArrayPtr vs(&data.uncalibrated_magnetic.uncalib[0], 6); + svec.setV(vs); + break; + } + case SENSOR_TYPE_MAGNETIC_FIELD: { + auto svec = log_event.initMagnetic(); + kj::ArrayPtr vs(&data.magnetic.v[0], 3); + svec.setV(vs); + svec.setStatus(data.magnetic.status); + break; + } + case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: { + auto svec = log_event.initGyroUncalibrated(); + // assuming the uncalib and bias floats are contiguous in memory + kj::ArrayPtr vs(&data.uncalibrated_gyro.uncalib[0], 6); + svec.setV(vs); + break; + } + case SENSOR_TYPE_GYROSCOPE: { + auto svec = log_event.initGyro(); + kj::ArrayPtr vs(&data.gyro.v[0], 3); + svec.setV(vs); + svec.setStatus(data.gyro.status); + break; + } + case SENSOR_TYPE_PROXIMITY: { + log_event.setProximity(data.distance); + break; + } + case SENSOR_TYPE_LIGHT: + log_event.setLight(data.light); + break; + } + + log_i++; + } + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + sensor_events_sock->send((char*)bytes.begin(), bytes.size()); + + if (re_init_sensors){ + LOGE("Resetting sensors"); + re_init_sensors = false; + break; + } + } + + delete sensor_events_sock; + delete c; + } +} + +}// Namespace end + +int main(int argc, char *argv[]) { + setpriority(PRIO_PROCESS, 0, -13); + signal(SIGINT, (sighandler_t)set_do_exit); + signal(SIGTERM, (sighandler_t)set_do_exit); + signal(SIGPIPE, (sighandler_t)sigpipe_handler); + + sensor_loop(); + + return 0; +}