selfdrive/sensord
George Hotz
parent
c0bfbc12c7
commit
5c9afcc785
|
|
@ -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'])
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
#!/bin/sh
|
||||
export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH"
|
||||
exec ./_gpsd
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,7 @@
|
|||
#ifndef RAWGPS_H
|
||||
#define RAWGPS_H
|
||||
|
||||
void rawgps_init();
|
||||
void rawgps_destroy();
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
#!/bin/sh
|
||||
export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH"
|
||||
exec ./_sensord
|
||||
|
|
@ -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;
|
||||
}
|
||||
Loading…
Reference in New Issue