selfdrive/sensord

old-commit-hash: 5c9afcc785

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'])

selfdrive/sensord/gpsd

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:35038a572308e61aa56a483b0edffbdc64322c04911a8422669c52a6fbe76a86
+size 79

selfdrive/sensord/gpsd.cc

@@ -0,0 +1,201 @@
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <assert.h>
+#include <sys/time.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/resource.h>
+
+#include <pthread.h>
+
+#include <cutils/log.h>
+
+#include <hardware/gps.h>
+#include <utils/Timers.h>
+
+#include <capnp/serialize.h>
+
+#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<cereal::Event>();
+  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<cereal::Event>();
+  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;
+}

selfdrive/sensord/libdiag.h

@@ -0,0 +1,40 @@
+#ifndef LIBDIAG_H
+#define LIBDIAG_H
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#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

selfdrive/sensord/rawgps.h

@@ -0,0 +1,7 @@
+#ifndef RAWGPS_H
+#define RAWGPS_H
+
+void rawgps_init();
+void rawgps_destroy();
+
+#endif

selfdrive/sensord/sensord

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ed52dac08364c5582ae5f7ab3ab8d541014d90a9acfeddd287817695ca0260b9
+size 82

selfdrive/sensord/sensors.cc

@@ -0,0 +1,245 @@
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <assert.h>
+#include <sys/time.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/resource.h>
+
+#include <pthread.h>
+
+#include <cutils/log.h>
+
+#include <hardware/sensors.h>
+#include <utils/Timers.h>
+
+#include <capnp/serialize.h>
+
+#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<cereal::Event>();
+      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<const float> 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<const float> vs(&data.uncalibrated_magnetic.uncalib[0], 6);
+          svec.setV(vs);
+          break;
+        }
+        case SENSOR_TYPE_MAGNETIC_FIELD: {
+          auto svec = log_event.initMagnetic();
+          kj::ArrayPtr<const float> 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<const float> vs(&data.uncalibrated_gyro.uncalib[0], 6);
+          svec.setV(vs);
+          break;
+        }
+        case SENSOR_TYPE_GYROSCOPE: {
+          auto svec = log_event.initGyro();
+          kj::ArrayPtr<const float> 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;
+}