spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

sensord cleanup (#2111) 

* sensors_qcom cleanup

* include vector and map

* abstract sensor init

* use a set

* add to relase files

* fix this up

* no ui changes

* cleanup

Co-authored-by: Comma Device <device@comma.ai>
pull/2099/head
Adeeb Shihadeh 2020-09-02 14:04:55 -07:00 committed by GitHub
parent 5c058140ed
commit daea946ad2
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 32 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
selfdrive/sensord/sensors_qcom.cc
View File

@ -10,7 +10,8 @@
#include <sys/types.h>
#include <sys/resource.h>
#include <pthread.h>
#include <map>
#include <set>
#include <cutils/log.h>
#include <hardware/sensors.h>
@ -20,15 +21,14 @@
#include "common/timing.h"
#include "common/swaglog.h"
// ACCELEROMETER_UNCALIBRATED is only in Android O
// https://developer.android.com/reference/android/hardware/Sensor.html#STRING_TYPE_ACCELEROMETER_UNCALIBRATED
#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
@ -70,30 +70,33 @@ void sensor_loop() {
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);
std::set<int> sensor_types = {
SENSOR_TYPE_ACCELEROMETER,
SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
SENSOR_TYPE_MAGNETIC_FIELD,
SENSOR_TYPE_GYROSCOPE_UNCALIBRATED,
SENSOR_TYPE_GYROSCOPE,
SENSOR_TYPE_PROXIMITY,
SENSOR_TYPE_LIGHT,
};
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);
std::map<int, int64_t> sensors = {
{SENSOR_GYRO_UNCALIBRATED, ms2ns(10)},
{SENSOR_MAGNETOMETER_UNCALIBRATED, ms2ns(100)},
{SENSOR_ACCELEROMETER, ms2ns(10)},
{SENSOR_GYRO, ms2ns(10)},
{SENSOR_MAGNETOMETER, ms2ns(100)},
{SENSOR_PROXIMITY, ms2ns(100)},
{SENSOR_LIGHT, ms2ns(100)}
};
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));
for (auto &s : sensors) {
device->activate(device, s.first, 0);
device->activate(device, s.first, 1);
device->setDelay(device, s.first, s.second);
}
// TODO: why is this 16?
static const size_t numEvents = 16;
sensors_event_t buffer[numEvents];
@ -107,26 +110,14 @@ void sensor_loop() {
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:
if (sensor_types.find(buffer[i].type) != sensor_types.end()) {
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);
event.setLogMonoTime(nanos_since_boot());
auto sensor_events = event.initSensorEvents(log_events);
@ -135,21 +126,11 @@ void sensor_loop() {
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:
if (sensor_types.find(data.type) == sensor_types.end()) {
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);