From 87473fbc41055f8e768089475616d642f3d45ffe Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Tue, 25 Aug 2020 18:31:49 -0700 Subject: [PATCH 01/11] Loggerd rotation test (#2077) * loggerd test * delete afterwards * add ccc * fix pylint * cleanup rotation test * this works * bump tolerance * clear the data after test Co-authored-by: Adeeb Shihadeh Co-authored-by: Comma Device --- selfdrive/loggerd/config.py | 24 +++---- selfdrive/loggerd/loggerd.cc | 11 +++- selfdrive/loggerd/tests/test_loggerd.py | 84 +++++++++++++++++++++++++ selfdrive/test/helpers.py | 14 +++-- 4 files changed, 113 insertions(+), 20 deletions(-) create mode 100755 selfdrive/loggerd/tests/test_loggerd.py diff --git a/selfdrive/loggerd/config.py b/selfdrive/loggerd/config.py index 0ee6fa671d..da3f3a99ca 100644 --- a/selfdrive/loggerd/config.py +++ b/selfdrive/loggerd/config.py @@ -10,20 +10,20 @@ SEGMENT_LENGTH = 60 def get_available_percent(default=None): - try: - statvfs = os.statvfs(ROOT) - available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks - except OSError: - available_percent = default + try: + statvfs = os.statvfs(ROOT) + available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks + except OSError: + available_percent = default - return available_percent + return available_percent def get_available_bytes(default=None): - try: - statvfs = os.statvfs(ROOT) - available_bytes = statvfs.f_bavail * statvfs.f_frsize - except OSError: - available_bytes = default + try: + statvfs = os.statvfs(ROOT) + available_bytes = statvfs.f_bavail * statvfs.f_frsize + except OSError: + available_bytes = default - return available_bytes + return available_bytes diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 9cc3043a70..72ad62fe6d 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -540,6 +540,11 @@ int main(int argc, char** argv) { return 0; } + int segment_length = SEGMENT_LENGTH; + if (getenv("LOGGERD_TEST")) { + segment_length = atoi(getenv("LOGGERD_SEGMENT_LENGTH")); + } + setpriority(PRIO_PROCESS, 0, -12); clear_locks(); @@ -619,7 +624,7 @@ int main(int argc, char** argv) { uint64_t bytes_count = 0; while (!do_exit) { - for (auto sock : poller->poll(100 * 1000)){ + for (auto sock : poller->poll(100 * 1000)) { while (true) { Message * msg = sock->receive(true); if (msg == NULL){ @@ -659,10 +664,10 @@ int main(int argc, char** argv) { } double ts = seconds_since_boot(); - if (ts - last_rotate_ts > SEGMENT_LENGTH) { + if (ts - last_rotate_ts > segment_length) { // rotate the log - last_rotate_ts += SEGMENT_LENGTH; + last_rotate_ts += segment_length; std::lock_guard guard(s.lock); s.rotate_last_frame_id = s.last_frame_id; diff --git a/selfdrive/loggerd/tests/test_loggerd.py b/selfdrive/loggerd/tests/test_loggerd.py new file mode 100755 index 0000000000..13eab535f1 --- /dev/null +++ b/selfdrive/loggerd/tests/test_loggerd.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +import math +import os +import random +import shutil +import time +import unittest +from pathlib import Path + +from common.params import Params +from common.hardware import EON, TICI +from common.timeout import Timeout +from selfdrive.test.helpers import with_processes +from selfdrive.loggerd.config import ROOT + + +# baseline file sizes for a 2s segment, in bytes +FULL_SIZE = 1253786 +if EON: + CAMERAS = { + "fcamera": FULL_SIZE, + "dcamera": 770920, + } +elif TICI: + CAMERAS = {f"{c}camera": FULL_SIZE for c in ["f", "e", "d"]} + +FILE_SIZE_TOLERANCE = 0.25 + +class TestLoggerd(unittest.TestCase): + + # TODO: all of loggerd should work on PC + @classmethod + def setUpClass(cls): + if not (EON or TICI): + raise unittest.SkipTest + + def setUp(self): + Params().put("RecordFront", "1") + self._clear_logs() + + self.segment_length = 2 + os.environ["LOGGERD_TEST"] = "1" + os.environ["LOGGERD_SEGMENT_LENGTH"] = str(self.segment_length) + + def tearDown(self): + self._clear_logs() + + def _clear_logs(self): + if os.path.exists(ROOT): + shutil.rmtree(ROOT) + + def _get_latest_segment_path(self): + last_route = sorted(Path(ROOT).iterdir(), key=os.path.getmtime)[-1] + return os.path.join(ROOT, last_route) + + # TODO: this should run faster than real time + @with_processes(['camerad', 'loggerd'], init_time=2) + def test_log_rotation(self): + # wait for first seg to start being written + time.sleep(5) + route_prefix_path = self._get_latest_segment_path().rsplit("--", 1)[0] + + num_segments = random.randint(80, 150) + for i in range(num_segments): + if i < num_segments - 1: + with Timeout(self.segment_length*3, error_msg=f"timed out waiting for segment {i}"): + while True: + seg_num = int(self._get_latest_segment_path().rsplit("--", 1)[1]) + if seg_num > i: + break + time.sleep(0.1) + else: + time.sleep(self.segment_length + 2) + + # check each camera file size + for camera, size in CAMERAS.items(): + f = f"{route_prefix_path}--{i}/{camera}.hevc" + self.assertTrue(os.path.exists(f), f"couldn't find {f}") + file_size = os.path.getsize(f) + self.assertTrue(math.isclose(file_size, size, rel_tol=FILE_SIZE_TOLERANCE), + f"{camera} failed size check: expected {size}, got {file_size}") + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index 1e53ef4245..8e852e997f 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -1,3 +1,4 @@ +import time import subprocess from functools import wraps from nose.tools import nottest @@ -23,22 +24,25 @@ def phone_only(x): else: return nottest(x) -def with_processes(processes): +def with_processes(processes, init_time=0): def wrapper(func): @wraps(func) - def wrap(): + def wrap(*args, **kwargs): # start and assert started - [start_managed_process(p) for p in processes] + for p in processes: + start_managed_process(p) + time.sleep(init_time) assert all(get_running()[name].exitcode is None for name in processes) # call the function try: - func() + func(*args, **kwargs) # assert processes are still started assert all(get_running()[name].exitcode is None for name in processes) finally: # kill and assert all stopped - [kill_managed_process(p) for p in processes] + for p in processes: + kill_managed_process(p) assert len(get_running()) == 0 return wrap return wrapper From 521b6688ea6f32e6712ac4bdb382c393963b2e07 Mon Sep 17 00:00:00 2001 From: ZwX1616 Date: Tue, 25 Aug 2020 21:07:50 -0700 Subject: [PATCH 02/11] wait longer for tici camerad startup --- selfdrive/loggerd/tests/test_loggerd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/loggerd/tests/test_loggerd.py b/selfdrive/loggerd/tests/test_loggerd.py index 13eab535f1..fe7f2862c8 100755 --- a/selfdrive/loggerd/tests/test_loggerd.py +++ b/selfdrive/loggerd/tests/test_loggerd.py @@ -54,7 +54,7 @@ class TestLoggerd(unittest.TestCase): return os.path.join(ROOT, last_route) # TODO: this should run faster than real time - @with_processes(['camerad', 'loggerd'], init_time=2) + @with_processes(['camerad', 'loggerd'], init_time=5) def test_log_rotation(self): # wait for first seg to start being written time.sleep(5) From 622e42d504458ebfbb23e945beb1a60bbfd0e63b Mon Sep 17 00:00:00 2001 From: robbederks Date: Wed, 26 Aug 2020 11:30:30 +0200 Subject: [PATCH 03/11] Tici sensord (#2072) * tici sensord: WIP * add sensor constants * rename files * base class * init sensors * publish something * dont leak memory * try reading from accel * Accel works * add time and sensor source * Update release files * If we want low BW we want 125hz * this should run gyro as well * add filter on gyro * also filter accel data * Add i2c files * cast makes macos unhappy * Same outputs as android sensord Co-authored-by: Tici Co-authored-by: Willem Melching --- SConstruct | 3 +- release/files_common | 8 +- selfdrive/common/SConscript | 4 +- selfdrive/common/i2c.cc | 80 +++++++++++++++++ selfdrive/common/i2c.h | 16 ++++ selfdrive/sensord/SConscript | 19 ++-- selfdrive/sensord/sensors/bmx055_accel.cc | 71 +++++++++++++++ selfdrive/sensord/sensors/bmx055_accel.hpp | 35 ++++++++ selfdrive/sensord/sensors/bmx055_gyro.cc | 81 +++++++++++++++++ selfdrive/sensord/sensors/bmx055_gyro.hpp | 36 ++++++++ selfdrive/sensord/sensors/bmx055_magn.cc | 43 ++++++++++ selfdrive/sensord/sensors/bmx055_magn.hpp | 21 +++++ selfdrive/sensord/sensors/constants.hpp | 17 ++++ selfdrive/sensord/sensors/i2c_sensor.cc | 24 ++++++ selfdrive/sensord/sensors/i2c_sensor.hpp | 23 +++++ .../sensord/{sensors.cc => sensors_qcom.cc} | 0 selfdrive/sensord/sensors_qcom2.cc | 86 +++++++++++++++++++ 17 files changed, 559 insertions(+), 8 deletions(-) create mode 100644 selfdrive/common/i2c.cc create mode 100644 selfdrive/common/i2c.h create mode 100644 selfdrive/sensord/sensors/bmx055_accel.cc create mode 100644 selfdrive/sensord/sensors/bmx055_accel.hpp create mode 100644 selfdrive/sensord/sensors/bmx055_gyro.cc create mode 100644 selfdrive/sensord/sensors/bmx055_gyro.hpp create mode 100644 selfdrive/sensord/sensors/bmx055_magn.cc create mode 100644 selfdrive/sensord/sensors/bmx055_magn.hpp create mode 100644 selfdrive/sensord/sensors/constants.hpp create mode 100644 selfdrive/sensord/sensors/i2c_sensor.cc create mode 100644 selfdrive/sensord/sensors/i2c_sensor.hpp rename selfdrive/sensord/{sensors.cc => sensors_qcom.cc} (100%) create mode 100644 selfdrive/sensord/sensors_qcom2.cc diff --git a/SConstruct b/SConstruct index 72a4acfceb..a12fa929b4 100644 --- a/SConstruct +++ b/SConstruct @@ -156,6 +156,7 @@ env = Environment( "#selfdrive/camerad/include", "#selfdrive/loggerd/include", "#selfdrive/modeld", + "#selfdrive/sensord", "#selfdrive/ui", "#cereal/messaging", "#cereal", @@ -308,10 +309,10 @@ SConscript(['selfdrive/loggerd/SConscript']) SConscript(['selfdrive/locationd/SConscript']) SConscript(['selfdrive/locationd/models/SConscript']) +SConscript(['selfdrive/sensord/SConscript']) if arch == "aarch64": SConscript(['selfdrive/logcatd/SConscript']) - SConscript(['selfdrive/sensord/SConscript']) if arch != "larch64": SConscript(['selfdrive/ui/SConscript']) diff --git a/release/files_common b/release/files_common index 2df9430159..bf350b7e3a 100644 --- a/release/files_common +++ b/release/files_common @@ -215,8 +215,11 @@ selfdrive/common/visionimg.cc selfdrive/common/visionimg.h selfdrive/common/spinner.c selfdrive/common/spinner.h + selfdrive/common/gpio.cc selfdrive/common/gpio.h +selfdrive/common/i2c.cc +selfdrive/common/i2c.h selfdrive/controls/__init__.py @@ -313,7 +316,10 @@ selfdrive/loggerd/deleter.py selfdrive/sensord/SConscript selfdrive/sensord/gpsd.cc selfdrive/sensord/libdiag.h -selfdrive/sensord/sensors.cc +selfdrive/sensord/sensors_qcom.cc +selfdrive/sensord/sensors_qcom2.cc +selfdrive/sensord/sensors/*.cc +selfdrive/sensord/sensors/*.hpp selfdrive/sensord/sensord selfdrive/sensord/gpsd diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript index 0ba5b1abcc..f70abf58c4 100644 --- a/selfdrive/common/SConscript +++ b/selfdrive/common/SConscript @@ -5,7 +5,9 @@ if SHARED: else: fxn = env.Library -_common = fxn('common', ['params.cc', 'swaglog.cc', 'util.c', 'cqueue.c', 'gpio.cc'], LIBS="json11") +common_libs = ['params.cc', 'swaglog.cc', 'util.c', 'cqueue.c', 'gpio.cc', 'i2c.cc'] + +_common = fxn('common', common_libs, LIBS="json11") _visionipc = fxn('visionipc', ['visionipc.c', 'ipc.c']) files = [ diff --git a/selfdrive/common/i2c.cc b/selfdrive/common/i2c.cc new file mode 100644 index 0000000000..a37b144dc2 --- /dev/null +++ b/selfdrive/common/i2c.cc @@ -0,0 +1,80 @@ +#include "i2c.h" + +#include +#include +#include +#include +#include +#include +#include "common/swaglog.h" + +#define UNUSED(x) (void)(x) + +#ifdef QCOM2 +// TODO: decide if we want to isntall libi2c-dev everywhere +#include + +I2CBus::I2CBus(uint8_t bus_id){ + char bus_name[20]; + snprintf(bus_name, 20, "/dev/i2c-%d", bus_id); + + i2c_fd = open(bus_name, O_RDWR); + if(i2c_fd < 0){ + throw std::runtime_error("Failed to open I2C bus"); + } +} + +I2CBus::~I2CBus(){ + if(i2c_fd >= 0){ close(i2c_fd); } +} + +int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len){ + int ret = 0; + + ret = ioctl(i2c_fd, I2C_SLAVE, device_address); + if(ret < 0) { goto fail; } + + ret = i2c_smbus_read_i2c_block_data(i2c_fd, register_address, len, buffer); + if((ret < 0) || (ret != len)) { goto fail; } + +fail: + return ret; +} + +int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data){ + int ret = 0; + + ret = ioctl(i2c_fd, I2C_SLAVE, device_address); + if(ret < 0) { goto fail; } + + ret = i2c_smbus_write_byte_data(i2c_fd, register_address, data); + if(ret < 0) { goto fail; } + +fail: + return ret; +} + +#else + +I2CBus::I2CBus(uint8_t bus_id){ + UNUSED(bus_id); + i2c_fd = -1; +} + +I2CBus::~I2CBus(){} + +int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len){ + UNUSED(device_address); + UNUSED(register_address); + UNUSED(buffer); + UNUSED(len); + return -1; +} + +int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data){ + UNUSED(device_address); + UNUSED(register_address); + UNUSED(data); + return -1; +} +#endif diff --git a/selfdrive/common/i2c.h b/selfdrive/common/i2c.h new file mode 100644 index 0000000000..d5788510d3 --- /dev/null +++ b/selfdrive/common/i2c.h @@ -0,0 +1,16 @@ +#pragma once + +#include +#include + +class I2CBus { + private: + int i2c_fd; + + public: + I2CBus(uint8_t bus_id); + ~I2CBus(); + + int read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len); + int set_register(uint8_t device_address, uint register_address, uint8_t data); +}; diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript index b578ac9514..dbfcebe75b 100644 --- a/selfdrive/sensord/SConscript +++ b/selfdrive/sensord/SConscript @@ -1,5 +1,14 @@ -Import('env', 'common', 'cereal', 'messaging') -env.Program('_sensord', 'sensors.cc', LIBS=['hardware', common, cereal, messaging, 'capnp', 'zmq', 'kj']) -lenv = env.Clone() -lenv['LIBPATH'] += ['/system/vendor/lib64'] -lenv.Program('_gpsd', ['gpsd.cc'], LIBS=['hardware', common, 'diag', 'time_genoff', cereal, messaging, 'capnp', 'zmq', 'kj']) +Import('env', 'arch', 'common', 'cereal', 'messaging') +if arch == "aarch64": + env.Program('_sensord', 'sensors_qcom.cc', LIBS=['hardware', common, cereal, messaging, 'capnp', 'zmq', 'kj']) + lenv = env.Clone() + lenv['LIBPATH'] += ['/system/vendor/lib64'] + lenv.Program('_gpsd', ['gpsd.cc'], LIBS=['hardware', common, 'diag', 'time_genoff', cereal, messaging, 'capnp', 'zmq', 'kj']) +else: + sensors = [ + 'sensors/i2c_sensor.cc', + 'sensors/bmx055_accel.cc', + 'sensors/bmx055_gyro.cc', + 'sensors/bmx055_magn.cc', + ] + env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=[common, cereal, messaging, 'capnp', 'zmq', 'kj']) diff --git a/selfdrive/sensord/sensors/bmx055_accel.cc b/selfdrive/sensord/sensors/bmx055_accel.cc new file mode 100644 index 0000000000..5eea0bcdfa --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_accel.cc @@ -0,0 +1,71 @@ +#include +#include "common/swaglog.h" +#include "common/timing.h" + +#include "bmx055_accel.hpp" + + +BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {} + +int BMX055_Accel::init(){ + int ret = 0; + uint8_t buffer[1]; + + ret = read_register(BMX055_ACCEL_I2C_REG_ID, buffer, 1); + if(ret < 0){ + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != BMX055_ACCEL_CHIP_ID){ + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_ACCEL_CHIP_ID); + ret = -1; + goto fail; + } + + // High bandwidth + // ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_ENABLE); + // if (ret < 0){ + // goto fail; + // } + + // Low bandwidth + ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_DISABLE); + if (ret < 0){ + goto fail; + } + ret = set_register(BMX055_ACCEL_I2C_REG_BW, BMX055_ACCEL_BW_125HZ); + if (ret < 0){ + goto fail; + } + +fail: + return ret; +} + +void BMX055_Accel::get_event(cereal::SensorEventData::Builder &event){ + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[6]; + int len = read_register(BMX055_ACCEL_I2C_REG_FIFO, buffer, sizeof(buffer)); + assert(len == 6); + + // 12 bit = +-2g + float scale = 9.81 * 2.0f / (1 << 11); + float x = -read_12_bit(buffer[0], buffer[1]) * scale; + float y = -read_12_bit(buffer[2], buffer[3]) * scale; + float z = read_12_bit(buffer[4], buffer[5]) * scale; + + event.setSource(cereal::SensorEventData::SensorSource::BMX055); + event.setVersion(1); + event.setSensor(SENSOR_ACCELEROMETER); + event.setType(SENSOR_TYPE_ACCELEROMETER); + event.setTimestamp(start_time); + + float xyz[] = {x, y, z}; + kj::ArrayPtr vs(&xyz[0], 3); + + auto svec = event.initAcceleration(); + svec.setV(vs); + svec.setStatus(true); + +} diff --git a/selfdrive/sensord/sensors/bmx055_accel.hpp b/selfdrive/sensord/sensors/bmx055_accel.hpp new file mode 100644 index 0000000000..7bfb97e77d --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_accel.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include "sensors/i2c_sensor.hpp" + +// Address of the chip on the bus +#define BMX055_ACCEL_I2C_ADDR 0x18 + +// Registers of the chip +#define BMX055_ACCEL_I2C_REG_ID 0x00 +#define BMX055_ACCEL_I2C_REG_BW 0x10 +#define BMX055_ACCEL_I2C_REG_HBW 0x13 +#define BMX055_ACCEL_I2C_REG_FIFO 0x3F + +// Constants +#define BMX055_ACCEL_CHIP_ID 0xFA + +#define BMX055_ACCEL_HBW_ENABLE 0b10000000 +#define BMX055_ACCEL_HBW_DISABLE 0b00000000 + +#define BMX055_ACCEL_BW_7_81HZ 0b01000 +#define BMX055_ACCEL_BW_15_63HZ 0b01001 +#define BMX055_ACCEL_BW_31_25HZ 0b01010 +#define BMX055_ACCEL_BW_62_5HZ 0b01011 +#define BMX055_ACCEL_BW_125HZ 0b01100 +#define BMX055_ACCEL_BW_250HZ 0b01101 +#define BMX055_ACCEL_BW_500HZ 0b01110 +#define BMX055_ACCEL_BW_1000HZ 0b01111 + +class BMX055_Accel : public I2CSensor { + uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;} +public: + BMX055_Accel(I2CBus *bus); + int init(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/bmx055_gyro.cc b/selfdrive/sensord/sensors/bmx055_gyro.cc new file mode 100644 index 0000000000..61e3d9e405 --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_gyro.cc @@ -0,0 +1,81 @@ +#include +#include +#include "common/swaglog.h" + +#include "bmx055_gyro.hpp" + +#define DEG2RAD(x) ((x) * M_PI / 180.0) + + +BMX055_Gyro::BMX055_Gyro(I2CBus *bus) : I2CSensor(bus) {} + +int BMX055_Gyro::init(){ + int ret = 0; + uint8_t buffer[1]; + + ret =read_register(BMX055_GYRO_I2C_REG_ID, buffer, 1); + if(ret < 0){ + LOGE("Reading chip ID failed: %d", ret); + goto fail; + } + + if(buffer[0] != BMX055_GYRO_CHIP_ID){ + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_GYRO_CHIP_ID); + ret = -1; + goto fail; + } + + // High bandwidth + // ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_ENABLE); + // if (ret < 0){ + // goto fail; + // } + + // Low bandwidth + ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_DISABLE); + if (ret < 0){ + goto fail; + } + + // 116 Hz filter + ret = set_register(BMX055_GYRO_I2C_REG_BW, BMX055_GYRO_BW_116HZ); + if (ret < 0){ + goto fail; + } + + // +- 125 deg/s range + ret = set_register(BMX055_GYRO_I2C_REG_RANGE, BMX055_GYRO_RANGE_125); + if (ret < 0){ + goto fail; + } + +fail: + return ret; +} + +void BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event){ + uint64_t start_time = nanos_since_boot(); + uint8_t buffer[6]; + int len = read_register(BMX055_GYRO_I2C_REG_FIFO, buffer, sizeof(buffer)); + assert(len == 6); + + // 16 bit = +- 125 deg/s + float scale = 125.0f / (1 << 15); + float x = -DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale); + float y = -DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale); + float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale); + + event.setSource(cereal::SensorEventData::SensorSource::BMX055); + event.setVersion(1); + event.setSensor(SENSOR_GYRO_UNCALIBRATED); + event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED); + event.setTimestamp(start_time); + + float xyz[] = {x, y, z}; + kj::ArrayPtr vs(&xyz[0], 3); + + auto svec = event.initGyroUncalibrated(); + svec.setV(vs); + svec.setStatus(true); + +} diff --git a/selfdrive/sensord/sensors/bmx055_gyro.hpp b/selfdrive/sensord/sensors/bmx055_gyro.hpp new file mode 100644 index 0000000000..407ee1608e --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_gyro.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include "sensors/i2c_sensor.hpp" + +// Address of the chip on the bus +#define BMX055_GYRO_I2C_ADDR 0x68 + +// Registers of the chip +#define BMX055_GYRO_I2C_REG_ID 0x00 +#define BMX055_GYRO_I2C_REG_RANGE 0x0F +#define BMX055_GYRO_I2C_REG_BW 0x10 +#define BMX055_GYRO_I2C_REG_HBW 0x13 +#define BMX055_GYRO_I2C_REG_FIFO 0x3F + +// Constants +#define BMX055_GYRO_CHIP_ID 0x0F + +#define BMX055_GYRO_HBW_ENABLE 0b10000000 +#define BMX055_GYRO_HBW_DISABLE 0b00000000 + +#define BMX055_GYRO_RANGE_2000 0b000 +#define BMX055_GYRO_RANGE_1000 0b001 +#define BMX055_GYRO_RANGE_500 0b010 +#define BMX055_GYRO_RANGE_250 0b011 +#define BMX055_GYRO_RANGE_125 0b100 + +#define BMX055_GYRO_BW_116HZ 0b0010 + + +class BMX055_Gyro : public I2CSensor { + uint8_t get_device_address() {return BMX055_GYRO_I2C_ADDR;} +public: + BMX055_Gyro(I2CBus *bus); + int init(); + void get_event(cereal::SensorEventData::Builder &event); +}; diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc new file mode 100644 index 0000000000..858d190f7f --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_magn.cc @@ -0,0 +1,43 @@ +#include + +#include "common/swaglog.h" + +#include "bmx055_magn.hpp" + + +BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {} + + +int BMX055_Magn::init(){ + int ret; + uint8_t buffer[1]; + + // suspend -> sleep + ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01); + if(ret < 0){ + LOGE("Enabling power failed: %d", ret); + return ret; + } + usleep(5 * 1000); // wait until the chip is powered on + + // read chip ID + ret = read_register(BMX055_MAGN_I2C_REG_ID, buffer, 1); + if(ret < 0){ + LOGE("Reading chip ID failed: %d", ret); + return ret; + } + + if(buffer[0] != BMX055_MAGN_CHIP_ID){ + LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_MAGN_CHIP_ID); + return -1; + } + + // perform self-test + + + // sleep -> active (normal, high-precision) + + + + return 0; +} diff --git a/selfdrive/sensord/sensors/bmx055_magn.hpp b/selfdrive/sensord/sensors/bmx055_magn.hpp new file mode 100644 index 0000000000..6be15a17f1 --- /dev/null +++ b/selfdrive/sensord/sensors/bmx055_magn.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include "sensors/i2c_sensor.hpp" + +// Address of the chip on the bus +#define BMX055_MAGN_I2C_ADDR 0x10 + +// Registers of the chip +#define BMX055_MAGN_I2C_REG_ID 0x40 +#define BMX055_MAGN_I2C_REG_PWR_0 0x4B + +// Constants +#define BMX055_MAGN_CHIP_ID 0x32 + +class BMX055_Magn : public I2CSensor{ + uint8_t get_device_address() {return BMX055_MAGN_I2C_ADDR;} +public: + BMX055_Magn(I2CBus *bus); + int init(); + void get_event(cereal::SensorEventData::Builder &event){}; +}; diff --git a/selfdrive/sensord/sensors/constants.hpp b/selfdrive/sensord/sensors/constants.hpp new file mode 100644 index 0000000000..69db84edc9 --- /dev/null +++ b/selfdrive/sensord/sensors/constants.hpp @@ -0,0 +1,17 @@ +#pragma once + + +#define SENSOR_ACCELEROMETER 1 +#define SENSOR_MAGNETOMETER 2 +#define SENSOR_MAGNETOMETER_UNCALIBRATED 3 +#define SENSOR_GYRO 4 +#define SENSOR_GYRO_UNCALIBRATED 5 +#define SENSOR_LIGHT 7 + +#define SENSOR_TYPE_ACCELEROMETER 1 +#define SENSOR_TYPE_GEOMAGNETIC_FIELD 2 +#define SENSOR_TYPE_GYROSCOPE 4 +#define SENSOR_TYPE_LIGHT 5 +#define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED 14 +#define SENSOR_TYPE_MAGNETIC_FIELD SENSOR_TYPE_GEOMAGNETIC_FIELD +#define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED 16 diff --git a/selfdrive/sensord/sensors/i2c_sensor.cc b/selfdrive/sensord/sensors/i2c_sensor.cc new file mode 100644 index 0000000000..e3000c8b02 --- /dev/null +++ b/selfdrive/sensord/sensors/i2c_sensor.cc @@ -0,0 +1,24 @@ +#include +#include "i2c_sensor.hpp" + +int16_t read_12_bit(uint8_t lsb, uint8_t msb){ + uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb & 0xF0); + return int16_t(combined) / (1 << 4); +} + +int16_t read_16_bit(uint8_t lsb, uint8_t msb){ + uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb); + return int16_t(combined); +} + + +I2CSensor::I2CSensor(I2CBus *bus) : bus(bus){ +} + +int I2CSensor::read_register(uint register_address, uint8_t *buffer, uint8_t len){ + return bus->read_register(get_device_address(), register_address, buffer, len); +} + +int I2CSensor::set_register(uint register_address, uint8_t data){ + return bus->set_register(get_device_address(), register_address, data); +} diff --git a/selfdrive/sensord/sensors/i2c_sensor.hpp b/selfdrive/sensord/sensors/i2c_sensor.hpp new file mode 100644 index 0000000000..37d7568736 --- /dev/null +++ b/selfdrive/sensord/sensors/i2c_sensor.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include +#include "cereal/gen/cpp/log.capnp.h" +#include "common/i2c.h" +#include "sensors/constants.hpp" + +int16_t read_12_bit(uint8_t lsb, uint8_t msb); +int16_t read_16_bit(uint8_t lsb, uint8_t msb); + + +class I2CSensor { +private: + I2CBus *bus; + virtual uint8_t get_device_address() = 0; + +public: + I2CSensor(I2CBus *bus); + int read_register(uint register_address, uint8_t *buffer, uint8_t len); + int set_register(uint register_address, uint8_t data); + virtual int init() = 0; + virtual void get_event(cereal::SensorEventData::Builder &event) = 0; +}; diff --git a/selfdrive/sensord/sensors.cc b/selfdrive/sensord/sensors_qcom.cc similarity index 100% rename from selfdrive/sensord/sensors.cc rename to selfdrive/sensord/sensors_qcom.cc diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc new file mode 100644 index 0000000000..c03c5197b1 --- /dev/null +++ b/selfdrive/sensord/sensors_qcom2.cc @@ -0,0 +1,86 @@ +#include +#include +#include +#include +#include + +#include "messaging.hpp" +#include "common/i2c.h" +#include "common/timing.h" +#include "common/swaglog.h" + +#include "sensors/constants.hpp" +#include "sensors/bmx055_accel.hpp" +#include "sensors/bmx055_gyro.hpp" +#include "sensors/bmx055_magn.hpp" + +volatile sig_atomic_t do_exit = 0; + +#define I2C_BUS_IMU 1 + + +void set_do_exit(int sig) { + do_exit = 1; +} + +int sensor_loop() { + I2CBus *i2c_bus_imu; + + try { + i2c_bus_imu = new I2CBus(I2C_BUS_IMU); + } catch (std::exception &e) { + LOGE("I2CBus init failed"); + return -1; + } + + BMX055_Accel accel(i2c_bus_imu); + BMX055_Gyro gyro(i2c_bus_imu); + BMX055_Magn magn(i2c_bus_imu); + + // Sensor init + std::vector sensors; + sensors.push_back(&accel); + sensors.push_back(&gyro); + // sensors.push_back(&magn); + + + for (I2CSensor * sensor : sensors){ + int err = sensor->init(); + if (err < 0){ + LOGE("Error initializing sensors"); + return -1; + } + } + + PubMaster pm({"sensorEvents"}); + + while (!do_exit){ + uint64_t log_time = nanos_since_boot(); + + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(log_time); + + int num_events = sensors.size(); + auto sensor_events = event.initSensorEvents(num_events); + + for (size_t i = 0; i < num_events; i++){ + auto event = sensor_events[i]; + sensors[i]->get_event(event); + } + + pm.send("sensorEvents", msg); + + // TODO actually run at 100Hz + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + return 0; +} + +int main(int argc, char *argv[]) { + setpriority(PRIO_PROCESS, 0, -13); + signal(SIGINT, set_do_exit); + signal(SIGTERM, set_do_exit); + + return sensor_loop(); +} From 41b0cba0dd226db3938dbfbc8152f074143149f8 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 26 Aug 2020 12:10:50 +0200 Subject: [PATCH 04/11] sensord: more precise 100 Hz, compensate for time spend measuring --- selfdrive/sensord/sensors_qcom2.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc index c03c5197b1..0ef8b2a0ca 100644 --- a/selfdrive/sensord/sensors_qcom2.cc +++ b/selfdrive/sensord/sensors_qcom2.cc @@ -55,6 +55,7 @@ int sensor_loop() { PubMaster pm({"sensorEvents"}); while (!do_exit){ + std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); uint64_t log_time = nanos_since_boot(); capnp::MallocMessageBuilder msg; @@ -71,8 +72,8 @@ int sensor_loop() { pm.send("sensorEvents", msg); - // TODO actually run at 100Hz - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); + std::this_thread::sleep_for(std::chrono::milliseconds(10) - (end - begin)); } return 0; } From c7152d54192605c833912b69cf85514ac2e1b1fc Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 26 Aug 2020 14:57:17 +0200 Subject: [PATCH 05/11] Hardware abstraction class (#2080) * hardware abstraction class * De Morgan * Rename pc hardware class * Fix sound card in controlsd * Pc get sim info * fix hardware in test * two more * No more random imei on android * no randomness on android * Need to return something that looks like imei for registration to work * Return proper network strength * Unused import * Bug fixes + gpsd is only android --- common/android.py | 472 +++++++++--------- common/basedir.py | 10 +- common/hardware.py | 50 ++ common/hardware_base.py | 34 ++ common/realtime.py | 2 +- release/files_common | 1 + selfdrive/athena/athenad.py | 20 +- .../boardd/tests/test_boardd_loopback.py | 2 +- selfdrive/controls/controlsd.py | 4 +- selfdrive/crash.py | 4 +- selfdrive/loggerd/uploader.py | 31 +- selfdrive/manager.py | 15 +- selfdrive/registration.py | 7 +- selfdrive/test/helpers.py | 9 +- .../test/process_replay/camera_replay.py | 3 +- selfdrive/test/test_sounds.py | 4 +- selfdrive/thermald/thermald.py | 7 +- tools/lib/auth_config.py | 14 +- 18 files changed, 390 insertions(+), 299 deletions(-) create mode 100644 common/hardware_base.py diff --git a/common/android.py b/common/android.py index 43bb0f3c1e..8fd69acd07 100644 --- a/common/android.py +++ b/common/android.py @@ -1,78 +1,32 @@ -import os import binascii import itertools +import os import re import struct import subprocess -import random + from cereal import log +from common.hardware_base import HardwareBase NetworkType = log.ThermalData.NetworkType NetworkStrength = log.ThermalData.NetworkStrength -ANDROID = os.path.isfile('/EON') - -def get_sound_card_online(): - return (os.path.isfile('/proc/asound/card0/state') and - open('/proc/asound/card0/state').read().strip() == 'ONLINE') - -def getprop(key): - if not ANDROID: - return "" - return subprocess.check_output(["getprop", key], encoding='utf8').strip() - -def get_imei(slot): - slot = str(slot) - if slot not in ("0", "1"): - raise ValueError("SIM slot must be 0 or 1") - - ret = parse_service_call_string(service_call(["iphonesubinfo", "3" , "i32", str(slot)])) - if not ret: - # allow non android to be identified differently - ret = "%015d" % random.randint(0, 1 << 32) - return ret - -def get_serial(): - ret = getprop("ro.serialno") - if ret == "": - ret = "cccccccc" - return ret - -def get_subscriber_info(): - ret = parse_service_call_string(service_call(["iphonesubinfo", "7"])) - if ret is None or len(ret) < 8: - return "" - return ret - -def reboot(reason=None): - if reason is None: - reason_args = ["null"] - else: - reason_args = ["s16", reason] - - subprocess.check_output([ - "service", "call", "power", "16", # IPowerManager.reboot - "i32", "0", # no confirmation, - *reason_args, - "i32", "1" # wait - ]) def service_call(call): - if not ANDROID: - return None - ret = subprocess.check_output(["service", "call", *call], encoding='utf8').strip() if 'Parcel' not in ret: return None return parse_service_call_bytes(ret) + def parse_service_call_unpack(r, fmt): try: return struct.unpack(fmt, r)[0] except Exception: return None + def parse_service_call_string(r): try: r = r[8:] # Cut off length field @@ -89,6 +43,7 @@ def parse_service_call_string(r): except Exception: return None + def parse_service_call_bytes(ret): try: r = b"" @@ -98,189 +53,248 @@ def parse_service_call_bytes(ret): except Exception: return None -def get_network_type(): - if not ANDROID: - return NetworkType.none - - wifi_check = parse_service_call_string(service_call(["connectivity", "2"])) - if wifi_check is None: - return NetworkType.none - elif 'WIFI' in wifi_check: - return NetworkType.wifi - else: - cell_check = parse_service_call_unpack(service_call(['phone', '59']), ">q") - # from TelephonyManager.java - cell_networks = { - 0: NetworkType.none, - 1: NetworkType.cell2G, - 2: NetworkType.cell2G, - 3: NetworkType.cell3G, - 4: NetworkType.cell2G, - 5: NetworkType.cell3G, - 6: NetworkType.cell3G, - 7: NetworkType.cell3G, - 8: NetworkType.cell3G, - 9: NetworkType.cell3G, - 10: NetworkType.cell3G, - 11: NetworkType.cell2G, - 12: NetworkType.cell3G, - 13: NetworkType.cell4G, - 14: NetworkType.cell4G, - 15: NetworkType.cell3G, - 16: NetworkType.cell2G, - 17: NetworkType.cell3G, - 18: NetworkType.cell4G, - 19: NetworkType.cell4G - } - return cell_networks.get(cell_check, NetworkType.none) - -def get_network_strength(network_type): - network_strength = NetworkStrength.unknown - - # from SignalStrength.java - def get_lte_level(rsrp, rssnr): - INT_MAX = 2147483647 - if rsrp == INT_MAX: - lvl_rsrp = NetworkStrength.unknown - elif rsrp >= -95: - lvl_rsrp = NetworkStrength.great - elif rsrp >= -105: - lvl_rsrp = NetworkStrength.good - elif rsrp >= -115: - lvl_rsrp = NetworkStrength.moderate + +def getprop(key): + return subprocess.check_output(["getprop", key], encoding='utf8').strip() + + +class Android(HardwareBase): + def get_sound_card_online(self): + return (os.path.isfile('/proc/asound/card0/state') and + open('/proc/asound/card0/state').read().strip() == 'ONLINE') + + def get_imei(self, slot): + slot = str(slot) + if slot not in ("0", "1"): + raise ValueError("SIM slot must be 0 or 1") + + return parse_service_call_string(service_call(["iphonesubinfo", "3", "i32", str(slot)])) + + def get_serial(self): + ret = getprop("ro.serialno") + if ret == "": + ret = "cccccccc" + return ret + + def get_subscriber_info(self): + ret = parse_service_call_string(service_call(["iphonesubinfo", "7"])) + if ret is None or len(ret) < 8: + return "" + return ret + + def reboot(self, reason=None): + # e.g. reason="recovery" to go into recover mode + if reason is None: + reason_args = ["null"] else: - lvl_rsrp = NetworkStrength.poor - if rssnr == INT_MAX: - lvl_rssnr = NetworkStrength.unknown - elif rssnr >= 45: - lvl_rssnr = NetworkStrength.great - elif rssnr >= 10: - lvl_rssnr = NetworkStrength.good - elif rssnr >= -30: - lvl_rssnr = NetworkStrength.moderate + reason_args = ["s16", reason] + + subprocess.check_output([ + "service", "call", "power", "16", # IPowerManager.reboot + "i32", "0", # no confirmation, + *reason_args, + "i32", "1" # wait + ]) + + def get_sim_info(self): + # Used for athena + # TODO: build using methods from this class + sim_state = getprop("gsm.sim.state").split(",") + network_type = getprop("gsm.network.type").split(',') + mcc_mnc = getprop("gsm.sim.operator.numeric") or None + + sim_id = parse_service_call_string(service_call(['iphonesubinfo', '11'])) + cell_data_state = parse_service_call_unpack(service_call(['phone', '46']), ">q") + cell_data_connected = (cell_data_state == 2) + + return { + 'sim_id': sim_id, + 'mcc_mnc': mcc_mnc, + 'network_type': network_type, + 'sim_state': sim_state, + 'data_connected': cell_data_connected + } + + def get_network_type(self): + wifi_check = parse_service_call_string(service_call(["connectivity", "2"])) + if wifi_check is None: + return NetworkType.none + elif 'WIFI' in wifi_check: + return NetworkType.wifi else: - lvl_rssnr = NetworkStrength.poor - return max(lvl_rsrp, lvl_rssnr) + cell_check = parse_service_call_unpack(service_call(['phone', '59']), ">q") + # from TelephonyManager.java + cell_networks = { + 0: NetworkType.none, + 1: NetworkType.cell2G, + 2: NetworkType.cell2G, + 3: NetworkType.cell3G, + 4: NetworkType.cell2G, + 5: NetworkType.cell3G, + 6: NetworkType.cell3G, + 7: NetworkType.cell3G, + 8: NetworkType.cell3G, + 9: NetworkType.cell3G, + 10: NetworkType.cell3G, + 11: NetworkType.cell2G, + 12: NetworkType.cell3G, + 13: NetworkType.cell4G, + 14: NetworkType.cell4G, + 15: NetworkType.cell3G, + 16: NetworkType.cell2G, + 17: NetworkType.cell3G, + 18: NetworkType.cell4G, + 19: NetworkType.cell4G + } + return cell_networks.get(cell_check, NetworkType.none) - def get_tdscdma_level(tdscmadbm): - lvl = NetworkStrength.unknown - if tdscmadbm > -25: - lvl = NetworkStrength.unknown - elif tdscmadbm >= -49: - lvl = NetworkStrength.great - elif tdscmadbm >= -73: - lvl = NetworkStrength.good - elif tdscmadbm >= -97: - lvl = NetworkStrength.moderate - elif tdscmadbm >= -110: - lvl = NetworkStrength.poor - return lvl - - def get_gsm_level(asu): - if asu <= 2 or asu == 99: + def get_network_strength(self, network_type): + network_strength = NetworkStrength.unknown + + # from SignalStrength.java + def get_lte_level(rsrp, rssnr): + INT_MAX = 2147483647 + if rsrp == INT_MAX: + lvl_rsrp = NetworkStrength.unknown + elif rsrp >= -95: + lvl_rsrp = NetworkStrength.great + elif rsrp >= -105: + lvl_rsrp = NetworkStrength.good + elif rsrp >= -115: + lvl_rsrp = NetworkStrength.moderate + else: + lvl_rsrp = NetworkStrength.poor + if rssnr == INT_MAX: + lvl_rssnr = NetworkStrength.unknown + elif rssnr >= 45: + lvl_rssnr = NetworkStrength.great + elif rssnr >= 10: + lvl_rssnr = NetworkStrength.good + elif rssnr >= -30: + lvl_rssnr = NetworkStrength.moderate + else: + lvl_rssnr = NetworkStrength.poor + return max(lvl_rsrp, lvl_rssnr) + + def get_tdscdma_level(tdscmadbm): lvl = NetworkStrength.unknown - elif asu >= 12: - lvl = NetworkStrength.great - elif asu >= 8: - lvl = NetworkStrength.good - elif asu >= 5: - lvl = NetworkStrength.moderate + if tdscmadbm > -25: + lvl = NetworkStrength.unknown + elif tdscmadbm >= -49: + lvl = NetworkStrength.great + elif tdscmadbm >= -73: + lvl = NetworkStrength.good + elif tdscmadbm >= -97: + lvl = NetworkStrength.moderate + elif tdscmadbm >= -110: + lvl = NetworkStrength.poor + return lvl + + def get_gsm_level(asu): + if asu <= 2 or asu == 99: + lvl = NetworkStrength.unknown + elif asu >= 12: + lvl = NetworkStrength.great + elif asu >= 8: + lvl = NetworkStrength.good + elif asu >= 5: + lvl = NetworkStrength.moderate + else: + lvl = NetworkStrength.poor + return lvl + + def get_evdo_level(evdodbm, evdosnr): + lvl_evdodbm = NetworkStrength.unknown + lvl_evdosnr = NetworkStrength.unknown + if evdodbm >= -65: + lvl_evdodbm = NetworkStrength.great + elif evdodbm >= -75: + lvl_evdodbm = NetworkStrength.good + elif evdodbm >= -90: + lvl_evdodbm = NetworkStrength.moderate + elif evdodbm >= -105: + lvl_evdodbm = NetworkStrength.poor + if evdosnr >= 7: + lvl_evdosnr = NetworkStrength.great + elif evdosnr >= 5: + lvl_evdosnr = NetworkStrength.good + elif evdosnr >= 3: + lvl_evdosnr = NetworkStrength.moderate + elif evdosnr >= 1: + lvl_evdosnr = NetworkStrength.poor + return max(lvl_evdodbm, lvl_evdosnr) + + def get_cdma_level(cdmadbm, cdmaecio): + lvl_cdmadbm = NetworkStrength.unknown + lvl_cdmaecio = NetworkStrength.unknown + if cdmadbm >= -75: + lvl_cdmadbm = NetworkStrength.great + elif cdmadbm >= -85: + lvl_cdmadbm = NetworkStrength.good + elif cdmadbm >= -95: + lvl_cdmadbm = NetworkStrength.moderate + elif cdmadbm >= -100: + lvl_cdmadbm = NetworkStrength.poor + if cdmaecio >= -90: + lvl_cdmaecio = NetworkStrength.great + elif cdmaecio >= -110: + lvl_cdmaecio = NetworkStrength.good + elif cdmaecio >= -130: + lvl_cdmaecio = NetworkStrength.moderate + elif cdmaecio >= -150: + lvl_cdmaecio = NetworkStrength.poor + return max(lvl_cdmadbm, lvl_cdmaecio) + + if network_type == NetworkType.none: + return network_strength + if network_type == NetworkType.wifi: + out = subprocess.check_output('dumpsys connectivity', shell=True).decode('utf-8') + network_strength = NetworkStrength.unknown + for line in out.split('\n'): + signal_str = "SignalStrength: " + if signal_str in line: + lvl_idx_start = line.find(signal_str) + len(signal_str) + lvl_idx_end = line.find(']', lvl_idx_start) + lvl = int(line[lvl_idx_start : lvl_idx_end]) + if lvl >= -50: + network_strength = NetworkStrength.great + elif lvl >= -60: + network_strength = NetworkStrength.good + elif lvl >= -70: + network_strength = NetworkStrength.moderate + else: + network_strength = NetworkStrength.poor + return network_strength else: - lvl = NetworkStrength.poor - return lvl - - def get_evdo_level(evdodbm, evdosnr): - lvl_evdodbm = NetworkStrength.unknown - lvl_evdosnr = NetworkStrength.unknown - if evdodbm >= -65: - lvl_evdodbm = NetworkStrength.great - elif evdodbm >= -75: - lvl_evdodbm = NetworkStrength.good - elif evdodbm >= -90: - lvl_evdodbm = NetworkStrength.moderate - elif evdodbm >= -105: - lvl_evdodbm = NetworkStrength.poor - if evdosnr >= 7: - lvl_evdosnr = NetworkStrength.great - elif evdosnr >= 5: - lvl_evdosnr = NetworkStrength.good - elif evdosnr >= 3: - lvl_evdosnr = NetworkStrength.moderate - elif evdosnr >= 1: - lvl_evdosnr = NetworkStrength.poor - return max(lvl_evdodbm, lvl_evdosnr) - - def get_cdma_level(cdmadbm, cdmaecio): - lvl_cdmadbm = NetworkStrength.unknown - lvl_cdmaecio = NetworkStrength.unknown - if cdmadbm >= -75: - lvl_cdmadbm = NetworkStrength.great - elif cdmadbm >= -85: - lvl_cdmadbm = NetworkStrength.good - elif cdmadbm >= -95: - lvl_cdmadbm = NetworkStrength.moderate - elif cdmadbm >= -100: - lvl_cdmadbm = NetworkStrength.poor - if cdmaecio >= -90: - lvl_cdmaecio = NetworkStrength.great - elif cdmaecio >= -110: - lvl_cdmaecio = NetworkStrength.good - elif cdmaecio >= -130: - lvl_cdmaecio = NetworkStrength.moderate - elif cdmaecio >= -150: - lvl_cdmaecio = NetworkStrength.poor - return max(lvl_cdmadbm, lvl_cdmaecio) - - if network_type == NetworkType.none: - return network_strength - if network_type == NetworkType.wifi: - out = subprocess.check_output('dumpsys connectivity', shell=True).decode('utf-8') - network_strength = NetworkStrength.unknown - for line in out.split('\n'): - signal_str = "SignalStrength: " - if signal_str in line: - lvl_idx_start = line.find(signal_str) + len(signal_str) - lvl_idx_end = line.find(']', lvl_idx_start) - lvl = int(line[lvl_idx_start : lvl_idx_end]) - if lvl >= -50: - network_strength = NetworkStrength.great - elif lvl >= -60: - network_strength = NetworkStrength.good - elif lvl >= -70: - network_strength = NetworkStrength.moderate - else: - network_strength = NetworkStrength.poor - return network_strength - else: - # check cell strength - out = subprocess.check_output('dumpsys telephony.registry', shell=True).decode('utf-8') - for line in out.split('\n'): - if "mSignalStrength" in line: - arr = line.split(' ') - ns = 0 - if ("gsm" in arr[14]): - rsrp = int(arr[9]) - rssnr = int(arr[11]) - ns = get_lte_level(rsrp, rssnr) - if ns == NetworkStrength.unknown: - tdscmadbm = int(arr[13]) - ns = get_tdscdma_level(tdscmadbm) + # check cell strength + out = subprocess.check_output('dumpsys telephony.registry', shell=True).decode('utf-8') + for line in out.split('\n'): + if "mSignalStrength" in line: + arr = line.split(' ') + ns = 0 + if ("gsm" in arr[14]): + rsrp = int(arr[9]) + rssnr = int(arr[11]) + ns = get_lte_level(rsrp, rssnr) if ns == NetworkStrength.unknown: - asu = int(arr[1]) - ns = get_gsm_level(asu) - else: - cdmadbm = int(arr[3]) - cdmaecio = int(arr[4]) - evdodbm = int(arr[5]) - evdosnr = int(arr[7]) - lvl_cdma = get_cdma_level(cdmadbm, cdmaecio) - lvl_edmo = get_evdo_level(evdodbm, evdosnr) - if lvl_edmo == NetworkStrength.unknown: - ns = lvl_cdma - elif lvl_cdma == NetworkStrength.unknown: - ns = lvl_edmo + tdscmadbm = int(arr[13]) + ns = get_tdscdma_level(tdscmadbm) + if ns == NetworkStrength.unknown: + asu = int(arr[1]) + ns = get_gsm_level(asu) else: - ns = min(lvl_cdma, lvl_edmo) - network_strength = max(network_strength, ns) + cdmadbm = int(arr[3]) + cdmaecio = int(arr[4]) + evdodbm = int(arr[5]) + evdosnr = int(arr[7]) + lvl_cdma = get_cdma_level(cdmadbm, cdmaecio) + lvl_edmo = get_evdo_level(evdodbm, evdosnr) + if lvl_edmo == NetworkStrength.unknown: + ns = lvl_cdma + elif lvl_cdma == NetworkStrength.unknown: + ns = lvl_edmo + else: + ns = min(lvl_cdma, lvl_edmo) + network_strength = max(network_strength, ns) - return network_strength + return network_strength diff --git a/common/basedir.py b/common/basedir.py index 4d62fdc19c..d98509ed6d 100644 --- a/common/basedir.py +++ b/common/basedir.py @@ -1,10 +1,10 @@ import os BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) -from common.android import ANDROID -if ANDROID: - PERSIST = "/persist" - PARAMS = "/data/params" -else: +from common.hardware import PC +if PC: PERSIST = os.path.join(BASEDIR, "persist") PARAMS = os.path.join(BASEDIR, "persist", "params") +else: + PERSIST = "/persist" + PARAMS = "/data/params" diff --git a/common/hardware.py b/common/hardware.py index a1bab3c313..28cfe53451 100644 --- a/common/hardware.py +++ b/common/hardware.py @@ -1,4 +1,54 @@ import os +import random +from typing import cast + +from cereal import log +from common.android import Android +from common.hardware_base import HardwareBase EON = os.path.isfile('/EON') TICI = os.path.isfile('/TICI') +PC = not (EON or TICI) +ANDROID = EON + + +NetworkType = log.ThermalData.NetworkType +NetworkStrength = log.ThermalData.NetworkStrength + + +class Pc(HardwareBase): + def get_sound_card_online(self): + return True + + def get_imei(self, slot): + return "%015d" % random.randint(0, 1 << 32) + + def get_serial(self): + return "cccccccc" + + def get_subscriber_info(self): + return "" + + def reboot(self, reason=None): + print("REBOOT!") + + def get_network_type(self): + return NetworkType.none + + def get_sim_info(self): + return { + 'sim_id': '', + 'mcc_mnc': None, + 'network_type': ["Unknown"], + 'sim_state': ["ABSENT"], + 'data_connected': False + } + + def get_network_strength(self, network_type): + return NetworkStrength.unknown + + +if EON: + HARDWARE = cast(HardwareBase, Android()) +else: + HARDWARE = cast(HardwareBase, Pc()) diff --git a/common/hardware_base.py b/common/hardware_base.py new file mode 100644 index 0000000000..d5cc08253b --- /dev/null +++ b/common/hardware_base.py @@ -0,0 +1,34 @@ +from abc import abstractmethod + +class HardwareBase: + @abstractmethod + def get_sound_card_online(self): + pass + + @abstractmethod + def get_imei(self, slot): + pass + + @abstractmethod + def get_serial(self): + pass + + @abstractmethod + def get_subscriber_info(self): + pass + + @abstractmethod + def reboot(self, reason=None): + pass + + @abstractmethod + def get_network_type(self): + pass + + @abstractmethod + def get_sim_info(self): + pass + + @abstractmethod + def get_network_strength(self, network_type): + pass diff --git a/common/realtime.py b/common/realtime.py index e734438646..5e5fb709ca 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -6,7 +6,7 @@ import subprocess import multiprocessing from cffi import FFI -from common.android import ANDROID +from common.hardware import ANDROID from common.common_pyx import sec_since_boot # pylint: disable=no-name-in-module, import-error diff --git a/release/files_common b/release/files_common index bf350b7e3a..02bee33d6c 100644 --- a/release/files_common +++ b/release/files_common @@ -19,6 +19,7 @@ common/.gitignore common/__init__.py common/android.py common/hardware.py +common/hardware_base.py common/gpio.py common/realtime.py common/clock.pyx diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 72e81a58bb..6f34cbed7c 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -20,7 +20,7 @@ from websocket import ABNF, WebSocketTimeoutException, create_connection import cereal.messaging as messaging from cereal.services import service_list -from common import android +from common.hardware import HARDWARE from common.api import Api from common.basedir import PERSIST from common.params import Params @@ -132,7 +132,7 @@ def reboot(): def do_reboot(): time.sleep(2) - android.reboot() + HARDWARE.reboot() threading.Thread(target=do_reboot).start() @@ -218,21 +218,7 @@ def getSshAuthorizedKeys(): @dispatcher.add_method def getSimInfo(): - sim_state = android.getprop("gsm.sim.state").split(",") - network_type = android.getprop("gsm.network.type").split(',') - mcc_mnc = android.getprop("gsm.sim.operator.numeric") or None - - sim_id = android.parse_service_call_string(android.service_call(['iphonesubinfo', '11'])) - cell_data_state = android.parse_service_call_unpack(android.service_call(['phone', '46']), ">q") - cell_data_connected = (cell_data_state == 2) - - return { - 'sim_id': sim_id, - 'mcc_mnc': mcc_mnc, - 'network_type': network_type, - 'sim_state': sim_state, - 'data_connected': cell_data_connected - } + return HARDWARE.get_sim_info() @dispatcher.add_method diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py index d6c110bbbe..8e1ec9ca25 100755 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -8,7 +8,7 @@ from functools import wraps import cereal.messaging as messaging from cereal import car from common.basedir import PARAMS -from common.android import ANDROID +from common.hardware import ANDROID from common.params import Params from common.spinner import Spinner from panda import Panda diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2546a598af..0b240ca5a9 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -2,7 +2,7 @@ import os import gc from cereal import car, log -from common.android import ANDROID, get_sound_card_online +from common.hardware import HARDWARE from common.numpy_fast import clip from common.realtime import sec_since_boot, set_realtime_priority, set_core_affinity, Ratekeeper, DT_CTRL from common.profiler import Profiler @@ -79,7 +79,7 @@ class Controls: internet_needed or not openpilot_enabled_toggle # detect sound card presence and ensure successful init - sounds_available = not ANDROID or get_sound_card_online() + sounds_available = HARDWARE.get_sound_card_online() car_recognized = self.CP.carName != 'mock' # If stock camera is disconnected, we loaded car controls and it's not dashcam mode diff --git a/selfdrive/crash.py b/selfdrive/crash.py index 7459f71668..22d6aa0680 100644 --- a/selfdrive/crash.py +++ b/selfdrive/crash.py @@ -6,9 +6,9 @@ import capnp from selfdrive.version import version, dirty from selfdrive.swaglog import cloudlog -from common.android import ANDROID +from common.hardware import PC -if os.getenv("NOLOG") or os.getenv("NOCRASH") or not ANDROID: +if os.getenv("NOLOG") or os.getenv("NOCRASH") or PC: def capture_exception(*args, **kwargs): pass diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 34bfc78959..453fe90ca1 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -1,24 +1,26 @@ #!/usr/bin/env python3 +import ctypes +import inspect +import json import os +import random import re +import subprocess +import threading import time -import json -import random -import ctypes -import inspect -import requests import traceback -import threading -import subprocess -from selfdrive.swaglog import cloudlog -from selfdrive.loggerd.config import ROOT +import requests -from common import android -from common.params import Params +from cereal import log +from common.hardware import HARDWARE from common.api import Api +from common.params import Params from common.xattr import getxattr, setxattr +from selfdrive.loggerd.config import ROOT +from selfdrive.swaglog import cloudlog +NetworkType = log.ThermalData.NetworkType UPLOAD_ATTR_NAME = 'user.upload' UPLOAD_ATTR_VALUE = b'1' @@ -69,13 +71,8 @@ def clear_locks(root): cloudlog.exception("clear_locks failed") def is_on_wifi(): - # ConnectivityManager.getActiveNetworkInfo() try: - # TODO: figure out why the android service call sometimes dies with SIGUSR2 (signal from MSGQ) - result = android.parse_service_call_string(android.service_call(["connectivity", "2"])) - if result is None: - return True - return 'WIFI' in result + return HARDWARE.get_network_type() == NetworkType.wifi except Exception: cloudlog.exception("is_on_wifi failed") return False diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 75168781b3..90b6b221ec 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -14,7 +14,7 @@ from selfdrive.swaglog import cloudlog, add_logentries_handler from common.basedir import BASEDIR, PARAMS -from common.android import ANDROID +from common.hardware import HARDWARE, ANDROID, PC WEBCAM = os.getenv("WEBCAM") is not None sys.path.append(os.path.join(BASEDIR, "pyextra")) os.environ['BASEDIR'] = BASEDIR @@ -156,7 +156,6 @@ from selfdrive.registration import register from selfdrive.version import version, dirty from selfdrive.loggerd.config import ROOT from selfdrive.launcher import launcher -from common import android from common.apk import update_apks, pm_apply_packages, start_offroad ThermalStatus = cereal.log.ThermalData.ThermalStatus @@ -253,13 +252,18 @@ if WEBCAM: 'dmonitoringmodeld', ] -if ANDROID: +if not PC: car_started_processes += [ 'sensord', - 'gpsd', 'dmonitoringmodeld', ] +if ANDROID: + car_started_processes += [ + 'gpsd', + ] + + def register_managed_process(name, desc, car_started=False): global managed_processes, car_started_processes, persistent_processes print("registering %s" % name) @@ -365,6 +369,7 @@ def kill_managed_process(name): join_process(running[name], 15) if running[name].exitcode is None: cloudlog.critical("unkillable process %s failed to die!" % name) + # TODO: Use method from HARDWARE if ANDROID: cloudlog.critical("FORCE REBOOTING PHONE!") os.system("date >> /sdcard/unkillable_reboot") @@ -536,7 +541,7 @@ def uninstall(): with open('/cache/recovery/command', 'w') as f: f.write('--wipe_data\n') # IPowerManager.reboot(confirm=false, reason="recovery", wait=true) - android.reboot(reason="recovery") + HARDWARE.reboot(reason="recovery") def main(): os.environ['PARAMS_PATH'] = PARAMS diff --git a/selfdrive/registration.py b/selfdrive/registration.py index 6e1596f35a..de2231a458 100644 --- a/selfdrive/registration.py +++ b/selfdrive/registration.py @@ -4,12 +4,13 @@ import json from datetime import datetime, timedelta from selfdrive.swaglog import cloudlog from selfdrive.version import version, terms_version, training_version, get_git_commit, get_git_branch, get_git_remote -from common.android import get_imei, get_serial, get_subscriber_info +from common.hardware import HARDWARE from common.api import api_get from common.params import Params from common.file_helpers import mkdirs_exists_ok from common.basedir import PERSIST + def register(): params = Params() params.put("Version", version) @@ -19,7 +20,7 @@ def register(): params.put("GitCommit", get_git_commit(default="")) params.put("GitBranch", get_git_branch(default="")) params.put("GitRemote", get_git_remote(default="")) - params.put("SubscriberInfo", get_subscriber_info()) + params.put("SubscriberInfo", HARDWARE.get_subscriber_info()) # create a key for auth # your private key is kept on your device persist partition and never sent to our servers @@ -50,7 +51,7 @@ def register(): try: cloudlog.info("getting pilotauth") resp = api_get("v2/pilotauth/", method='POST', timeout=15, - imei=get_imei(0), imei2=get_imei(1), serial=get_serial(), public_key=public_key, register_token=register_token) + imei=HARDWARE.get_imei(0), imei2=HARDWARE.get_imei(1), serial=HARDWARE.get_serial(), public_key=public_key, register_token=register_token) dongleauth = json.loads(resp.text) dongle_id = dongleauth["dongle_id"] diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index 8e852e997f..47d302fff4 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -3,7 +3,7 @@ import subprocess from functools import wraps from nose.tools import nottest -from common.android import ANDROID +from common.hardware import PC from common.apk import update_apks, start_offroad, pm_apply_packages, android_packages from common.params import Params from selfdrive.version import training_version, terms_version @@ -19,10 +19,10 @@ def set_params_enabled(): params.put("CompletedTrainingVersion", training_version) def phone_only(x): - if ANDROID: - return x - else: + if PC: return nottest(x) + else: + return x def with_processes(processes, init_time=0): def wrapper(func): @@ -68,4 +68,3 @@ def with_apks(): assert apk_is_not_running, package return wrap return wrapper - diff --git a/selfdrive/test/process_replay/camera_replay.py b/selfdrive/test/process_replay/camera_replay.py index 501f8f6111..fb50b0c4af 100755 --- a/selfdrive/test/process_replay/camera_replay.py +++ b/selfdrive/test/process_replay/camera_replay.py @@ -5,7 +5,7 @@ import time from typing import Any from tqdm import tqdm -from common.android import ANDROID +from common.hardware import ANDROID os.environ['CI'] = "1" if ANDROID: os.environ['QCOM_REPLAY'] = "1" @@ -101,4 +101,3 @@ if __name__ == "__main__": f.write(diff2) sys.exit(int(failed)) - diff --git a/selfdrive/test/test_sounds.py b/selfdrive/test/test_sounds.py index e9946efb0f..2ee7b0826c 100755 --- a/selfdrive/test/test_sounds.py +++ b/selfdrive/test/test_sounds.py @@ -5,7 +5,7 @@ import subprocess from cereal import log, car import cereal.messaging as messaging from selfdrive.test.helpers import phone_only, with_processes -from common.android import get_sound_card_online +from common.hardware import HARDWARE from common.realtime import DT_CTRL AudibleAlert = car.CarControl.HUDControl.AudibleAlert @@ -30,7 +30,7 @@ def get_total_writes(): @phone_only def test_sound_card_init(): - assert get_sound_card_online() + assert HARDWARE.get_sound_card_online() @phone_only diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 63f1a7e9b6..255015a761 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -9,9 +9,8 @@ from smbus2 import SMBus import cereal.messaging as messaging from cereal import log -from common.android import get_network_strength, get_network_type from common.filter_simple import FirstOrderFilter -from common.hardware import EON, TICI +from common.hardware import EON, HARDWARE, TICI from common.numpy_fast import clip, interp from common.params import Params, put_nonblocking from common.realtime import DT_TRML, sec_since_boot @@ -241,8 +240,8 @@ def thermald_thread(): # get_network_type is an expensive call. update every 10s if (count % int(10. / DT_TRML)) == 0: try: - network_type = get_network_type() - network_strength = get_network_strength(network_type) + network_type = HARDWARE.get_network_type() + network_strength = HARDWARE.get_network_strength(network_type) except Exception: cloudlog.exception("Error getting network status") diff --git a/tools/lib/auth_config.py b/tools/lib/auth_config.py index 0cc3e33b51..b59fa2e89f 100644 --- a/tools/lib/auth_config.py +++ b/tools/lib/auth_config.py @@ -1,17 +1,21 @@ import json import os -from common.android import ANDROID +from common.hardware import PC from common.file_helpers import mkdirs_exists_ok + class MissingAuthConfigError(Exception): pass -if ANDROID: - CONFIG_DIR = "/tmp/.comma" -else: + +if PC: CONFIG_DIR = os.path.expanduser('~/.comma') +else: + CONFIG_DIR = "/tmp/.comma" + mkdirs_exists_ok(CONFIG_DIR) + def get_token(): try: with open(os.path.join(CONFIG_DIR, 'auth.json')) as f: @@ -20,9 +24,11 @@ def get_token(): except Exception: raise MissingAuthConfigError('Authenticate with tools/lib/auth.py') + def set_token(token): with open(os.path.join(CONFIG_DIR, 'auth.json'), 'w') as f: json.dump({'access_token': token}, f) + def clear_token(): os.unlink(os.path.join(CONFIG_DIR, 'auth.json')) From 5782efe3ae7882a6a8eee0086b9c612c1d276814 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 26 Aug 2020 17:09:23 +0200 Subject: [PATCH 06/11] Systemd logcatd (#2085) * Systemd logcatd infrastructure * This should work * Cleanup * Split * More compact and cleanup * Add to ubuntu setup package list * Run logcatd on all non pc platforms --- Dockerfile.openpilot_base | 1 + SConstruct | 2 +- release/files_common | 3 +- selfdrive/logcatd/SConscript | 8 +- .../{logcatd.cc => logcatd_android.cc} | 0 selfdrive/logcatd/logcatd_systemd.cc | 73 +++++++++++++++++++ selfdrive/manager.py | 6 +- tools/ubuntu_setup.sh | 1 + 8 files changed, 89 insertions(+), 5 deletions(-) rename selfdrive/logcatd/{logcatd.cc => logcatd_android.cc} (100%) create mode 100644 selfdrive/logcatd/logcatd_systemd.cc diff --git a/Dockerfile.openpilot_base b/Dockerfile.openpilot_base index 4cec8a36a7..25ac0826c8 100644 --- a/Dockerfile.openpilot_base +++ b/Dockerfile.openpilot_base @@ -27,6 +27,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ libopencv-dev \ libssl-dev \ libsqlite3-dev \ + libsystemd-dev \ libusb-1.0-0-dev \ libczmq-dev \ libzmq3-dev \ diff --git a/SConstruct b/SConstruct index a12fa929b4..3dffd914ad 100644 --- a/SConstruct +++ b/SConstruct @@ -311,7 +311,7 @@ SConscript(['selfdrive/locationd/SConscript']) SConscript(['selfdrive/locationd/models/SConscript']) SConscript(['selfdrive/sensord/SConscript']) -if arch == "aarch64": +if arch != "Darwin": SConscript(['selfdrive/logcatd/SConscript']) if arch != "larch64": diff --git a/release/files_common b/release/files_common index 02bee33d6c..6238bb4e63 100644 --- a/release/files_common +++ b/release/files_common @@ -294,7 +294,8 @@ selfdrive/locationd/calibrationd.py selfdrive/locationd/calibration_helpers.py selfdrive/logcatd/SConscript -selfdrive/logcatd/logcatd.cc +selfdrive/logcatd/logcatd_android.cc +selfdrive/logcatd/logcatd_systemd.cc selfdrive/proclogd/SConscript selfdrive/proclogd/proclogd.cc diff --git a/selfdrive/logcatd/SConscript b/selfdrive/logcatd/SConscript index 67ad673002..7f87cbaddf 100644 --- a/selfdrive/logcatd/SConscript +++ b/selfdrive/logcatd/SConscript @@ -1,2 +1,6 @@ -Import('env', 'cereal', 'messaging') -env.Program('logcatd.cc', LIBS=[cereal, messaging, 'cutils', 'zmq', 'czmq', 'capnp', 'kj']) +Import('env', 'cereal', 'messaging', 'arch') + +if arch == "aarch64": + env.Program('logcatd', 'logcatd_android.cc', LIBS=[cereal, messaging, 'cutils', 'zmq', 'czmq', 'capnp', 'kj']) +else: + env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[cereal, messaging, 'zmq', 'capnp', 'kj', 'systemd', 'json11']) diff --git a/selfdrive/logcatd/logcatd.cc b/selfdrive/logcatd/logcatd_android.cc similarity index 100% rename from selfdrive/logcatd/logcatd.cc rename to selfdrive/logcatd/logcatd_android.cc diff --git a/selfdrive/logcatd/logcatd_systemd.cc b/selfdrive/logcatd/logcatd_systemd.cc new file mode 100644 index 0000000000..3cbba3f1a1 --- /dev/null +++ b/selfdrive/logcatd/logcatd_systemd.cc @@ -0,0 +1,73 @@ +#include +#include +#include +#include + +#include "json11.hpp" +#include + +#include "common/timing.h" +#include "messaging.hpp" + +int main(int argc, char *argv[]) { + PubMaster pm({"androidLog"}); + + sd_journal *journal; + assert(sd_journal_open(&journal, 0) >= 0); + assert(sd_journal_get_fd(journal) >= 0); // needed so sd_journal_wait() works properly if files rotate + assert(sd_journal_seek_tail(journal) >= 0); + + int r; + while (true) { + r = sd_journal_next(journal); + assert(r >= 0); + + // Wait for new message if we didn't receive anything + if (r == 0){ + do { + r = sd_journal_wait(journal, (uint64_t)-1); + assert(r >= 0); + } while (r == SD_JOURNAL_NOP); + + r = sd_journal_next(journal); + assert(r >= 0); + + if (r == 0) continue; // Try again if we still didn't get anything + } + + uint64_t timestamp = 0; + r = sd_journal_get_realtime_usec(journal, ×tamp); + assert(r >= 0); + + const void *data; + size_t length; + std::map kv; + + SD_JOURNAL_FOREACH_DATA(journal, data, length){ + std::string str((char*)data, length); + + // Split "KEY=VALUE"" on "=" and put in map + std::size_t found = str.find("="); + if (found != std::string::npos){ + kv[str.substr(0, found)] = str.substr(found + 1, std::string::npos); + } + } + + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + // Build message + auto androidEntry = event.initAndroidLog(); + androidEntry.setTs(timestamp); + androidEntry.setMessage(json11::Json(kv).dump()); + if (kv.count("_PID")) androidEntry.setPid(std::atoi(kv["_PID"].c_str())); + if (kv.count("PRIORITY")) androidEntry.setPriority(std::atoi(kv["PRIORITY"].c_str())); + if (kv.count("SYSLOG_IDENTIFIER")) androidEntry.setTag(kv["SYSLOG_IDENTIFIER"]); + + pm.send("androidLog", msg); + } + + sd_journal_close(journal); + return 0; +} diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 90b6b221ec..c81c6f27d6 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -217,9 +217,13 @@ persistent_processes = [ 'uploader', ] -if ANDROID: +if not PC: persistent_processes += [ 'logcatd', + ] + +if ANDROID: + persistent_processes += [ 'tombstoned', 'updated', 'deleter', diff --git a/tools/ubuntu_setup.sh b/tools/ubuntu_setup.sh index 6b36369771..94c0850da1 100755 --- a/tools/ubuntu_setup.sh +++ b/tools/ubuntu_setup.sh @@ -35,6 +35,7 @@ sudo apt-get update && sudo apt-get install -y \ libczmq-dev \ libsdl-image1.2-dev libsdl-mixer1.2-dev libsdl-ttf2.0-dev libsmpeg-dev \ libsdl1.2-dev libportmidi-dev libswscale-dev libavformat-dev libavcodec-dev libfreetype6-dev \ + libsystemd-dev \ locales \ ocl-icd-libopencl1 \ ocl-icd-opencl-dev \ From cd6ec8211cebc502ef072ea8ebc0e03eab07f358 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 26 Aug 2020 17:18:05 +0200 Subject: [PATCH 07/11] larch64 modeld fixes (#2086) --- selfdrive/modeld/modeld | 6 +++++- selfdrive/modeld/modeld.cc | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/selfdrive/modeld/modeld b/selfdrive/modeld/modeld index 5369034dfe..a9d545e305 100755 --- a/selfdrive/modeld/modeld +++ b/selfdrive/modeld/modeld @@ -1,4 +1,8 @@ #!/bin/sh -export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$HOME/openpilot/phonelibs/snpe/larch64:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" +if [ -d /system ]; then + export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$LD_LIBRARY_PATH" +else + export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/larch64:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" +fi exec ./_modeld diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 9411a0ec02..be47d1811a 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -98,7 +98,7 @@ int main(int argc, char **argv) { PubMaster pm({"model", "cameraOdometry"}); SubMaster sm({"pathPlan", "frame"}); -#ifdef QCOM +#if defined(QCOM) || defined(QCOM2) cl_device_type device_type = CL_DEVICE_TYPE_DEFAULT; #else cl_device_type device_type = CL_DEVICE_TYPE_CPU; From e88edeb051c393d7df84d5cf9b33ad0668bd4e99 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 26 Aug 2020 19:06:56 +0200 Subject: [PATCH 08/11] apport support for tombstoned (#2087) * apport support for tombstoned * Update manager * Update comment --- selfdrive/manager.py | 2 +- selfdrive/tombstoned.py | 43 +++++++++++++++++++++++++++-------------- 2 files changed, 30 insertions(+), 15 deletions(-) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index c81c6f27d6..0113aa1afe 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -220,11 +220,11 @@ persistent_processes = [ if not PC: persistent_processes += [ 'logcatd', + 'tombstoned', ] if ANDROID: persistent_processes += [ - 'tombstoned', 'updated', 'deleter', ] diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 4dc969b5c7..fbebddbd72 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -12,10 +12,16 @@ MAX_SIZE = 100000 * 10 # Normal size is 40-100k, allow up to 1M def get_tombstones(): - """Returns list of (filename, ctime) for all tombstones in /data/tombstones""" - DIR_DATA = "/data/tombstones/" - return [(DIR_DATA + fn, int(os.stat(DIR_DATA + fn).st_ctime)) - for fn in os.listdir(DIR_DATA) if fn.startswith("tombstone")] + """Returns list of (filename, ctime) for all tombstones in /data/tombstones + and apport crashlogs in /var/crash""" + files = [] + for folder in ["/data/tombstones/", "/var/crash/"]: + if os.path.exists(folder): + for fn in os.listdir(folder): + if fn.startswith("tombstone") or fn.endswith(".crash"): + path = os.path.join(folder, fn) + files.append((path, int(os.stat(path).st_ctime))) + return files def report_tombstone(fn, client): @@ -28,17 +34,27 @@ def report_tombstone(fn, client): contents = f.read() # Get summary for sentry title - message = " ".join(contents.split('\n')[5:7]) + if fn.endswith(".crash"): + lines = contents.split('\n') + message = lines[6] - # Cut off pid/tid, since that varies per run - name_idx = message.find('name') - if name_idx >= 0: - message = message[name_idx:] + status_idx = contents.find('ProcStatus') + if status_idx >= 0: + lines = contents[status_idx:].split('\n') + message += " " + lines[1] + else: + message = " ".join(contents.split('\n')[5:7]) + + # Cut off pid/tid, since that varies per run + name_idx = message.find('name') + if name_idx >= 0: + message = message[name_idx:] + + # Cut off fault addr + fault_idx = message.find(', fault addr') + if fault_idx >= 0: + message = message[:fault_idx] - # Cut off fault addr - fault_idx = message.find(', fault addr') - if fault_idx >= 0: - message = message[:fault_idx] cloudlog.error({'tombstone': message}) client.captureMessage( @@ -53,7 +69,6 @@ def report_tombstone(fn, client): def main(): initial_tombstones = set(get_tombstones()) - client = Client('https://d3b175702f62402c91ade04d1c547e68:b20d68c813c74f63a7cdf9c4039d8f56@sentry.io/157615', install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty}, string_max_length=10000) From a7be2b090d39fe360eab1c9cca32a3c4d3bb0f13 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 26 Aug 2020 17:49:20 +0000 Subject: [PATCH 09/11] larch64 dmonitoringmodeld fixes --- selfdrive/modeld/dmonitoringmodeld | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/selfdrive/modeld/dmonitoringmodeld b/selfdrive/modeld/dmonitoringmodeld index 51b520d743..cf6ba8c014 100755 --- a/selfdrive/modeld/dmonitoringmodeld +++ b/selfdrive/modeld/dmonitoringmodeld @@ -1,5 +1,11 @@ #!/bin/sh -export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$HOME/openpilot/phonelibs/snpe/larch64:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" -export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/" +if [ -d /system ]; then + export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$LD_LIBRARY_PATH" + export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/" +else + export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/larch64:$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH" + export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/larch64/" +fi + exec ./_dmonitoringmodeld From c8e0cd3ddd46485c1b70c43a8732001cb6de7f22 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 26 Aug 2020 20:12:56 +0200 Subject: [PATCH 10/11] hardware.py: PC is wifi so uploader works --- common/hardware.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/hardware.py b/common/hardware.py index 28cfe53451..d255219560 100644 --- a/common/hardware.py +++ b/common/hardware.py @@ -33,7 +33,7 @@ class Pc(HardwareBase): print("REBOOT!") def get_network_type(self): - return NetworkType.none + return NetworkType.wifi def get_sim_info(self): return { From 29d83f02463d6963a0826f4997d2ad393447346d Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 26 Aug 2020 20:13:33 +0200 Subject: [PATCH 11/11] bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index ecef0a19d0..6ae6221cf2 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit ecef0a19d0f72d8fd3151593b7bd1a112d5f63e2 +Subproject commit 6ae6221cf2346f1881e38696c28d097a470131c4