Tici Agent
5 years ago
commit
11b55d3f5b
46 changed files with 1195 additions and 350 deletions
@ -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" |
||||
|
||||
@ -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.wifi |
||||
|
||||
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()) |
||||
|
||||
@ -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 |
||||
@ -0,0 +1,80 @@ |
||||
#include "i2c.h" |
||||
|
||||
#include <cassert> |
||||
#include <unistd.h> |
||||
#include <stdexcept> |
||||
#include <stdio.h> |
||||
#include <fcntl.h> |
||||
#include <sys/ioctl.h> |
||||
#include "common/swaglog.h" |
||||
|
||||
#define UNUSED(x) (void)(x) |
||||
|
||||
#ifdef QCOM2 |
||||
// TODO: decide if we want to isntall libi2c-dev everywhere
|
||||
#include <linux/i2c-dev.h> |
||||
|
||||
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 |
||||
@ -0,0 +1,16 @@ |
||||
#pragma once |
||||
|
||||
#include <stdint.h> |
||||
#include <sys/types.h> |
||||
|
||||
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); |
||||
}; |
||||
@ -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']) |
||||
|
||||
@ -0,0 +1,73 @@ |
||||
#include <iostream> |
||||
#include <cassert> |
||||
#include <string> |
||||
#include <map> |
||||
|
||||
#include "json11.hpp" |
||||
#include <systemd/sd-journal.h> |
||||
|
||||
#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<std::string, std::string> 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<cereal::Event>(); |
||||
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; |
||||
} |
||||
@ -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=5) |
||||
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() |
||||
@ -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 |
||||
|
||||
|
||||
@ -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 |
||||
|
||||
|
||||
@ -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']) |
||||
|
||||
@ -0,0 +1,71 @@ |
||||
#include <cassert> |
||||
#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<const float> vs(&xyz[0], 3); |
||||
|
||||
auto svec = event.initAcceleration(); |
||||
svec.setV(vs); |
||||
svec.setStatus(true); |
||||
|
||||
} |
||||
@ -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); |
||||
}; |
||||
@ -0,0 +1,81 @@ |
||||
#include <cassert> |
||||
#include <cmath> |
||||
#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<const float> vs(&xyz[0], 3); |
||||
|
||||
auto svec = event.initGyroUncalibrated(); |
||||
svec.setV(vs); |
||||
svec.setStatus(true); |
||||
|
||||
} |
||||
@ -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); |
||||
}; |
||||
@ -0,0 +1,43 @@ |
||||
#include <unistd.h> |
||||
|
||||
#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; |
||||
} |
||||
@ -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){}; |
||||
}; |
||||
@ -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 |
||||
@ -0,0 +1,24 @@ |
||||
#include <iostream> |
||||
#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); |
||||
} |
||||
@ -0,0 +1,23 @@ |
||||
#pragma once |
||||
|
||||
#include <cstdint> |
||||
#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; |
||||
}; |
||||
@ -0,0 +1,87 @@ |
||||
#include <vector> |
||||
#include <csignal> |
||||
#include <chrono> |
||||
#include <thread> |
||||
#include <sys/resource.h> |
||||
|
||||
#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<I2CSensor *> 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){ |
||||
std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); |
||||
uint64_t log_time = nanos_since_boot(); |
||||
|
||||
capnp::MallocMessageBuilder msg; |
||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
||||
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); |
||||
|
||||
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; |
||||
} |
||||
|
||||
int main(int argc, char *argv[]) { |
||||
setpriority(PRIO_PROCESS, 0, -13); |
||||
signal(SIGINT, set_do_exit); |
||||
signal(SIGTERM, set_do_exit); |
||||
|
||||
return sensor_loop(); |
||||
} |
||||
Loading…
Reference in new issue