Tici Agent
5 years ago
commit
11b55d3f5b
46 changed files with 1195 additions and 350 deletions
@ -1,10 +1,10 @@ |
|||||||
import os |
import os |
||||||
BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) |
BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) |
||||||
|
|
||||||
from common.android import ANDROID |
from common.hardware import PC |
||||||
if ANDROID: |
if PC: |
||||||
PERSIST = "/persist" |
|
||||||
PARAMS = "/data/params" |
|
||||||
else: |
|
||||||
PERSIST = os.path.join(BASEDIR, "persist") |
PERSIST = os.path.join(BASEDIR, "persist") |
||||||
PARAMS = os.path.join(BASEDIR, "persist", "params") |
PARAMS = os.path.join(BASEDIR, "persist", "params") |
||||||
|
else: |
||||||
|
PERSIST = "/persist" |
||||||
|
PARAMS = "/data/params" |
||||||
|
|||||||
@ -1,4 +1,54 @@ |
|||||||
import os |
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') |
EON = os.path.isfile('/EON') |
||||||
TICI = os.path.isfile('/TICI') |
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') |
Import('env', 'cereal', 'messaging', 'arch') |
||||||
env.Program('logcatd.cc', LIBS=[cereal, messaging, 'cutils', 'zmq', 'czmq', 'capnp', 'kj']) |
|
||||||
|
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 |
#!/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 ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/" |
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 |
exec ./_dmonitoringmodeld |
||||||
|
|
||||||
|
|||||||
@ -1,4 +1,8 @@ |
|||||||
#!/bin/sh |
#!/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 |
exec ./_modeld |
||||||
|
|
||||||
|
|||||||
@ -1,5 +1,14 @@ |
|||||||
Import('env', 'common', 'cereal', 'messaging') |
Import('env', 'arch', 'common', 'cereal', 'messaging') |
||||||
env.Program('_sensord', 'sensors.cc', LIBS=['hardware', common, cereal, messaging, 'capnp', 'zmq', 'kj']) |
if arch == "aarch64": |
||||||
lenv = env.Clone() |
env.Program('_sensord', 'sensors_qcom.cc', LIBS=['hardware', common, cereal, messaging, 'capnp', 'zmq', 'kj']) |
||||||
lenv['LIBPATH'] += ['/system/vendor/lib64'] |
lenv = env.Clone() |
||||||
lenv.Program('_gpsd', ['gpsd.cc'], LIBS=['hardware', common, 'diag', 'time_genoff', cereal, messaging, 'capnp', 'zmq', 'kj']) |
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