openpilot_comma/selfdrive/sensord/sensors.cc

#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 <czmq.h>

#include <capnp/serialize.h>

#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 int do_exit = 0;

namespace {

void set_do_exit(int sig) {
  do_exit = 1;
}

void sigpipe_handler(int sig) {
  LOGE("SIGPIPE received");
}


void sensor_loop() {
  LOG("*** sensor loop");
  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];

  auto sensor_events_sock = zsock_new_pub("@tcp://*:8003");
  assert(sensor_events_sock);
  auto sensor_events_sock_raw = zsock_resolve(sensor_events_sock);

  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();
    zmq_send(sensor_events_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);

  }
  LOG("bye");
}

}

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;
}