mawei/onepilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update251203 (#233)

Browse Source
This commit is contained in:
carrot
2025-12-03 10:28:27 +09:00
committed by GitHub
parent d6899edd97
commit c5ebcbcb97
347 changed files with 8678 additions and 13489 deletions
Download Patch File Download Diff File Expand all files Collapse all files
RELEASES.md
+8
View File
@@ -1,3 +1,11 @@
Carrot2-v9 (2025-12-03)
========================
* ST model
* fix CasperEV FCA11
* fix DriverMonitoring alert (for USA)
* apply livePose
* update sensor code
Carrot2-v9 (2025-10-17)
========================
* Nuggets In Dijon model
SConstruct
+2 -6
View File
@@ -330,12 +330,9 @@ Export('env', 'qt_env', 'arch', 'real_arch')
# Build common module
SConscript(['common/SConscript'])
Import('_common', '_gpucommon')
Import('_common')
common = [_common, 'json11', 'zmq']
gpucommon = [_gpucommon]
Export('common', 'gpucommon')
Export('common')
# Build messaging (cereal + msgq + socketmaster + their dependencies)
# Enable swaglog include in submodules
@@ -365,7 +362,6 @@ SConscript([
])
if arch != "Darwin":
SConscript([
'system/sensord/SConscript',
'system/logcatd/SConscript',
])
cereal/services.py
+1 -1
View File
@@ -52,7 +52,7 @@ _services: dict[str, tuple] = {
"clocks": (True, 0.1, 1),
"ubloxRaw": (True, 20.),
"livePose": (True, 20., 4),
"liveLocationKalman": (True, 20., 5),
#"liveLocationKalman": (True, 20., 5),
"liveParameters": (True, 20., 5),
"cameraOdometry": (True, 20., 10),
"thumbnail": (True, 1 / 60., 1),
common/SConscript
+3 -12
View File
@@ -4,22 +4,13 @@ common_libs = [
'params.cc',
'swaglog.cc',
'util.cc',
'i2c.cc',
'watchdog.cc',
'ratekeeper.cc'
]
if arch != "Darwin":
common_libs.append('gpio.cc')
_common = env.Library('common', common_libs, LIBS="json11")
files = [
'ratekeeper.cc',
'clutil.cc',
]
_gpucommon = env.Library('gpucommon', files)
Export('_common', '_gpucommon')
_common = env.Library('common', common_libs, LIBS="json11")
Export('_common')
if GetOption('extras'):
env.Program('tests/test_common',
common/constants.py
+23
View File
@@ -0,0 +1,23 @@
import numpy as np
# conversions
class CV:
# Speed
MPH_TO_KPH = 1.609344
KPH_TO_MPH = 1. / MPH_TO_KPH
MS_TO_KPH = 3.6
KPH_TO_MS = 1. / MS_TO_KPH
MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH
MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS
MS_TO_KNOTS = 1.9438
KNOTS_TO_MS = 1. / MS_TO_KNOTS
# Angle
DEG_TO_RAD = np.pi / 180.
RAD_TO_DEG = 1. / DEG_TO_RAD
# Mass
LB_TO_KG = 0.453592
ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2
common/filter_simple.py
+17
View File
@@ -20,6 +20,23 @@ class FirstOrderFilter:
self.x = x
return self.x
class BounceFilter(FirstOrderFilter):
def __init__(self, x0, rc, dt, initialized=True, bounce=2):
self.velocity = FirstOrderFilter(0.0, 0.15, dt)
self.bounce = bounce
super().__init__(x0, rc, dt, initialized)
def update(self, x):
super().update(x)
scale = self.dt / (1.0 / 60.0) # tuned at 60 fps
self.velocity.x += (x - self.x) * self.bounce * scale * self.dt
self.velocity.update(0.0)
if abs(self.velocity.x) < 1e-5:
self.velocity.x = 0.0
self.x += self.velocity.x
return self.x
class MyMovingAverage:
def __init__(self, window_size, value=None):
self.window_size = window_size
common/gpio.cc
-84
View File
@@ -1,84 +0,0 @@
#include "common/gpio.h"
#include <string>
#ifdef __APPLE__
int gpio_init(int pin_nr, bool output) {
return 0;
}
int gpio_set(int pin_nr, bool high) {
return 0;
}
int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr) {
return 0;
}
#else
#include <fcntl.h>
#include <unistd.h>
#include <cstring>
#include <linux/gpio.h>
#include <sys/ioctl.h>
#include "common/util.h"
#include "common/swaglog.h"
int gpio_init(int pin_nr, bool output) {
char pin_dir_path[50];
int pin_dir_path_len = snprintf(pin_dir_path, sizeof(pin_dir_path),
"/sys/class/gpio/gpio%d/direction", pin_nr);
if (pin_dir_path_len <= 0) {
return -1;
}
const char *value = output ? "out" : "in";
return util::write_file(pin_dir_path, (void*)value, strlen(value));
}
int gpio_set(int pin_nr, bool high) {
char pin_val_path[50];
int pin_val_path_len = snprintf(pin_val_path, sizeof(pin_val_path),
"/sys/class/gpio/gpio%d/value", pin_nr);
if (pin_val_path_len <= 0) {
return -1;
}
return util::write_file(pin_val_path, (void*)(high ? "1" : "0"), 1);
}
int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr) {
// Assumed that all interrupt pins are unexported and rights are given to
// read from gpiochip0.
std::string gpiochip_path = "/dev/gpiochip" + std::to_string(gpiochiop_id);
int fd = open(gpiochip_path.c_str(), O_RDONLY);
if (fd < 0) {
LOGE("Error opening gpiochip0 fd");
return -1;
}
// Setup event
struct gpioevent_request rq;
rq.lineoffset = pin_nr;
rq.handleflags = GPIOHANDLE_REQUEST_INPUT;
/* Requesting both edges as the data ready pulse from the lsm6ds sensor is
very short(75us) and is mostly detected as falling edge instead of rising.
So if it is detected as rising the following falling edge is skipped. */
rq.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES;
strncpy(rq.consumer_label, consumer_label, std::size(rq.consumer_label) - 1);
int ret = util::safe_ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, &rq);
if (ret == -1) {
LOGE("Unable to get line event from ioctl : %s", strerror(errno));
close(fd);
return -1;
}
close(fd);
return rq.fd;
}
#endif
common/gpio.h
-33
View File
@@ -1,33 +0,0 @@
#pragma once
// Pin definitions
#ifdef QCOM2
#define GPIO_HUB_RST_N 30
#define GPIO_UBLOX_RST_N 32
#define GPIO_UBLOX_SAFEBOOT_N 33
#define GPIO_GNSS_PWR_EN 34 /* SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN */
#define GPIO_STM_RST_N 124
#define GPIO_STM_BOOT0 134
#define GPIO_BMX_ACCEL_INT 21
#define GPIO_BMX_GYRO_INT 23
#define GPIO_BMX_MAGN_INT 87
#define GPIO_LSM_INT 84
#define GPIOCHIP_INT 0
#else
#define GPIO_HUB_RST_N 0
#define GPIO_UBLOX_RST_N 0
#define GPIO_UBLOX_SAFEBOOT_N 0
#define GPIO_GNSS_PWR_EN 0 /* SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN */
#define GPIO_STM_RST_N 0
#define GPIO_STM_BOOT0 0
#define GPIO_BMX_ACCEL_INT 0
#define GPIO_BMX_GYRO_INT 0
#define GPIO_BMX_MAGN_INT 0
#define GPIO_LSM_INT 0
#define GPIOCHIP_INT 0
#endif
int gpio_init(int pin_nr, bool output);
int gpio_set(int pin_nr, bool high);
int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr);
common/i2c.cc
-92
View File
@@ -1,92 +0,0 @@
#include "common/i2c.h"
#include <fcntl.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <cassert>
#include <cstdio>
#include <stdexcept>
#include "common/swaglog.h"
#include "common/util.h"
#define UNUSED(x) (void)(x)
#ifdef QCOM2
// TODO: decide if we want to install libi2c-dev everywhere
extern "C" {
#include <linux/i2c-dev.h>
#include <i2c/smbus.h>
}
I2CBus::I2CBus(uint8_t bus_id) {
char bus_name[20];
snprintf(bus_name, 20, "/dev/i2c-%d", bus_id);
i2c_fd = HANDLE_EINTR(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) {
std::lock_guard lk(m);
int ret = 0;
ret = HANDLE_EINTR(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) {
std::lock_guard lk(m);
int ret = 0;
ret = HANDLE_EINTR(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
common/i2c.h
-19
View File
@@ -1,19 +0,0 @@
#pragma once
#include <cstdint>
#include <mutex>
#include <sys/types.h>
class I2CBus {
private:
int i2c_fd;
std::mutex m;
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);
};
common/mock/__init__.py
+1 -1
View File
@@ -13,7 +13,7 @@ from openpilot.common.realtime import Ratekeeper
MOCK_GENERATOR = {
"livePose": generate_livePose
"livePose": generate_livePose,
"liveLocationKalman": generate_liveLocationKalman
}
common/params_keys.h
+4
View File
@@ -183,6 +183,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
{"AutoNaviSpeedSafetyFactor", PERSISTENT},
{"AutoNaviCountDownMode", PERSISTENT},
{"TurnSpeedControlMode", PERSISTENT},
{"CarrotSmartSpeedControl", PERSISTENT},
{"MapTurnSpeedFactor", PERSISTENT},
{"ModelTurnSpeedFactor", PERSISTENT},
{"StoppingAccel", PERSISTENT},
@@ -281,6 +282,9 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
{"MuteDoor", PERSISTENT},
{"MuteSeatbelt", PERSISTENT},
{"CarrotException", CLEAR_ON_MANAGER_START},
{"CarrotSpeed", PERSISTENT},
{"CarrotSpeedViz", PERSISTENT},
{"CarrotSpeedTable", PERSISTENT},
{"CarName", PERSISTENT},
{"EVTable", PERSISTENT},
{"LongPitch", PERSISTENT},
common/utils.py
+118
View File
@@ -0,0 +1,118 @@
import io
import os
import tempfile
import contextlib
import subprocess
import time
import functools
from subprocess import Popen, PIPE, TimeoutExpired
import zstandard as zstd
from openpilot.common.swaglog import cloudlog
LOG_COMPRESSION_LEVEL = 10 # little benefit up to level 15. level ~17 is a small step change
class CallbackReader:
"""Wraps a file, but overrides the read method to also
call a callback function with the number of bytes read so far."""
def __init__(self, f, callback, *args):
self.f = f
self.callback = callback
self.cb_args = args
self.total_read = 0
def __getattr__(self, attr):
return getattr(self.f, attr)
def read(self, *args, **kwargs):
chunk = self.f.read(*args, **kwargs)
self.total_read += len(chunk)
self.callback(*self.cb_args, self.total_read)
return chunk
@contextlib.contextmanager
def atomic_write_in_dir(path: str, mode: str = 'w', buffering: int = -1, encoding: str | None = None, newline: str | None = None,
overwrite: bool = False):
"""Write to a file atomically using a temporary file in the same directory as the destination file."""
dir_name = os.path.dirname(path)
if not overwrite and os.path.exists(path):
raise FileExistsError(f"File '{path}' already exists. To overwrite it, set 'overwrite' to True.")
with tempfile.NamedTemporaryFile(mode=mode, buffering=buffering, encoding=encoding, newline=newline, dir=dir_name, delete=False) as tmp_file:
yield tmp_file
tmp_file_name = tmp_file.name
os.replace(tmp_file_name, path)
def get_upload_stream(filepath: str, should_compress: bool) -> tuple[io.BufferedIOBase, int]:
if not should_compress:
file_size = os.path.getsize(filepath)
file_stream = open(filepath, "rb")
return file_stream, file_size
# Compress the file on the fly
compressed_stream = io.BytesIO()
compressor = zstd.ZstdCompressor(level=LOG_COMPRESSION_LEVEL)
with open(filepath, "rb") as f:
compressor.copy_stream(f, compressed_stream)
compressed_size = compressed_stream.tell()
compressed_stream.seek(0)
return compressed_stream, compressed_size
# remove all keys that end in DEPRECATED
def strip_deprecated_keys(d):
for k in list(d.keys()):
if isinstance(k, str):
if k.endswith('DEPRECATED'):
d.pop(k)
elif isinstance(d[k], dict):
strip_deprecated_keys(d[k])
return d
def run_cmd(cmd: list[str], cwd=None, env=None) -> str:
return subprocess.check_output(cmd, encoding='utf8', cwd=cwd, env=env).strip()
def run_cmd_default(cmd: list[str], default: str = "", cwd=None, env=None) -> str:
try:
return run_cmd(cmd, cwd=cwd, env=env)
except subprocess.CalledProcessError:
return default
@contextlib.contextmanager
def managed_proc(cmd: list[str], env: dict[str, str]):
proc = Popen(cmd, env=env, stdout=PIPE, stderr=PIPE)
try:
yield proc
finally:
if proc.poll() is None:
proc.terminate()
try:
proc.wait(timeout=5)
except TimeoutExpired:
proc.kill()
def retry(attempts=3, delay=1.0, ignore_failure=False):
def decorator(func):
@functools.wraps(func)
def wrapper(*args, **kwargs):
for _ in range(attempts):
try:
return func(*args, **kwargs)
except Exception:
cloudlog.exception(f"{func.__name__} failed, trying again")
time.sleep(delay)
if ignore_failure:
cloudlog.error(f"{func.__name__} failed after retry")
else:
raise Exception(f"{func.__name__} failed after retry")
return wrapper
return decorator
launch_chffrplus.sh
+6
View File
@@ -88,6 +88,12 @@ function launch {
echo "shapely installing."
pip install shapely
fi
if python -c "import kaitaistruct" > /dev/null 2>&1; then
echo "kaitaistruct already installed."
else
echo "kaitaistruct installing."
pip install kaitaistruct
fi
# events language init
#LANG=$(cat ${PARAMS_ROOT}/d/LanguageSetting)
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+7 -3
View File
@@ -333,15 +333,17 @@ class CarController(CarControllerBase):
if self.CP.carFingerprint in CAN_GEARS["send_mdps12"]: # send mdps12 to LKAS to prevent LKAS error
can_sends.append(hyundaican.create_mdps12(self.packer, self.frame, CS.mdps12))
casper_opt = self.CP.carFingerprint in (CAR.HYUNDAI_CASPER_EV)
if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
self.hyundai_jerk.make_jerk(self.CP, CS, accel, actuators, hud_control)
self.hyundai_jerk.check_carrot_cruise(CC, CS, hud_control, stopping, accel, actuators.aTarget)
#jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
if camera_scc:
can_sends.extend(hyundaican.create_acc_commands_scc(self.packer, CC.enabled, accel, self.hyundai_jerk, int(self.frame / 2),
hud_control, set_speed_in_units, stopping,
CC.cruiseControl.override, use_fca, CS, self.soft_hold_mode))
CC.cruiseControl.override, casper_opt, CS, self.soft_hold_mode))
else:
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, self.hyundai_jerk, int(self.frame / 2),
hud_control, set_speed_in_units, stopping,
@@ -355,8 +357,10 @@ class CarController(CarControllerBase):
# 5 Hz ACC options
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
if camera_scc:
#if CS.scc13 is not None:
# can_sends.append(hyundaican.create_acc_opt_copy(CS, self.packer))
if CS.scc13 is not None:
if casper_opt:
#can_sends.append(hyundaican.create_acc_opt_copy(CS, self.packer))
pass
pass
else:
can_sends.extend(hyundaican.create_acc_opt(self.packer, self.CP))
opendbc_repo/opendbc/car/hyundai/carstate.py
+10 -1
View File
@@ -129,13 +129,15 @@ class CarState(CarStateBase):
ecu_disabled = True
if ecu_disabled:
self.SCC11 = self.SCC12 = self.SCC13 = self.SCC14 = False
self.SCC11 = self.SCC12 = self.SCC13 = self.SCC14 = self.FCA11 = False
else:
bus_cruise = 2 if self.CP.flags & HyundaiFlags.CAMERA_SCC else 0
self.SCC11 = True if 1056 in fingerprints[bus_cruise] else False
self.SCC12 = True if 1057 in fingerprints[bus_cruise] else False
self.SCC13 = True if 1290 in fingerprints[bus_cruise] else False
self.SCC14 = True if 905 in fingerprints[bus_cruise] else False
self.FCA11 = False
self.FCA11_bus = Bus.cam
self.HAS_LFA_BUTTON = True if 913 in fingerprints[0] else False
self.CRUISE_BUTTON_ALT = True if 1007 in fingerprints[0] else False
@@ -179,6 +181,12 @@ class CarState(CarStateBase):
elif self.controls_ready_count == 100:
print("cp_cam.seen_addresses =", cp_cam.seen_addresses)
print("cp.seen_addresses =", cp.seen_addresses)
if 909 in cp_cam.seen_addresses:
self.FCA11 = True
self.FCA11_bus = Bus.cam
elif 909 in cp.seen_addresses:
self.FCA11 = True
self.FCA11_bus = Bus.pt
if cp_alt is not None:
print("cp_alt.seen_addresses =", cp_alt.seen_addresses)
@@ -360,6 +368,7 @@ class CarState(CarStateBase):
self.scc12 = cp_cruise.vl["SCC12"] if self.SCC12 else None
self.scc13 = cp_cruise.vl["SCC13"] if self.SCC13 else None
self.scc14 = cp_cruise.vl["SCC14"] if self.SCC14 else None
self.fca11 = can_parsers[self.FCA11_bus].vl["FCA11"] if self.FCA11 else None
cluSpeed = cp.vl["CLU11"]["CF_Clu_Vanz"]
decimal = cp.vl["CLU11"]["CF_Clu_VanzDecimal"]
if 0. < decimal < 0.5:
opendbc_repo/opendbc/car/hyundai/hyundaican.py
+12
View File
@@ -210,6 +210,14 @@ def create_acc_commands_scc(packer, enabled, accel, jerk, idx, hud_control, set_
values["ObjDistStat"] = objGap2
commands.append(packer.make_can_msg("SCC14", 0, values))
if CS.fca11 is not None and use_fca: # CASPER_EV의 경우 FCA11에서 fail이 간헐적 발생함.. 그냥막자.. 원인불명..
values = copy.copy(CS.fca11)
if values["FCA_Failinfo"] != 0:
values["FCA_Status"] = 2
values["FCA_Failinfo"] = 0
fca11_dat = packer.make_can_msg("FCA11", 0, values)[1]
values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7])
commands.append(packer.make_can_msg("FCA11", 0, values))
# Only send FCA11 on cars where it exists on the bus
if False: #use_fca:
# note that some vehicles most likely have an alternate checksum/counter definition
@@ -227,6 +235,10 @@ def create_acc_commands_scc(packer, enabled, accel, jerk, idx, hud_control, set_
return commands
def create_acc_opt_copy(CS, packer):
values = copy.copy(CS.scc13)
if values["NEW_SIGNAL_1"] == 255:
values["NEW_SIGNAL_1"] = 218
values["NEW_SIGNAL_2"] = 0
return packer.make_can_msg("SCC13", 0, CS.scc13)
def create_acc_commands(packer, enabled, accel, jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, CP, CS, soft_hold_mode):
opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
+2 -1
View File
@@ -517,10 +517,11 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle
values["LKA_ICON"] = 4 if lat_active else 3 if lat_enabled else 0
values["FCA_ALT_ICON"] = 0
if values["ALERTS_2"] in [1, 2, 5]:
if values["ALERTS_2"] in [1, 2, 5, 10, 22]: # 10,22: 운전자모니터 알람/경고
values["ALERTS_2"] = 0
values["DAW_ICON"] = 0
values["SOUNDS_1"] = 0 # 운전자모니터경고음.
values["SOUNDS_2"] = 0 # 2: STEER중지 경고후에도 사운드가 나옴.
values["SOUNDS_4"] = 0 # 차선변경알림? 에이 그냥0으로..
opendbc_repo/opendbc/safety/safety/safety_hyundai.h
+21 -12
View File
@@ -200,6 +200,7 @@ uint32_t last_ts_scc12_from_op = 0;
uint32_t last_ts_scc13_from_op = 0;
uint32_t last_ts_mdps12_from_op = 0;
uint32_t last_ts_fca11_from_op = 0;
uint32_t last_ts_fca12_from_op = 0;
static bool hyundai_tx_hook(const CANPacket_t *to_send) {
const TorqueSteeringLimits HYUNDAI_STEERING_LIMITS = HYUNDAI_LIMITS(512, 10, 10);
@@ -210,7 +211,7 @@ static bool hyundai_tx_hook(const CANPacket_t *to_send) {
int addr = GET_ADDR(to_send);
// FCA11: Block any potential actuation
if (addr == 0x38D) {
if (false && addr == 0x38D) {
int CR_VSM_DecCmd = GET_BYTE(to_send, 1);
bool FCA_CmdAct = GET_BIT(to_send, 20U);
bool CF_VSM_DecCmdAct = GET_BIT(to_send, 31U);
@@ -277,16 +278,19 @@ static bool hyundai_tx_hook(const CANPacket_t *to_send) {
tx = false;
}
}
uint32_t now = microsecond_timer_get();
if(addr == 832)
last_ts_lkas11_from_op = (tx == 0 ? 0 : microsecond_timer_get());
last_ts_lkas11_from_op = (tx == 0 ? 0 : now);
else if(addr == 1057)
last_ts_scc12_from_op = (tx == 0 ? 0 : microsecond_timer_get());
last_ts_scc12_from_op = (tx == 0 ? 0 : now);
else if(addr == 593)
last_ts_mdps12_from_op = (tx == 0 ? 0 : microsecond_timer_get());
else if(addr == 909)
last_ts_fca11_from_op = (tx == 0 ? 0 : microsecond_timer_get());
last_ts_mdps12_from_op = (tx == 0 ? 0 : now);
else if (addr == 909)
last_ts_fca11_from_op = (tx == 0 ? 0 : now);
else if (addr == 1155)
last_ts_fca12_from_op = (tx == 0 ? 0 : now);
else if(addr == 1290)
last_ts_scc13_from_op = (tx == 0 ? 0 : microsecond_timer_get());
last_ts_scc13_from_op = (tx == 0 ? 0 : now);
return tx;
}
@@ -313,9 +317,10 @@ static int hyundai_fwd_hook(int bus_num, int addr) {
bool is_lfahda_msg = addr == 1157;
bool is_scc_msg = addr == 1056 || addr == 1057 || addr == 905;
bool is_scc13_msg = addr == 1290;
bool is_fca_msg = addr == 909 || addr == 1155;
bool is_fca11_msg = addr == 909;
bool is_fca12_msg = addr == 1155;
bool block_msg = is_lkas_msg || is_lfahda_msg || is_scc_msg || is_scc13_msg; //|| is_fca_msg;
bool block_msg = is_lkas_msg || is_lfahda_msg || is_scc_msg || is_scc13_msg || is_fca11_msg || is_fca12_msg;
if (!block_msg) {
bus_fwd = 0;
}
@@ -330,11 +335,15 @@ static int hyundai_fwd_hook(int bus_num, int addr) {
bus_fwd = 0;
}
else if (is_scc13_msg) {
if (now - last_ts_scc13_from_op >= 400000)
if (now - last_ts_scc13_from_op >= 800000)
bus_fwd = 0;
}
else if(is_fca_msg) {
if(now - last_ts_fca11_from_op >= 400000)
else if (is_fca11_msg) {
if (now - last_ts_fca11_from_op >= 400000)
bus_fwd = 0;
}
else if (is_fca12_msg) {
if (now - last_ts_fca12_from_op >= 400000)
bus_fwd = 0;
}
}
restart.sh
+1 -1
View File
@@ -1,2 +1,2 @@
git pull
tmux kill-session -t comma; rm -f /tmp/safe_staging_overlay.lock; sleep 1;tmux new -s comma -d "/data/openpilot/launch_openpilot.sh"
tmux kill-session -t comma; rm -f /tmp/safe_staging_overlay.lock; sleep 1;tmux new -s comma -d "bash -lc '/data/openpilot/launch_openpilot.sh'"
selfdrive/car/cruise.py
+15
View File
@@ -247,6 +247,7 @@ class VCruiseCarrot:
self.autoGasSyncSpeed = self.params.get_bool("AutoGasSyncSpeed") * unit_factor
self.autoSpeedUptoRoadSpeedLimit = self.params.get_float("AutoSpeedUptoRoadSpeedLimit") * 0.01
self.autoRoadSpeedAdjust = self.params.get_float("AutoRoadSpeedAdjust") * 0.01
self.smartSpeedControl = self.params.get_int("CarrotSmartSpeedControl")
useLaneLineSpeed = self.params.get_int("UseLaneLineSpeed") * unit_factor
if self.useLaneLineSpeed != useLaneLineSpeed:
@@ -441,6 +442,20 @@ class VCruiseCarrot:
return button_kph, button_type, self.long_pressed
def _carrot_command(self, v_cruise_kph, button_type, long_pressed):
carrot_speed = self.params_memory.get_int("CarrotSpeed")
if carrot_speed != 0:
if carrot_speed > 0:
if self.smartSpeedControl in [1,3]:
v_cruise_kph = max(carrot_speed, v_cruise_kph)
else:
if self.smartSpeedControl == 3:
v_cruise_kph = -carrot_speed
#elif self.smartSpeedControl == 1:
# v_cruise_kph = max(-carrot_speed, v_cruise_kph)
elif self.smartSpeedControl == 2:
v_cruise_kph = min(-carrot_speed, v_cruise_kph)
self.params_memory.put_int_nonblocking("CarrotSpeed", 0)
self._add_log(f"Carrot speed set to {v_cruise_kph}")
if self.carrot_cmd_index_last != self.carrot_cmd_index:
self.carrot_cmd_index_last = self.carrot_cmd_index
print(f"Carrot command(cruise.py): {self.carrot_cmd} {self.carrot_arg}")
selfdrive/carrot/carrot_man.py
+94 -3
View File
@@ -22,6 +22,9 @@ from openpilot.selfdrive.navd.helpers import Coordinate
from opendbc.car.common.conversions import Conversions as CV
from openpilot.selfdrive.carrot.carrot_serv import CarrotServ
from openpilot.selfdrive.carrot.carrot_speed import CarrotSpeed
from openpilot.common.gps import get_gps_location_service
try:
from shapely.geometry import LineString
@@ -185,7 +188,8 @@ class CarrotMan:
print("************************************************CarrotMan init************************************************")
self.params = Params()
self.params_memory = Params("/dev/shm/params")
self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl', 'navRouteNavd', 'liveLocationKalman', 'navInstruction'])
self.gps_location_service = get_gps_location_service(self.params)
self.sm = messaging.SubMaster(['deviceState', 'carState', 'controlsState', 'radarState', 'longitudinalPlan', 'modelV2', 'selfdriveState', 'carControl', 'navRouteNavd', self.gps_location_service, 'navInstruction'])
self.pm = messaging.PubMaster(['carrotMan', "navRoute", "navInstructionCarrot"])
self.carrot_serv = CarrotServ()
@@ -258,9 +262,22 @@ class CarrotMan:
sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
frame = 0
self.save_toggle_values()
rk = Ratekeeper(10, print_delay_threshold=None)
carrot_speed = CarrotSpeed(neighbor_ring=2)
self.params_memory.put_int_nonblocking("CarrotSpeed", 0)
rk = Ratekeeper(20, print_delay_threshold=None)
carrotIndex_last = self.carrot_serv.carrotIndex
phone_gps_frame = self.carrot_serv.phone_gps_frame
carrot_speed_active_count = 0
self.v_cruise_last = 0
self.long_active = False
self.v_cruise_change = 0
self._last_vt = 0.0
self.gas_pressed_count = 0
self._last_viz_t = 0.0
while self.is_running:
try:
self.sm.update(0)
@@ -273,7 +290,16 @@ class CarrotMan:
#print("coords=", coords)
#print("curvatures=", curvatures)
self.carrot_serv.update_navi(remote_ip, self.sm, self.pm, vturn_speed, coords, distances, route_speed)
self.carrot_serv.update_navi(remote_ip, self.sm, self.pm, vturn_speed, coords, distances, route_speed, self.gps_location_service)
if phone_gps_frame != self.carrot_serv.phone_gps_frame:
phone_gps_frame = self.carrot_serv.phone_gps_frame
carrot_speed_active_count = 10
else:
carrot_speed_active_count -= 1
if carrot_speed_active_count > 0:
self.carrot_speed_serv(carrot_speed, frame)
if frame % 20 == 0 or remote_addr is not None:
try:
@@ -318,6 +344,71 @@ class CarrotMan:
traceback.print_exc()
time.sleep(1)
def carrot_speed_serv(self, carrot_speed, frame):
v_ego = a_ego = 0.0
gas_pressed = False
if self.sm.alive['carState'] and self.sm.alive['carControl']:
CS = self.sm['carState']
CC = self.sm['carControl']
v_ego = CS.vEgo
a_ego = CS.aEgo
gas_pressed = CS.gasPressed
v_ego_kph = v_ego * 3.6
if gas_pressed:
self.gas_pressed_count = 200
self.v_cruise_change = 0
elif self._last_vt == CS.vCruise:
self.v_cruise_last = CS.vCruise
elif self.long_active and CC.longActive and self.gas_pressed_count == 0:
if self.v_cruise_last < CS.vCruise: # 속도가 증가하면
self.v_cruise_change = 100
elif self.v_cruise_last > CS.vCruise: # 속도가 감소하면
if v_ego_kph < CS.vCruise: # 주행속도가 느리면
self.v_cruise_change = 100
else: # 주행속도가 빠르면
self.v_cruise_change = -100
if self.v_cruise_change != 0:
self.gas_pressed_count = 0
else:
self.v_cruise_change = 0
self.long_active = CC.longActive
self.v_cruise_last = CS.vCruise
else:
self.v_cruise_change = 0
now = time.monotonic()
heading = self.carrot_serv.bearing #nPosAnglePhone
lat, lon = self.carrot_serv.vpPosPointLat, self.carrot_serv.vpPosPointLon #self.carrot_serv.estimate_position(self.carrot_serv.phone_latitude, self.carrot_serv.phone_longitude, heading, v_ego, now - self.carrot_serv.last_update_gps_time_phone)
vt = carrot_speed.query_target_dist(lat, lon, heading, 0.0)
if self.v_cruise_change != 0:
carrot_speed.add_sample(lat, lon, heading, self.v_cruise_last if self.v_cruise_change > 0 else (- self.v_cruise_last))
if self.v_cruise_change > 0:
self.v_cruise_change -= 1
if self.v_cruise_change < 0:
self.v_cruise_change += 1
else:
if self.gas_pressed_count > 0:
vt = max(vt, self.v_cruise_last)
carrot_speed.add_sample(lat, lon, heading, vt)
self.params_memory.put_int_nonblocking("CarrotSpeed", int(vt))
self._last_vt = vt
if gas_pressed and a_ego < -0.5: #self._last_vt < 0.0:
carrot_speed.invalidate_last_hit(window_s=2.0, action="clear")
self.gas_pressed_count = max(0, self.gas_pressed_count - 1)
if now - self._last_viz_t > 0.5: # 2Hz
self._last_viz_t = now
viz_json = carrot_speed.export_cells_around(lat, lon, heading, ring=2, max_points=64)
# 메모리 Params에 쓰는 게 좋음 (디스크 말고)
self.params_memory.put_nonblocking("CarrotSpeedViz", viz_json)
carrot_speed.maybe_save()
def carrot_navi_route(self):
if self.carrot_serv.active_carrot > 1:
selfdrive/carrot/carrot_serv.py
+25 -17
View File
@@ -19,6 +19,7 @@ from openpilot.common.filter_simple import MyMovingAverage
from openpilot.system.hardware import PC, TICI
from openpilot.selfdrive.navd.helpers import Coordinate
from opendbc.car.common.conversions import Conversions as CV
from openpilot.common.gps import get_gps_location_service
nav_type_mapping = {
12: ("turn", "left", 1),
@@ -145,8 +146,11 @@ class CarrotServ:
self.bearing = 0.0
self.gps_valid = False
self.gps_accuracy_phone = 0.0
self.phone_gps_accuracy = 0.0
self.gps_accuracy_device = 0.0
self.phone_latitude = 0.0
self.phone_longitude = 0.0
self.phone_gps_frame = 0
self.totalDistance = 0
self.xSpdLimit = 0
@@ -641,15 +645,14 @@ class CarrotServ:
self.xSpdType = -1
self.xSpdDist = 0
def _update_gps(self, v_ego, sm):
llk = 'liveLocationKalman'
location = sm[llk]
def _update_gps(self, v_ego, sm, gps_service):
gps = sm[gps_service]
#print(f"location = {sm.valid[llk]}, {sm.updated[llk]}, {sm.recv_frame[llk]}, {sm.recv_time[llk]}")
if not sm.updated['carState'] or not sm.updated['carControl']: # or not sm.updated[llk]:
return self.nPosAngle
CS = sm['carState']
CC = sm['carControl']
self.gps_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
self.gps_valid = sm.updated[gps_service] and gps.hasFix
now = time.monotonic()
gps_updated_phone = (now - self.last_update_gps_time_phone) < 3
@@ -658,8 +661,8 @@ class CarrotServ:
bearing = self.nPosAngle
if gps_updated_phone:
self.bearing_offset = 0.0
elif sm.valid[llk]:
bearing = math.degrees(location.calibratedOrientationNED.value[2])
elif self.gps_valid:
bearing = self.nPosAngle = gps.bearingDeg
if self.gps_valid:
self.bearing_offset = 0.0
elif self.active_carrot > 0:
@@ -669,13 +672,13 @@ class CarrotServ:
#print(f"bearing = {bearing:.1f}, posA=={self.nPosAngle:.1f}, posP=={self.nPosAnglePhone:.1f}, offset={self.bearing_offset:.1f}, {gps_updated_phone}, {gps_updated_navi}")
gpsDelayTimeAdjust = 0.0
if gps_updated_navi:
gpsDelayTimeAdjust = 1.0
gpsDelayTimeAdjust = 0 #1.0
external_gps_update_timedout = not (gps_updated_phone or gps_updated_navi)
#print(f"gps_valid = {self.gps_valid}, bearing = {bearing:.1f}, pos = {location.positionGeodetic.value[0]:.6f}, {location.positionGeodetic.value[1]:.6f}")
if self.gps_valid and external_gps_update_timedout: # 내부GPS가 자동하고 carrotman으로부터 gps신호가 없는경우
self.vpPosPointLatNavi = location.positionGeodetic.value[0]
self.vpPosPointLonNavi = location.positionGeodetic.value[1]
self.vpPosPointLatNavi = gps.latitude
self.vpPosPointLonNavi = gps.longitude
self.last_calculate_gps_time = now #sm.recv_time[llk]
elif gps_updated_navi: # carrot navi로부터 gps신호가 수신되는 경우..
if abs(self.bearing_measured - bearing) < 0.1:
@@ -853,7 +856,7 @@ class CarrotServ:
self.xSpdDist = distance
self.xSpdType =xSpdType
def update_navi(self, remote_ip, sm, pm, vturn_speed, coords, distances, route_speed):
def update_navi(self, remote_ip, sm, pm, vturn_speed, coords, distances, route_speed, gps_service):
self.debugText = ""
self.update_params()
@@ -874,7 +877,7 @@ class CarrotServ:
road_speed_limit_changed = True if self.nRoadLimitSpeed != self.nRoadLimitSpeed_last else False
self.nRoadLimitSpeed_last = self.nRoadLimitSpeed
#self.bearing = self.nPosAngle #self._update_gps(v_ego, sm)
self.bearing = self._update_gps(v_ego, sm)
self.bearing = self._update_gps(v_ego, sm, gps_service)
self.xSpdDist = max(self.xSpdDist - delta_dist, -1000)
self.xDistToTurn = self.xDistToTurn - delta_dist
@@ -1281,15 +1284,20 @@ class CarrotServ:
# 3초간 navi 데이터가 없으면, phone gps로 업데이트
if "latitude" in json:
self.nPosAnglePhone = float(json.get("heading", self.nPosAngle))
self.phone_latitude = float(json.get("latitude", self.vpPosPointLatNavi))
self.phone_longitude = float(json.get("longitude", self.vpPosPointLonNavi))
self.phone_gps_accuracy = float(json.get("accuracy", 0))
if self.phone_gps_accuracy < 15.0:
self.phone_gps_frame += 1
if (now - self.last_update_gps_time_navi) > 3.0:
self.vpPosPointLatNavi = float(json.get("latitude", self.vpPosPointLatNavi))
self.vpPosPointLonNavi = float(json.get("longitude", self.vpPosPointLonNavi))
self.vpPosPointLatNavi = self.phone_latitude
self.vpPosPointLonNavi = self.phone_longitude
self.nPosAngle = self.nPosAnglePhone
# self.nPosSpeed = self.ve # TODO speed from v_ego
self.last_update_gps_time_phone = self.last_calculate_gps_time = now
self.gps_accuracy_phone = float(json.get("accuracy", 0))
self.last_update_gps_time_phone = self.last_calculate_gps_time = now
self.nPosSpeed = float(json.get("gps_speed", 0))
print(f"phone gps: {self.vpPosPointLatNavi}, {self.vpPosPointLonNavi}, {self.gps_accuracy_phone}, {self.nPosSpeed}")
print(f"phone gps: {self.vpPosPointLatNavi}, {self.vpPosPointLonNavi}, {self.phone_gps_accuracy}, {self.nPosSpeed}")
import traceback
selfdrive/carrot/carrot_speed.py
+401
View File
@@ -0,0 +1,401 @@
# -*- coding: utf-8 -*-
"""
CarrotSpeedTable v2.1 (Params backend, JSON+gzip, 1e-4° grid, 8 buckets)
- 저장 키: "CarrotSpeedTable"
- 포맷(JSON): {"format":"v5","dir_buckets":8,"cells":{"gy,gx":[[v,ts],...]} }
- gzip 저장/로드 지원 (기본 on). 기존 비압축 v2도 로드 가능.
- 격자: 위/경도 각 1e-4° 스냅(한국 위도에서 약 9~11m)
- 저장: 단일 speed(부호 포함)만 해당 셀 1곳에 기록
* 입력 > 0: 기존 None/음수/더 작은 양수면 갱신(더 큰 +)
* 입력 < 0: 기존 None/양수/덜 음수면 갱신(더 작은 -)
- 조회: 전방 lookahead 셀 → 없으면 이웃 탐색(ring=1)
* 본셀: 시간 필터 없음
* 이웃: 오래된 데이터만 사용(age ≥ 120s)
- 정리(청소) 없음: 오래된 데이터도 유지
"""
import json, math, threading, time, gzip
from typing import Optional, Tuple, Dict, List
from openpilot.common.params import Params
# ---------- 지오/도우미 ----------
def quantize_1e4(lat: float, lon: float) -> Tuple[int, int]:
gy = int(math.floor(lat * 1e4 + 0.5))
gx = int(math.floor(lon * 1e4 + 0.5))
return gy, gx
def heading_to_bucket(heading_deg: float) -> int:
# 8 버킷 고정
step = 45.0 # 360/8
i = int((heading_deg % 360.0) // step)
if i < 0: return 0
if i > 7: return 7
return i
DIR_8 = {
0: ( 1, 0), # 북
1: ( 1, 1), # 북동
2: ( 0, 1), # 동
3: (-1, 1), # 남동
4: (-1, 0), # 남
5: (-1, -1), # 남서
6: ( 0, -1), # 서
7: ( 1, -1), # 북서
}
def project_point(lat: float, lon: float, heading_deg: float, distance_m: float) -> Tuple[float, float]:
if distance_m <= 0.0:
return lat, lon
R = 6_371_000.0
h = math.radians(heading_deg)
dlat = (distance_m * math.cos(h)) / R
dlon = (distance_m * math.sin(h)) / (R * math.cos(math.radians(lat)))
return lat + math.degrees(dlat), lon + math.degrees(dlon)
def _is_gzip(data: bytes) -> bool:
return len(data) >= 2 and data[0] == 0x1F and data[1] == 0x8B
# ---------- 메인 클래스 ----------
class CarrotSpeed:
KEY = "CarrotSpeedTable"
def __init__(self,
neighbor_ring: int = 1,
neighbor_old_threshold_s: int = 120,
use_gzip: bool = True,
gzip_level: int = 5):
# 고정 사양
self.buckets = 8
# 파라미터
self.neighbor_ring = max(0, int(neighbor_ring))
self.neighbor_old_threshold_s = int(neighbor_old_threshold_s)
self.use_gzip = bool(use_gzip)
self.gzip_level = int(gzip_level)
# 내부 상태
self._lock = threading.RLock()
# _cells[(gy,gx)] = [[value or None, ts(int seconds) or None] * 8]
self._cells: Dict[Tuple[int, int], List[List[Optional[float]]]] = {}
self._dirty = False
self._last_save = 0
self._params = Params()
self._load_from_params_if_exists()
self._last_hit = None # (gy, gx, b, ts_when_read)
self._last_hit_read_ms = 0 # 밀리초
# ----- 내부 유틸 -----
def _ensure_cell(self, gy: int, gx: int) -> List[List[Optional[float]]]:
arr = self._cells.get((gy, gx))
if arr is None:
arr = [[None, None] for _ in range(self.buckets)] # [v, ts]
self._cells[(gy, gx)] = arr
return arr
def _now(self) -> int:
# int 초
return int(time.time())
def _age(self, ts: Optional[float]) -> Optional[int]:
if ts is None:
return None
return self._now() - int(ts)
def _neighbor_indices(self, gy: int, gx: int) -> List[Tuple[int, int]]:
r = self.neighbor_ring
if r <= 0:
return []
out = []
for dy in range(-r, r + 1):
for dx in range(-r, r + 1):
if dy == 0 and dx == 0:
continue
out.append((gy + dy, gx + dx))
return out
def _neighbors_8(self, gy, gx):
for dy in (-1, 0, 1):
for dx in (-1, 0, 1):
if dy == 0 and dx == 0:
continue
yield gy + dy, gx + dx
def _try_cell_bucket_old(self, arr, b):
v, ts = arr[b]
if v is None or ts is None:
return None, None
if self._now() - int(ts) < self.neighbor_old_threshold_s:
return None, None
return float(v), b
# ----- 공용 API -----
def export_cells_around(self, lat: float, lon: float,
heading_deg: float,
ring: int = 1, max_points: int = 64) -> str:
"""
현재 lat, lon 기준 주변 그리드(ring 범위)에서
값이 있는 셀들을 (lat, lon, speed) 리스트로 JSON으로 반환.
Params("CarrotSpeedViz")에 그대로 넣을 용도.
"""
gy0, gx0 = quantize_1e4(lat, lon)
b0 = heading_to_bucket(heading_deg)
pts = []
with self._lock:
for dy in range(-ring, ring + 1):
for dx in range(-ring, ring + 1):
gy = gy0 + dy
gx = gx0 + dx
arr = self._cells.get((gy, gx))
if not arr:
continue
# 먼저 exact bucket(b0)
v, ts = arr[b0]
if v is not None:
cell_lat = (gy + 0.5) * 1e-4
cell_lon = (gx + 0.5) * 1e-4
pts.append([cell_lat, cell_lon, float(v)])
if len(pts) >= max_points:
return json.dumps({"pts": pts}, separators=(",",":"))
# 없다면 좌/우
for b in ((b0 - 1) % self.buckets, (b0 + 1) % self.buckets):
v, ts = arr[b]
if v is None:
continue
cell_lat = (gy + 0.5) * 1e-4
cell_lon = (gx + 0.5) * 1e-4
pts.append([cell_lat, cell_lon, float(v)])
if len(pts) >= max_points:
return json.dumps({"pts": pts}, separators=(",",":"))
return json.dumps({"pts": pts}, separators=(",",":"))
def add_sample(self, lat: float, lon: float, heading_deg: float, speed_signed: float):
"""
단일 speed(부호 포함) 저장.
- 기준 셀(현재 위치) + heading 기준 좌/우 1셀, 2셀까지 동일 speed 기록
- 각 셀 안에서는 heading 버킷 b와 b±1 세 개 버킷 모두 같은 값으로 갱신.
- >0: 기존 음수/None도 교체, 기존 양수면 평균으로 완만하게 갱신.
- <0: 항상 새 음수로 덮어쓰기(돌발 감속 우선).
==0: 무시
"""
v_in = round(float(speed_signed), 1)
if v_in == 0.0:
return
# 현재 위치를 그리드로
gy0, gx0 = quantize_1e4(lat, lon)
b = heading_to_bucket(heading_deg)
now = self._now()
# bucket에 해당하는 전진 방향 그리드 벡터
dy_f, dx_f = DIR_8[b]
# heading 기준 좌/우 1셀, 2셀 (project_point 사용 X)
# 좌 = 전진벡터를 90° 회전 (dy,dx) -> (dx,-dy)
# 우 = 전진벡터를 -90° 회전 (dy,dx) -> (-dx,dy)
dy_l1, dx_l1 = dx_f, -dy_f
dy_r1, dx_r1 = -dx_f, dy_f
dy_l2, dx_l2 = 2 * dy_l1, 2 * dx_l1
dy_r2, dx_r2 = 2 * dy_r1, 2 * dx_r1
# 기록할 셀들: 중앙 + 좌/우 1칸 + 좌/우 2칸
target_cells = {
(gy0, gx0),
(gy0 + dy_l1, gx0 + dx_l1),
(gy0 + dy_r1, gx0 + dx_r1),
(gy0 + dy_l2, gx0 + dx_l2),
(gy0 + dy_r2, gx0 + dx_r2),
}
with self._lock:
for gy, gx in target_cells:
arr = self._ensure_cell(gy, gx)
# b, b-1, b+1 세 버킷 모두 같은 정책으로 업데이트
for off in (0, -1, +1):
bi = (b + off) % self.buckets
v_old, ts_old = arr[bi]
if v_old is None:
# 처음 쓰는 버킷
arr[bi] = [v_in, now]
else:
if v_in > 0.0:
# 가속 정보: 기존 양수면 평균, 음수면 교체
if v_old < 0.0:
# 음수 -> 양수로 바뀌면 새 양수로 교체 (ts는 기존 유지)
arr[bi] = [v_in, ts_old]
else:
new_val = round((v_old + v_in) / 2.0, 1)
arr[bi] = [new_val, ts_old]
else:
# 감속 정보: 항상 새 음수로 덮어쓰기, ts는 기존 유지
arr[bi] = [v_in, ts_old]
self._dirty = True
def query_target(self, lat: float, lon: float, heading_deg: float, v_ego: float,
lookahead_s: float = 2.0) -> float:
dist = max(0.0, float(v_ego) * float(lookahead_s))
return self.query_target_dist(lat, lon, heading_deg, dist)
def query_target_dist(self, lat: float, lon: float, heading_deg: float, dist: float) -> float:
b = heading_to_bucket(heading_deg)
cand_ds = [dist]
for off in (3.0, -3.0):
d2 = dist + off
if d2 >= 0.0:
cand_ds.append(d2)
with self._lock:
for d in cand_ds:
y, x = project_point(lat, lon, heading_deg, d)
gy, gx = quantize_1e4(y, x)
arr = self._cells.get((gy, gx))
if not arr:
continue
v, b_sel = self._try_cell_bucket_old(arr, b)
if v is not None:
now_sec = int(time.time())
self._last_hit = (gy, gx, b_sel, now_sec)
self._last_hit_read_ms = int(time.time() * 1000)
return v
return 0.0
def invalidate_last_hit(self, window_s: float = 2.0, action: str = "clear") -> bool:
if self._last_hit is None:
return False
gy, gx, b, read_ts = self._last_hit
now = int(time.time())
if (now - int(read_ts)) > window_s:
return False
with self._lock:
arr = self._cells.get((gy, gx))
if not arr:
return False
# b, b-1, b+1 모두 invalidate
for off in (0, -1, +1):
bi = (b + off) % self.buckets
v, ts = arr[bi]
if action == "clear":
if v is not None and v < 0.0:
arr[bi] = [None, None]
else: # "age_bump"
if v is not None:
arr[bi] = [v, now]
else:
# 값이 없으면 넘어가기만 (그 버킷만 skip)
pass
self._dirty = True
return True
def maybe_save(self, interval_s: int = 60) -> None:
now = self._now()
if (not self._dirty) or (now - self._last_save < interval_s):
return
self.save()
def save(self) -> None:
payload = self._encode_payload()
self._params.put_nonblocking(self.KEY, payload)
self._last_save = self._now()
self._dirty = False
def close(self) -> None:
try:
if self._dirty:
self.save()
except Exception:
pass
# ----- 직렬화 -----
def _encode_payload(self) -> bytes:
with self._lock:
cells = {}
for (gy, gx), arr in self._cells.items():
key = f"{gy},{gx}"
# arr: [[v, ts], ...] (ts는 int 또는 None)
cells[key] = [[None if v is None else float(v),
None if ts is None else int(ts)] for (v, ts) in arr]
obj = {"format": "v5", "dir_buckets": self.buckets, "cells": cells}
raw = json.dumps(obj, separators=(",", ":")).encode("utf-8")
if self.use_gzip:
return gzip.compress(raw, compresslevel=self.gzip_level)
return raw
def _load_from_params_if_exists(self) -> None:
raw = self._params.get(self.KEY)
if not raw:
return
try:
data_bytes = raw
if _is_gzip(data_bytes):
data_bytes = gzip.decompress(data_bytes)
data = json.loads(data_bytes.decode("utf-8"))
# v3 아니면 삭제/초기화
if data.get("format") != "v5":
self._params.remove(self.KEY)
with self._lock:
self._cells = {}
self._dirty = False
return
buckets = int(data.get("dir_buckets", 8))
if buckets != 8:
# 버킷 불일치도 삭제/초기화
self._params.remove(self.KEY)
with self._lock:
self._cells = {}
self._dirty = False
return
restored: Dict[Tuple[int, int], List[List[Optional[float]]]] = {}
for key, arr in data.get("cells", {}).items():
gy, gx = map(int, key.split(","))
fixed: List[List[Optional[float]]] = []
if isinstance(arr, list) and len(arr) == 8:
for pair in arr:
if isinstance(pair, list) and len(pair) == 2:
v, ts = pair
v2 = None if v is None else float(v)
# ts는 int로 강제
ts2 = None if ts is None else int(ts)
fixed.append([v2, ts2])
else:
fixed.append([None, None])
else:
fixed = [[None, None] for _ in range(8)]
restored[(gy, gx)] = fixed
with self._lock:
self._cells = restored
self._dirty = False
except Exception:
# 파싱 실패 시 안전 초기화
self._params.delete(self.KEY)
with self._lock:
self._cells = {}
self._dirty = False
selfdrive/carrot_settings.json
+13
View File
@@ -1744,6 +1744,19 @@
"default": 1,
"unit": 1
},
{
"group": "감속제어",
"name": "CarrotSmartSpeedControl",
"title": "스마트속도제어(속도재생)",
"descr": "0: 속도제어안함\n 1: 가속만, 2: 감속만, 3: 모두",
"egroup": "SPEED",
"etitle": "Smart Speed Control(Replay)",
"edescr": "0:not use, 1:accel, 2:decel, 3:all",
"min": 0,
"max": 3,
"default": 0,
"unit": 1
},
{
"group": "감속제어",
"name": "MapTurnSpeedFactor",
selfdrive/controls/controlsd.py
+11 -18
View File
@@ -48,7 +48,7 @@ class Controls:
self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
'carrotMan', 'lateralPlan', 'radarState', 'liveLocationKalman',
'carrotMan', 'lateralPlan', 'radarState',
'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
self.pm = messaging.PubMaster(['carControl', 'controlsState'])
@@ -180,7 +180,7 @@ class Controls:
actuators.curvature = float(self.desired_curvature)
steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
self.steer_limited_by_controls, self.desired_curvature,
self.sm['liveLocationKalman'], curvature_limited,
CC, curvature_limited,
model_data=self.sm['modelV2'])
actuators.torque = float(steer)
actuators.steeringAngleDeg = float(steeringAngleDeg)
@@ -237,23 +237,16 @@ class Controls:
# Orientation and angle rates can be useful for carcontroller
# Only calibrated (car) frame is relevant for the carcontroller
#if self.calibrated_pose is not None:
# CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
# CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
if self.calibrated_pose is not None:
CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
orientation_value = list(self.sm['liveLocationKalman'].calibratedOrientationNED.value)
if len(orientation_value) > 2:
CC.orientationNED = orientation_value
angular_rate_value = list(self.sm['liveLocationKalman'].angularVelocityCalibrated.value)
if len(angular_rate_value) > 2:
CC.angularVelocity = angular_rate_value
acceleration_value = list(self.sm['liveLocationKalman'].accelerationCalibrated.value)
if len(acceleration_value) > 2:
if abs(acceleration_value[0]) > 16.0:
print("Collision detected. disable openpilot, restart")
self.params.put_bool("OpenpilotEnabledToggle", False)
self.params.put_int("SoftRestartTriggered", 1)
#acceleration_value = list(self.sm['liveLocationKalman'].accelerationCalibrated.value)
#if len(acceleration_value) > 2:
# if abs(acceleration_value[0]) > 16.0:
# print("Collision detected. disable openpilot, restart")
# self.params.put_bool("OpenpilotEnabledToggle", False)
# self.params.put_int("SoftRestartTriggered", 1)
CC.cruiseControl.override = CC.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise)
selfdrive/controls/lib/latcontrol.py
+1 -1
View File
@@ -17,7 +17,7 @@ class LatControl(ABC):
self.steer_max = 1.0
@abstractmethod
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited, model_data=None):
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, CC, curvature_limited, model_data=None):
pass
def reset(self):
selfdrive/controls/lib/latcontrol_angle.py
+1 -1
View File
@@ -17,7 +17,7 @@ class LatControlAngle(LatControl):
#self.factor = 0.5
#print("Angle factor", self.factor)
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited, model_data=None):
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, CC, curvature_limited, model_data=None):
angle_log = log.ControlsState.LateralAngleState.new_message()
if not active:
selfdrive/controls/lib/latcontrol_pid.py
+1 -1
View File
@@ -17,7 +17,7 @@ class LatControlPID(LatControl):
super().reset()
self.pid.reset()
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited, model_data=None):
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, CC, curvature_limited, model_data=None):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
selfdrive/controls/lib/latcontrol_torque.py
+4 -4
View File
@@ -139,7 +139,7 @@ class LatControlTorque(LatControl):
self.torque_params.latAccelOffset = latAccelOffset
self.torque_params.friction = friction
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, llk, curvature_limited, model_data=None):
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, CC, curvature_limited, model_data=None):
self.frame += 1
if self.frame % 10 == 0:
lateralTorqueCustom = self.params.get_int("LateralTorqueCustom")
@@ -181,7 +181,7 @@ class LatControlTorque(LatControl):
actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0)
actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2
else:
actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo
actual_curvature_llk = CC.angularVelocity[2] / CS.vEgo #llk.angularVelocityCalibrated.value[2] / CS.vEgo
actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk])
curvature_deadzone = 0.0
desired_lateral_accel = desired_curvature * CS.vEgo ** 2
@@ -219,8 +219,8 @@ class LatControlTorque(LatControl):
# update past data
pitch = 0
roll = params.roll
if len(llk.calibratedOrientationNED.value) > 1:
pitch = self.pitch.update(llk.calibratedOrientationNED.value[1])
if len(CC.orientionNED) > 1:
pitch = self.pitch.update(CC.orientationNED[1])
roll = roll_pitch_adjust(roll, pitch)
self.roll_deque.append(roll)
self.lateral_accel_desired_deque.append(desired_lateral_accel)
selfdrive/controls/lib/lateral_planner.py
+91 -9
View File
@@ -95,12 +95,16 @@ class LateralPlanner:
# clip speed , lateral planning is not possible at 0 speed
measured_curvature = sm['controlsState'].curvature
v_ego_car = sm['carState'].vEgo
v_ego_car = max(sm['carState'].vEgo, MIN_SPEED)
speed_kph = v_ego_car * 3.6
self.v_ego = v_ego_car
self.curve_speed = sm['carrotMan'].vTurnSpeed
# Parse model predictions
md = sm['modelV2']
model_active = False
if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE:
model_active = True
self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
self.t_idxs = np.array(md.position.t)
self.plan_yaw = np.array(md.orientation.z)
@@ -125,9 +129,9 @@ class LateralPlanner:
if self.useLaneLineSpeedApply == 0 or self.laneless_only:
self.useLaneLineMode = False
elif self.v_ego*3.6 >= self.useLaneLineSpeedApply + 2:
elif speed_kph >= self.useLaneLineSpeedApply + 2:
self.useLaneLineMode = True
elif self.v_ego*3.6 < self.useLaneLineSpeedApply - 2:
elif speed_kph < self.useLaneLineSpeedApply - 2:
self.useLaneLineMode = False
# Turn off lanes during lane change
@@ -143,10 +147,15 @@ class LateralPlanner:
self.LP.lane_width_left = md.meta.laneWidthLeft
self.LP.lane_width_right = md.meta.laneWidthRight
self.LP.curvature = measured_curvature
self.path_xyz, self.lanelines_active = self.LP.get_d_path(sm['carState'], self.v_ego, self.t_idxs, self.path_xyz, self.curve_speed)
#if self.LP.lanefull_mode:
# self.plan_yaw, self.plan_yaw_rate = self.LP.calculate_plan_yaw_and_yaw_rate(self.path_xyz)
self.path_xyz, self.lanelines_active = self.LP.get_d_path(sm['carState'], v_ego_car, self.t_idxs, self.path_xyz, self.curve_speed)
if self.lanelines_active:
self.plan_yaw, self.plan_yaw_rate = yaw_from_path_no_scipy(
self.path_xyz, self.v_plan,
smooth_window=5,
clip_rate=2.0,
align_first_yaw=None #md.orientation.z[0] # 초기 정렬
)
self.latDebugText = self.LP.debugText
#self.lanelines_active = True if self.LP.d_prob > 0.3 and self.LP.lanefull_mode else False
@@ -212,12 +221,14 @@ class LateralPlanner:
lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
lateralPlan.distances = self.lat_mpc.x_sol[0:CONTROL_N, 0].tolist()
v_div = np.maximum(self.v_plan[:CONTROL_N], 6.0)
if len(self.v_plan) == TRAJECTORY_SIZE:
lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3] / self.v_plan[0:CONTROL_N]).tolist()
lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3] / v_div).tolist()
else:
lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3] / self.v_ego).tolist()
lateralPlan.curvatureRates = [float(x.item() / self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
v_div2 = max(self.v_ego, 6.0)
lateralPlan.curvatureRates = [float(x.item() / v_div2) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
@@ -263,3 +274,74 @@ class LateralPlanner:
pm.send('lateralPlan', plan_send)
def smooth_moving_avg(arr, window=5):
if window < 2:
return arr
if window % 2 == 0:
window += 1
pad = window // 2
arr_pad = np.pad(arr, (pad, pad), mode='edge')
kernel = np.ones(window) / window
return np.convolve(arr_pad, kernel, mode='same')[pad:-pad]
def yaw_from_path_no_scipy(path_xyz, v_plan, smooth_window=5,
clip_rate=2.0, align_first_yaw=None):
v0 = float(np.asarray(v_plan)[0]) if len(v_plan) else 0.0
# 저속(≤6 m/s)에서는 창을 크게
if v0 <= 6.0:
smooth_window = max(smooth_window, 9) # 9~11 권장
N = path_xyz.shape[0]
x = path_xyz[:, 0].astype(float)
y = path_xyz[:, 1].astype(float)
if N < 5:
return np.zeros(N, np.float32), np.zeros(N, np.float32)
# 1) s(호길이) 계산
dx = np.diff(x)
dy = np.diff(y)
ds_seg = np.sqrt(dx*dx + dy*dy)
ds_seg[ds_seg < 0.05] = 0.05
s = np.zeros(N, float)
s[1:] = np.cumsum(ds_seg)
if s[-1] < 0.5: # 총 호길이 < 0.5m면 미분 결과 의미가 약함
return np.zeros(N, np.float32), np.zeros(N, np.float32)
# 2) smoothing (이동평균)
x_smooth = smooth_moving_avg(x, smooth_window)
y_smooth = smooth_moving_avg(y, smooth_window)
# 3) 1·2차 도함수(s축 미분)
dx_ds = np.gradient(x_smooth, s)
dy_ds = np.gradient(y_smooth, s)
d2x_ds2 = np.gradient(dx_ds, s)
d2y_ds2 = np.gradient(dy_ds, s)
# 4) yaw = atan2(dy/ds, dx/ds)
yaw = np.unwrap(np.arctan2(dy_ds, dx_ds))
# 5) 곡률 kappa = ...
denom = (dx_ds*dx_ds + dy_ds*dy_ds)**1.5
denom[denom < 1e-9] = 1e-9
kappa = (dx_ds * d2y_ds2 - dy_ds * d2x_ds2) / denom
# 6) yaw_rate = kappa * v
v = np.asarray(v_plan, float)
yaw_rate = kappa * v
if v0 <= 6.0:
# 이동평균으로 미세 요동 감쇄(창 5~7)
yaw_rate = smooth_moving_avg(yaw_rate, window=7)
# 7) 초기 yaw 정렬 (선택)
if align_first_yaw is not None:
bias = yaw[0] - float(align_first_yaw)
yaw = yaw - bias
# 8) 안정화
yaw = np.where(np.isfinite(yaw), yaw, 0.0)
yaw_rate = np.where(np.isfinite(yaw_rate), yaw_rate, 0.0)
yaw_rate = np.clip(yaw_rate, -abs(clip_rate), abs(clip_rate))
return yaw.astype(np.float32), yaw_rate.astype(np.float32)
selfdrive/locationd/.gitignore
-1
View File
@@ -1,3 +1,2 @@
params_learner
paramsd
locationd
selfdrive/locationd/SConscript
+1 -24
View File
@@ -1,6 +1,4 @@
Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
loc_libs = [messaging, common, 'pthread', 'dl']
Import('env', 'rednose')
# build ekf models
rednose_gen_dir = 'models/generated'
@@ -14,13 +12,6 @@ pose_ekf = env.RednoseCompileFilter(
extra_gen_artifacts=[],
gen_script_deps=rednose_gen_deps,
)
live_ekf = env.RednoseCompileFilter(
target='live',
filter_gen_script='models/live_kf.py',
output_dir=rednose_gen_dir,
extra_gen_artifacts=['live_kf_constants.h'],
gen_script_deps=rednose_gen_deps,
)
car_ekf = env.RednoseCompileFilter(
target='car',
filter_gen_script='models/car_kf.py',
@@ -28,17 +19,3 @@ car_ekf = env.RednoseCompileFilter(
extra_gen_artifacts=[],
gen_script_deps=rednose_gen_deps,
)
# locationd build
locationd_sources = ["locationd.cc", "models/live_kf.cc"]
lenv = env.Clone()
# ekf filter libraries need to be linked, even if no symbols are used
if arch != "Darwin":
lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"]
lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath)
lenv["RPATH"].append(Dir(rednose_gen_dir).abspath)
locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations)
lenv.Depends(locationd, rednose)
lenv.Depends(locationd, live_ekf)
selfdrive/locationd/locationd.cc
-774
View File
@@ -1,774 +0,0 @@
#include "selfdrive/locationd/locationd.h"
#include <sys/time.h>
#include <sys/resource.h>
#include <algorithm>
#include <cmath>
#include <vector>
using namespace EKFS;
using namespace Eigen;
ExitHandler do_exit;
const double ACCEL_SANITY_CHECK = 100.0; // m/s^2
const double ROTATION_SANITY_CHECK = 10.0; // rad/s
const double TRANS_SANITY_CHECK = 200.0; // m/s
const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg)
const double ALTITUDE_SANITY_CHECK = 10000; // m
const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
const double VALID_TIME_SINCE_RESET = 1.0; // s
const double VALID_POS_STD = 50.0; // m
const double MAX_RESET_TRACKER = 5.0;
const double SANE_GPS_UNCERTAINTY = 1500.0; // m
const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker
const double RESET_TRACKER_DECAY = 0.99995;
const double DECAY = 0.9993; // ~10 secs to resume after a bad input
const double MAX_FILTER_REWIND_TIME = 0.8; // s
const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30;
// TODO: GPS sensor time offsets are empirically calculated
// They should be replaced with synced time from a real clock
const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s
const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
const float GPS_POS_STD_THRESHOLD = 50.0;
const float GPS_VEL_STD_THRESHOLD = 5.0;
const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0;
const float GPS_POS_STD_RESET_THRESHOLD = 2.0;
const float GPS_VEL_STD_RESET_THRESHOLD = 0.5;
const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0;
const int GPS_ORIENTATION_ERROR_RESET_CNT = 3;
const bool DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0";
static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
VectorXd res(floatlist.size());
for (int i = 0; i < floatlist.size(); i++) {
res[i] = floatlist[i];
}
return res;
}
static Vector4d quat2vector(const Quaterniond& quat) {
return Vector4d(quat.w(), quat.x(), quat.y(), quat.z());
}
static Quaterniond vector2quat(const VectorXd& vec) {
return Quaterniond(vec(0), vec(1), vec(2), vec(3));
}
static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) {
meas.setValue(kj::arrayPtr(val.data(), val.size()));
meas.setStd(kj::arrayPtr(std.data(), std.size()));
meas.setValid(valid);
}
static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) {
// To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix
return ((rot_matrix * cov_in) * rot_matrix.transpose());
}
static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) {
// Stds cannot be rotated like values, only covariances can be rotated
return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt();
}
Localizer::Localizer(LocalizerGnssSource gnss_source) {
this->kf = std::make_unique<LiveKalman>();
this->reset_kalman();
this->calib = Vector3d(0.0, 0.0, 0.0);
this->device_from_calib = MatrixXdr::Identity(3, 3);
this->calib_from_device = MatrixXdr::Identity(3, 3);
for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) {
this->posenet_stds.push_back(10.0);
}
VectorXd ecef_pos = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
this->configure_gnss_source(gnss_source);
}
void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
VectorXd predicted_state = this->kf->get_x();
MatrixXdr predicted_cov = this->kf->get_P();
VectorXd predicted_std = predicted_cov.diagonal().array().sqrt();
VectorXd fix_ecef = predicted_state.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
VectorXd fix_ecef_std = predicted_std.segment<STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START);
VectorXd vel_ecef = predicted_state.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
VectorXd vel_ecef_std = predicted_std.segment<STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START);
VectorXd fix_pos_geo_vec = this->get_position_geodetic();
VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr orientation_ecef_cov = predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose();
VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose());
VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START);
VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt();
VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
MatrixXdr vel_angular_cov = predicted_cov.block<STATE_ANGULAR_VELOCITY_ERR_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START);
VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt();
VectorXd vel_device = device_from_ecef * vel_ecef;
VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose();
MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
condensed_cov.topLeftCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
condensed_cov.topRightCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
condensed_cov.bottomRightCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
condensed_cov.bottomLeftCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size());
H_input << device_from_ecef_eul, vel_ecef;
MatrixXdr HH = this->kf->H(H_input);
MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose();
VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt();
VectorXd vel_calib = this->calib_from_device * vel_device;
VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt();
VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt();
VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
VectorXd nextfix_ecef = fix_ecef + vel_ecef;
VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
VectorXd accDevice = predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
VectorXd accDeviceErr = predicted_std.segment<STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START);
VectorXd angVelocityDevice = predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
VectorXd angVelocityDeviceErr = predicted_std.segment<STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START);
Vector3d nans = Vector3d(NAN, NAN, NAN);
// TODO fill in NED and Calibrated stds
// write measurements to msg
init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode);
init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode);
init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode);
init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode);
init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode);
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode);
init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated);
if (DEBUG) {
init_measurement(fix.initFilterState(), predicted_state, predicted_std, true);
}
double old_mean = 0.0, new_mean = 0.0;
int i = 0;
for (double x : this->posenet_stds) {
if (i < POSENET_STD_HIST_HALF) {
old_mean += x;
} else {
new_mean += x;
}
i++;
}
old_mean /= POSENET_STD_HIST_HALF;
new_mean /= POSENET_STD_HIST_HALF;
// experimentally found these values, no false positives in 20k minutes of driving
bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0);
fix.setPosenetOK(!(std_spike && this->car_speed > 5.0));
fix.setDeviceStable(!this->device_fell);
fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER);
fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff);
this->device_fell = false;
//fix.setGpsWeek(this->time.week);
//fix.setGpsTimeOfWeek(this->time.tow);
fix.setUnixTimestampMillis(this->unix_timestamp_millis);
double time_since_reset = this->kf->get_filter_time() - this->last_reset_time;
fix.setTimeSinceReset(time_since_reset);
if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) {
fix.setStatus(cereal::LiveLocationKalman::Status::VALID);
} else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) {
fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED);
} else {
fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED);
}
}
VectorXd Localizer::get_position_geodetic() {
VectorXd fix_ecef = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef);
return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt);
}
VectorXd Localizer::get_state() {
return this->kf->get_x();
}
VectorXd Localizer::get_stdev() {
return this->kf->get_P().diagonal().array().sqrt();
}
bool Localizer::are_inputs_ok() {
return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid;
}
void Localizer::observation_timings_invalid_reset(){
this->observation_timings_invalid = false;
}
void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) {
// TODO does not yet account for double sensor readings in the log
// Ignore empty readings (e.g. in case the magnetometer had no data ready)
if (log.getTimestamp() == 0) {
return;
}
double sensor_time = 1e-9 * log.getTimestamp();
// sensor time and log time should be close
if (std::abs(current_time - sensor_time) > 0.1) {
LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
this->observation_timings_invalid = true;
return;
} else if (!this->is_timestamp_valid(sensor_time)) {
this->observation_timings_invalid = true;
return;
}
// TODO: handle messages from two IMUs at the same time
if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
return;
}
// Gyro Uncalibrated
if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
auto v = log.getGyroUncalibrated().getV();
auto meas = Vector3d(-v[2], -v[1], -v[0]);
VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START);
float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]);
float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1];
bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold;
if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) {
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
this->observation_values_invalid["gyroscope"] *= DECAY;
} else {
this->observation_values_invalid["gyroscope"] += 1.0;
}
}
// Accelerometer
if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) {
auto v = log.getAcceleration().getV();
// TODO: reduce false positives and re-enable this check
// check if device fell, estimate 10 for g
// 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
// this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
auto meas = Vector3d(-v[2], -v[1], -v[0]);
if (meas.norm() < ACCEL_SANITY_CHECK) {
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
this->observation_values_invalid["accelerometer"] *= DECAY;
} else {
this->observation_values_invalid["accelerometer"] += 1.0;
}
}
}
void Localizer::input_fake_gps_observations(double current_time) {
// This is done to make sure that the error estimate of the position does not blow up
// when the filter is in no-gps mode
// Steps : first predict -> observe current obs with reasonable STD
this->kf->predict(current_time);
VectorXd current_x = this->kf->get_x();
VectorXd ecef_pos = current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov();
const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov();
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
}
void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) {
bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
//this->gps_valid = false;
this->determine_gps_mode(current_time);
return;
}
double sensor_time = current_time - sensor_time_offset;
// Process message
//this->gps_valid = true;
this->gps_mode = true;
Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
this->converter = std::make_unique<LocalCoord>(geodetic);
VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2));
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
this->unix_timestamp_millis = log.getUnixTimestampMillis();
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef);
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg()));
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
for (int i = 0; i < orientation_error.size(); i++) {
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
if (orientation_error(i) < 0.0) {
orientation_error(i) += 2.0 * M_PI;
}
orientation_error(i) -= M_PI;
}
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) {
LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
} else if (gps_est_error > 100.0) {
LOGE("Locationd vs ubloxLocation position difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
}
this->last_gps_msg = sensor_time;
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
}
void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) {
if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
this->determine_gps_mode(current_time);
return;
}
double sensor_time = log.getMeasTime() * 1e-9;
sensor_time -= this->gps_time_offset;
auto ecef_pos_v = log.getPositionECEF().getValue();
VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]);
// indexed at 0 cause all std values are the same MAE
auto ecef_pos_std = log.getPositionECEF().getStd()[0];
MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal();
auto ecef_vel_v = log.getVelocityECEF().getValue();
VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]);
// indexed at 0 cause all std values are the same MAE
auto ecef_vel_std = log.getVelocityECEF().getStd()[0];
MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal();
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef);
LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]};
VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector();
double bearing_rad = atan2(ned_vel[1], ned_vel[0]);
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad);
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
for (int i = 0; i < orientation_error.size(); i++) {
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
if (orientation_error(i) < 0.0) {
orientation_error(i) += 2.0 * M_PI;
}
orientation_error(i) -= M_PI;
}
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
this->determine_gps_mode(current_time);
return;
}
// prevent jumping gnss measurements (covered lots, standstill...)
bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD;
orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD;
orientation_reset &= !this->standstill;
if (orientation_reset) {
this->orientation_reset_count++;
} else {
this->orientation_reset_count = 0;
}
if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
// always reset on first gps message and if the location is off but the accuracy is high
LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
} else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) {
LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
this->orientation_reset_count = 0;
}
this->gps_mode = true;
this->last_gps_msg = sensor_time;
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
}
void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) {
this->car_speed = std::abs(log.getVEgo());
this->standstill = log.getStandstill();
if (this->standstill) {
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) });
}
}
void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) {
VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
if (!this->is_timestamp_valid(current_time)) {
this->observation_timings_invalid = true;
return;
}
if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
this->posenet_stds.pop_front();
this->posenet_stds.push_back(trans_calib_std[0]);
// Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise
trans_calib_std *= 10.0;
rot_calib_std *= 10.0;
MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal();
MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal();
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
{ rot_device }, { rot_device_cov });
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
{ trans_device }, { trans_device_cov });
this->observation_values_invalid["cameraOdometry"] *= DECAY;
this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]);
}
void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {
if (!this->is_timestamp_valid(current_time)) {
this->observation_timings_invalid = true;
return;
}
if (log.getRpyCalib().size() > 0) {
auto live_calib = floatlist2vector(log.getRpyCalib());
if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
this->observation_values_invalid["liveCalibration"] += 1.0;
return;
}
this->calib = live_calib;
this->device_from_calib = euler2rot(this->calib);
this->calib_from_device = this->device_from_calib.transpose();
this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED;
this->observation_values_invalid["liveCalibration"] *= DECAY;
}
}
void Localizer::reset_kalman(double current_time) {
const VectorXd &init_x = this->kf->get_initial_x();
const MatrixXdr &init_P = this->kf->get_initial_P();
this->reset_kalman(current_time, init_x, init_P);
}
void Localizer::finite_check(double current_time) {
bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all();
if (!all_finite) {
LOGE("Non-finite values detected, kalman reset");
this->reset_kalman(current_time);
}
}
void Localizer::time_check(double current_time) {
if (std::isnan(this->last_reset_time)) {
this->last_reset_time = current_time;
}
if (std::isnan(this->first_valid_log_time)) {
this->first_valid_log_time = current_time;
}
double filter_time = this->kf->get_filter_time();
bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10);
if (big_time_gap) {
LOGE("Time gap of over 10s detected, kalman reset");
this->reset_kalman(current_time);
}
}
void Localizer::update_reset_tracker() {
// reset tracker is tuned to trigger when over 1reset/10s over 2min period
if (this->is_gps_ok()) {
this->reset_tracker *= RESET_TRACKER_DECAY;
} else {
this->reset_tracker = 0.0;
}
}
void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) {
// too nonlinear to init on completely wrong
VectorXd current_x = this->kf->get_x();
MatrixXdr current_P = this->kf->get_P();
MatrixXdr init_P = this->kf->get_initial_P();
const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P();
int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
current_x.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient;
current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START) = init_vel;
current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) = init_pos;
init_P.block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal();
init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START,
STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal();
this->reset_kalman(current_time, current_x, init_P);
}
void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) {
this->kf->init_state(init_x, init_P, current_time);
this->last_reset_time = current_time;
this->reset_tracker += 1.0;
}
void Localizer::handle_msg_bytes(const char *data, const size_t size) {
AlignedBuffer aligned_buf;
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size));
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
this->handle_msg(event);
}
void Localizer::handle_msg(const cereal::Event::Reader& log) {
double t = log.getLogMonoTime() * 1e-9;
this->time_check(t);
if (log.isAccelerometer()) {
this->handle_sensor(t, log.getAccelerometer());
} else if (log.isGyroscope()) {
this->handle_sensor(t, log.getGyroscope());
} else if (log.isGpsLocation()) {
this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
} else if (log.isGpsLocationExternal()) {
this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
//} else if (log.isGnssMeasurements()) {
// this->handle_gnss(t, log.getGnssMeasurements());
} else if (log.isCarState()) {
this->handle_car_state(t, log.getCarState());
} else if (log.isCameraOdometry()) {
this->handle_cam_odo(t, log.getCameraOdometry());
} else if (log.isLiveCalibration()) {
this->handle_live_calib(t, log.getLiveCalibration());
}
this->finite_check();
this->update_reset_tracker();
}
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK,
bool sensorsOK, bool gpsOK, bool msgValid) {
cereal::Event::Builder evt = msg_builder.initEvent();
evt.setValid(msgValid);
cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
this->build_live_location(liveLoc);
liveLoc.setSensorsOK(sensorsOK);
liveLoc.setGpsOK(gpsOK);
liveLoc.setInputsOK(inputsOK);
return msg_builder.toBytes();
}
bool Localizer::is_gps_ok() {
return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0;
}
bool Localizer::critical_services_valid(const std::map<std::string, double> &critical_services) {
for (auto &kv : critical_services){
if (kv.second >= INPUT_INVALID_THRESHOLD){
return false;
}
}
return true;
}
bool Localizer::is_timestamp_valid(double current_time) {
double filter_time = this->kf->get_filter_time();
if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) {
LOGE("Observation timestamp is older than the max rewind threshold of the filter");
return false;
}
return true;
}
void Localizer::determine_gps_mode(double current_time) {
// 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode
// 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs
// 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is
VectorXd current_pos_std = this->kf->get_P().block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt();
if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){
if (this->gps_mode){
this->gps_mode = false;
this->reset_kalman(current_time);
} else {
this->input_fake_gps_observations(current_time);
}
}
}
void Localizer::configure_gnss_source(const LocalizerGnssSource &source) {
this->gnss_source = source;
if (source == LocalizerGnssSource::UBLOX) {
this->gps_std_factor = 10.0;
this->gps_variance_factor = 1.0;
this->gps_vertical_variance_factor = 1.0;
this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET;
} else {
this->gps_std_factor = 2.0;
this->gps_variance_factor = 0.0;
this->gps_vertical_variance_factor = 3.0;
this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET;
}
}
int Localizer::locationd_thread() {
Params params;
LocalizerGnssSource source;
const char* gps_location_socket;
if (params.getBool("UbloxAvailable")) {
source = LocalizerGnssSource::UBLOX;
gps_location_socket = "gpsLocationExternal";
} else {
source = LocalizerGnssSource::QCOM;
gps_location_socket = "gpsLocation";
}
this->configure_gnss_source(source);
const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
"carState", "accelerometer", "gyroscope"};
SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
PubMaster pm({"liveLocationKalman"});
uint64_t cnt = 0;
bool filterInitialized = false;
const std::vector<std::string> critical_input_services = {"cameraOdometry", "liveCalibration", "accelerometer", "gyroscope"};
for (std::string service : critical_input_services) {
this->observation_values_invalid.insert({service, 0.0});
}
bool ignore_gps = true;
while (!do_exit) {
sm.update();
if (filterInitialized){
this->observation_timings_invalid_reset();
for (const char* service : service_list) {
if (sm.updated(service) && sm.valid(service)){
const cereal::Event::Reader log = sm[service];
this->handle_msg(log);
}
}
} else {
//filterInitialized = sm.allAliveAndValid();
bool allValid = true;
for (const char* service : service_list) {
if (service != gps_location_socket && !sm.valid(service)) {
allValid = false;
break;
}
}
filterInitialized = allValid;
}
const char* trigger_msg = "cameraOdometry";
if (sm.updated(trigger_msg)) {
bool inputsOK = sm.allValid() && this->are_inputs_ok();
if (ignore_gps) {
inputsOK = this->are_inputs_ok();
}
bool gpsOK = this->is_gps_ok();
bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"});
/*
if (!sm.allValid()) {
for (const char* service : service_list) {
if (!sm.valid(service)) {
printf("Service %s is INVALID! (Alive: %d)\n", service, sm.alive(service));
}
}
}
printf("InputsOK: %d, SensorsOK: %d, GPSOK: %d, FilterInitialized: %d\n", inputsOK, sensorsOK, gpsOK, filterInitialized);
*/
// Log time to first fix
if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) {
this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time);
}
MessageBuilder msg_builder;
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
pm.send("liveLocationKalman", bytes.begin(), bytes.size());
if (cnt % 1200 == 0 && gpsOK) { // once a minute
//ignore_gps = false;
VectorXd posGeo = this->get_position_geodetic();
std::string lastGPSPosJSON = util::string_format(
"{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
params.putNonBlocking("LastGPSPosition", lastGPSPosJSON);
}
cnt++;
}
}
return 0;
}
int main() {
util::set_realtime_priority(5);
Localizer localizer;
return localizer.locationd_thread();
}
selfdrive/locationd/locationd.h
-100
View File
@@ -1,100 +0,0 @@
#pragma once
#include <eigen3/Eigen/Dense>
#include <deque>
#include <fstream>
#include <memory>
#include <map>
#include <string>
#include "cereal/messaging/messaging.h"
#include "common/transformations/coordinates.hpp"
#include "common/transformations/orientation.hpp"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
#include "system/sensord/sensors/constants.h"
#define VISION_DECIMATION 2
#define SENSOR_DECIMATION 10
#include "selfdrive/locationd/models/live_kf.h"
#define POSENET_STD_HIST_HALF 20
enum LocalizerGnssSource {
UBLOX, QCOM
};
class Localizer {
public:
Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX);
int locationd_thread();
void reset_kalman(double current_time = NAN);
void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R);
void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P);
void finite_check(double current_time = NAN);
void time_check(double current_time = NAN);
void update_reset_tracker();
bool is_gps_ok();
bool critical_services_valid(const std::map<std::string, double> &critical_services);
bool is_timestamp_valid(double current_time);
void determine_gps_mode(double current_time);
bool are_inputs_ok();
void observation_timings_invalid_reset();
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
void build_live_location(cereal::LiveLocationKalman::Builder& fix);
Eigen::VectorXd get_position_geodetic();
Eigen::VectorXd get_state();
Eigen::VectorXd get_stdev();
void handle_msg_bytes(const char *data, const size_t size);
void handle_msg(const cereal::Event::Reader& log);
void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log);
void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset);
void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log);
void handle_car_state(double current_time, const cereal::CarState::Reader& log);
void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log);
void input_fake_gps_observations(double current_time);
private:
std::unique_ptr<LiveKalman> kf;
Eigen::VectorXd calib;
MatrixXdr device_from_calib;
MatrixXdr calib_from_device;
bool calibrated = false;
double car_speed = 0.0;
double last_reset_time = NAN;
std::deque<double> posenet_stds;
std::unique_ptr<LocalCoord> converter;
int64_t unix_timestamp_millis = 0;
double reset_tracker = 0.0;
bool device_fell = false;
bool gps_mode = false;
double first_valid_log_time = NAN;
double ttff = NAN;
double last_gps_msg = 0;
LocalizerGnssSource gnss_source;
bool observation_timings_invalid = false;
std::map<std::string, double> observation_values_invalid;
bool standstill = true;
int32_t orientation_reset_count = 0;
float gps_std_factor;
float gps_variance_factor;
float gps_vertical_variance_factor;
double gps_time_offset;
Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std
void configure_gnss_source(const LocalizerGnssSource &source);
};
selfdrive/locationd/models/car_kf.py
+1 -1
View File
@@ -5,7 +5,7 @@ from typing import Any
import numpy as np
from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.selfdrive.locationd.models.constants import ObservationKind
from openpilot.common.swaglog import cloudlog
selfdrive/locationd/models/live_kf.cc
-122
View File
@@ -1,122 +0,0 @@
#include "selfdrive/locationd/models/live_kf.h"
using namespace EKFS;
using namespace Eigen;
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
}
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
}
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
std::vector<Eigen::Map<Eigen::VectorXd>> res;
for (const Eigen::VectorXd &vec : vec_vec) {
res.push_back(get_mapvec(vec));
}
return res;
}
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
std::vector<Eigen::Map<MatrixXdr>> res;
for (const MatrixXdr &mat : mat_vec) {
res.push_back(get_mapmat(mat));
}
return res;
}
LiveKalman::LiveKalman() {
this->dim_state = live_initial_x.rows();
this->dim_state_err = live_initial_P_diag.rows();
this->initial_x = live_initial_x;
this->initial_P = live_initial_P_diag.asDiagonal();
this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal();
this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal();
this->reset_orientation_P = live_reset_orientation_diag.asDiagonal();
this->Q = live_Q_diag.asDiagonal();
for (auto& pair : live_obs_noise_diag) {
this->obs_noise[pair.first] = pair.second.asDiagonal();
}
// init filter
this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
std::vector<int>{3}, std::vector<std::string>(), 0.8);
}
void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
MatrixXdr covs = covs_diag.asDiagonal();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
void LiveKalman::init_state(const VectorXd &state, double filter_time) {
MatrixXdr covs = this->filter->covs();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
VectorXd LiveKalman::get_x() {
return this->filter->state();
}
MatrixXdr LiveKalman::get_P() {
return this->filter->covs();
}
double LiveKalman::get_filter_time() {
return this->filter->get_filter_time();
}
std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) {
std::vector<MatrixXdr> R;
for (int i = 0; i < n; i++) {
R.push_back(this->obs_noise[kind]);
}
return R;
}
std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
std::optional<Estimate> r;
if (R.size() == 0) {
R = this->get_R(kind, meas.size());
}
r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
return r;
}
void LiveKalman::predict(double t) {
this->filter->predict(t);
}
const Eigen::VectorXd &LiveKalman::get_initial_x() {
return this->initial_x;
}
const MatrixXdr &LiveKalman::get_initial_P() {
return this->initial_P;
}
const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() {
return this->fake_gps_pos_cov;
}
const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() {
return this->fake_gps_vel_cov;
}
const MatrixXdr &LiveKalman::get_reset_orientation_P() {
return this->reset_orientation_P;
}
MatrixXdr LiveKalman::H(const VectorXd &in) {
assert(in.size() == 6);
Matrix<double, 3, 6, Eigen::RowMajor> res;
this->filter->get_extra_routine("H")((double*)in.data(), res.data());
return res;
}
selfdrive/locationd/models/live_kf.h
-66
View File
@@ -1,66 +0,0 @@
#pragma once
#include <string>
#include <cmath>
#include <memory>
#include <unordered_map>
#include <vector>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include "generated/live_kf_constants.h"
#include "rednose/helpers/ekf_sym.h"
#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth)
using namespace EKFS;
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
class LiveKalman {
public:
LiveKalman();
void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
void init_state(const Eigen::VectorXd &state, double filter_time);
Eigen::VectorXd get_x();
MatrixXdr get_P();
double get_filter_time();
std::vector<MatrixXdr> get_R(int kind, int n);
std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
void predict(double t);
const Eigen::VectorXd &get_initial_x();
const MatrixXdr &get_initial_P();
const MatrixXdr &get_fake_gps_pos_cov();
const MatrixXdr &get_fake_gps_vel_cov();
const MatrixXdr &get_reset_orientation_P();
MatrixXdr H(const Eigen::VectorXd &in);
private:
std::string name = "live";
std::shared_ptr<EKFSym> filter;
int dim_state;
int dim_state_err;
Eigen::VectorXd initial_x;
MatrixXdr initial_P;
MatrixXdr fake_gps_pos_cov;
MatrixXdr fake_gps_vel_cov;
MatrixXdr reset_orientation_P;
MatrixXdr Q; // process noise
std::unordered_map<int, MatrixXdr> obs_noise;
};
selfdrive/locationd/models/live_kf.py
-242
View File
@@ -1,242 +0,0 @@
#!/usr/bin/env python3
import sys
import os
import numpy as np
from openpilot.selfdrive.locationd.models.constants import ObservationKind
import sympy as sp
import inspect
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
from rednose.helpers.ekf_sym import gen_code
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
def numpy2eigenstring(arr):
assert(len(arr.shape) == 1)
arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '')
return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()"
class States:
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters
ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases
ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2
ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2
# Error-state has different slices because it is an ESKF
ECEF_POS_ERR = slice(0, 3)
ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error
ECEF_VELOCITY_ERR = slice(6, 9)
ANGULAR_VELOCITY_ERR = slice(9, 12)
GYRO_BIAS_ERR = slice(12, 15)
ACCELERATION_ERR = slice(15, 18)
ACC_BIAS_ERR = slice(18, 21)
class LiveKalman:
name = 'live'
initial_x = np.array([3.88e6, -3.37e6, 3.76e6,
0.42254641, -0.31238054, -0.83602975, -0.15788347, # NED [0,0,0] -> ECEF Quat
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0])
# state covariance
initial_P_diag = np.array([10**2, 10**2, 10**2,
0.01**2, 0.01**2, 0.01**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
1**2, 1**2, 1**2,
100**2, 100**2, 100**2,
0.01**2, 0.01**2, 0.01**2])
# state covariance when resetting midway in a segment
reset_orientation_diag = np.array([1**2, 1**2, 1**2])
# fake observation covariance, to ensure the uncertainty estimate of the filter is under control
fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
# process noise
Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
0.001**2, 0.001**2, 0.001**2,
0.01**2, 0.01**2, 0.01**2,
0.1**2, 0.1**2, 0.1**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
3**2, 3**2, 3**2,
0.005**2, 0.005**2, 0.005**2])
obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])}
@staticmethod
def generate_code(generated_dir):
name = LiveKalman.name
dim_state = LiveKalman.initial_x.shape[0]
dim_state_err = LiveKalman.initial_P_diag.shape[0]
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
x, y, z = state[States.ECEF_POS, :]
q = state[States.ECEF_ORIENTATION, :]
v = state[States.ECEF_VELOCITY, :]
vx, vy, vz = v
omega = state[States.ANGULAR_VELOCITY, :]
vroll, vpitch, vyaw = omega
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
acceleration = state[States.ACCELERATION, :]
acc_bias = state[States.ACC_BIAS, :]
dt = sp.Symbol('dt')
# calibration and attitude rotation matrices
quat_rot = quat_rotate(*q)
# Got the quat predict equations from here
# A New Quaternion-Based Kalman Filter for
# Real-Time Attitude Estimation Using the Two-Step
# Geometrically-Intuitive Correction Algorithm
A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
[vroll, 0, vyaw, -vpitch],
[vpitch, -vyaw, 0, vroll],
[vyaw, vpitch, -vroll, 0]])
q_dot = A * q
# Time derivative of the state as a function of state
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.ECEF_POS, :] = v
state_dot[States.ECEF_ORIENTATION, :] = q_dot
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
# Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
state_err = sp.Matrix(state_err_sym)
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
acceleration_err = state_err[States.ACCELERATION_ERR, :]
# Time derivative of the state error as a function of state error and state
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
state_err_dot[States.ECEF_POS_ERR, :] = v_err
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
f_err_sym = state_err + dt * state_err_dot
# Observation matrix modifier
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
# these error functions are defined so that say there
# is a nominal x and true x:
# true x = err_function(nominal x, delta x)
# delta x = inv_err_function(nominal x, true x)
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
delta_quat = sp.Matrix(np.ones(4))
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
eskf_params = [[err_function_sym, nom_x, delta_x],
[inv_err_function_sym, nom_x, true_x],
H_mod_sym, f_err_sym, state_err_sym]
#
# Observation functions
#
h_gyro_sym = sp.Matrix([
vroll + roll_bias,
vpitch + pitch_bias,
vyaw + yaw_bias])
pos = sp.Matrix([x, y, z])
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
h_acc_sym = (gravity + acceleration + acc_bias)
h_acc_stationary_sym = acceleration
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
h_pos_sym = sp.Matrix([x, y, z])
h_vel_sym = sp.Matrix([vx, vy, vz])
h_orientation_sym = q
h_relative_motion = sp.Matrix(quat_rot.T * v)
obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pos_sym, ObservationKind.ECEF_POS, None],
[h_vel_sym, ObservationKind.ECEF_VEL, None],
[h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
# this returns a sympy routine for the jacobian of the observation function of the local vel
in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz
h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
# write constants to extra header file for use in cpp
live_kf_header = "#pragma once\n\n"
live_kf_header += "#include <unordered_map>\n"
live_kf_header += "#include <eigen3/Eigen/Dense>\n\n"
for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)):
assert(slc.step is None) # unsupported
live_kf_header += f'#define STATE_{state}_START {slc.start}\n'
live_kf_header += f'#define STATE_{state}_END {slc.stop}\n'
live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n'
live_kf_header += "\n"
for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)):
live_kf_header += f'#define OBSERVATION_{kind} {val}\n'
live_kf_header += "\n"
live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n"
live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n"
live_kf_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {\n"
for kind, noise in LiveKalman.obs_noise_diag.items():
live_kf_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n"
live_kf_header += "};\n\n"
open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header)
if __name__ == "__main__":
generated_dir = sys.argv[2]
LiveKalman.generate_code(generated_dir)
selfdrive/locationd/paramsd.py
+13 -8
View File
@@ -15,6 +15,8 @@ from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
from openpilot.common.swaglog import cloudlog
from openpilot.common.gps import get_gps_location_service
MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s
ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits
ROLL_MIN, ROLL_MAX = np.radians(-10), np.radians(10)
@@ -245,7 +247,9 @@ def retrieve_initial_vehicle_params(params: Params, CP: car.CarParams, replay: b
if debug and len(initial_filter_std) != 0:
p_initial = np.diag(initial_filter_std)
steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg
#steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg
#steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetDeg
steer_ratio, stiffness_factor = lp.steerRatio, lp.stiffnessFactor
retrieve_success = True
except Exception as e:
cloudlog.error(f"Failed to retrieve initial values: {e}")
@@ -269,7 +273,8 @@ def main():
REPLAY = bool(int(os.getenv("REPLAY", "0")))
pm = messaging.PubMaster(['liveParameters'])
sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', 'liveLocationKalman'], poll='livePose')
gps_location_service = get_gps_location_service(Params())
sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', gps_location_service], poll='livePose', ignore_alive=[gps_location_service], ignore_valid=[gps_location_service])
params = Params()
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
@@ -289,12 +294,12 @@ def main():
t = sm.logMonoTime[which] * 1e-9
learner.handle_log(t, which, sm[which])
if sm.updated['liveLocationKalman']:
location = sm['liveLocationKalman']
if (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid and location.gpsOK:
bearing = math.degrees(location.calibratedOrientationNED.value[2])
lat = location.positionGeodetic.value[0]
lon = location.positionGeodetic.value[1]
if sm.updated[gps_location_service]:
gps = sm[gps_location_service]
if gps.hasFix:
bearing = gps.bearingDeg
lat = gps.latitude
lon = gps.longitude
params_memory.put("LastGPSPosition", json.dumps({"latitude": lat, "longitude": lon, "bearing": bearing}))
selfdrive/locationd/torqued.py
+1 -1
View File
@@ -5,7 +5,7 @@ from collections import deque, defaultdict
import cereal.messaging as messaging
from cereal import car, log
from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process, DT_MDL
from openpilot.common.filter_simple import FirstOrderFilter
selfdrive/modeld/SConscript
+6 -6
View File
@@ -1,11 +1,11 @@
import os
import glob
Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'transformations')
Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'visionipc', 'transformations')
lenv = env.Clone()
lenvCython = envCython.Clone()
libs = [cereal, messaging, visionipc, gpucommon, common, 'capnp', 'kj', 'pthread']
libs = [cereal, messaging, visionipc, common, 'capnp', 'kj', 'pthread']
frameworks = []
common_src = [
@@ -32,7 +32,7 @@ lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LI
tinygrad_files = ["#"+x for x in glob.glob(env.Dir("#tinygrad_repo").relpath + "/**", recursive=True, root_dir=env.Dir("#").abspath) if 'pycache' not in x]
# Get model metadata
for model_name in ['driving_vision', 'driving_policy']:
for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']:
fn = File(f"models/{model_name}").abspath
script_files = [File(Dir("#selfdrive/modeld").File("get_model_metadata.py").abspath)]
cmd = f'python3 {Dir("#selfdrive/modeld").abspath}/get_model_metadata.py {fn}.onnx'
@@ -50,9 +50,9 @@ def tg_compile(flags, model_name):
# Compile small models
for model_name in ['driving_vision', 'driving_policy', 'dmonitoring_model']:
flags = {
'larch64': 'DEV=QCOM',
'Darwin': 'DEV=CPU IMAGE=0',
}.get(arch, 'DEV=LLVM IMAGE=0')
'larch64': 'DEV=QCOM FLOAT16=1 NOLOCALS=1 IMAGE=2 JIT_BATCH_SIZE=0',
'Darwin': f'DEV=CPU HOME={os.path.expanduser("~")}', # tinygrad calls brew which needs a $HOME in the env
}.get(arch, 'DEV=CPU CPU_LLVM=1')
tg_compile(flags, model_name)
# Compile BIG model if USB GPU is available
selfdrive/modeld/dmonitoringmodeld.py
+51 -73
View File
@@ -1,14 +1,9 @@
#!/usr/bin/env python3
import os
from openpilot.system.hardware import TICI
os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
from tinygrad.tensor import Tensor
from tinygrad.dtype import dtypes
if TICI:
from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
os.environ['QCOM'] = '1'
else:
os.environ['LLVM'] = '1'
import math
import time
import pickle
import ctypes
@@ -21,48 +16,16 @@ from cereal.messaging import PubMaster, SubMaster
from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
from openpilot.common.swaglog import cloudlog
from openpilot.common.realtime import config_realtime_process
from openpilot.common.transformations.model import dmonitoringmodel_intrinsics, DM_INPUT_SIZE
from openpilot.common.transformations.model import dmonitoringmodel_intrinsics
from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame
from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid
from openpilot.system import sentry
MODEL_WIDTH, MODEL_HEIGHT = DM_INPUT_SIZE
CALIB_LEN = 3
FEATURE_LEN = 512
OUTPUT_SIZE = 84 + FEATURE_LEN
from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid, safe_exp
from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
PROCESS_NAME = "selfdrive.modeld.dmonitoringmodeld"
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl'
class DriverStateResult(ctypes.Structure):
_fields_ = [
("face_orientation", ctypes.c_float*3),
("face_position", ctypes.c_float*3),
("face_orientation_std", ctypes.c_float*3),
("face_position_std", ctypes.c_float*3),
("face_prob", ctypes.c_float),
("_unused_a", ctypes.c_float*8),
("left_eye_prob", ctypes.c_float),
("_unused_b", ctypes.c_float*8),
("right_eye_prob", ctypes.c_float),
("left_blink_prob", ctypes.c_float),
("right_blink_prob", ctypes.c_float),
("sunglasses_prob", ctypes.c_float),
("occluded_prob", ctypes.c_float),
("ready_prob", ctypes.c_float*4),
("not_ready_prob", ctypes.c_float*2)]
class DMonitoringModelResult(ctypes.Structure):
_fields_ = [
("driver_state_lhd", DriverStateResult),
("driver_state_rhd", DriverStateResult),
("poor_vision_prob", ctypes.c_float),
("wheel_on_right_prob", ctypes.c_float),
("features", ctypes.c_float*FEATURE_LEN)]
METADATA_PATH = Path(__file__).parent / 'models/dmonitoring_model_metadata.pkl'
class ModelState:
@@ -70,11 +33,14 @@ class ModelState:
output: np.ndarray
def __init__(self, cl_ctx):
assert ctypes.sizeof(DMonitoringModelResult) == OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float)
with open(METADATA_PATH, 'rb') as f:
model_metadata = pickle.load(f)
self.input_shapes = model_metadata['input_shapes']
self.output_slices = model_metadata['output_slices']
self.frame = MonitoringModelFrame(cl_ctx)
self.numpy_inputs = {
'calib': np.zeros((1, CALIB_LEN), dtype=np.float32),
'calib': np.zeros(self.input_shapes['calib'], dtype=np.float32),
}
self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
@@ -90,45 +56,53 @@ class ModelState:
if TICI:
# The imgs tensors are backed by opencl memory, only need init once
if 'input_img' not in self.tensor_inputs:
self.tensor_inputs['input_img'] = qcom_tensor_from_opencl_address(input_img_cl.mem_address, (1, MODEL_WIDTH*MODEL_HEIGHT), dtype=dtypes.uint8)
self.tensor_inputs['input_img'] = qcom_tensor_from_opencl_address(input_img_cl.mem_address, self.input_shapes['input_img'], dtype=dtypes.uint8)
else:
self.tensor_inputs['input_img'] = Tensor(self.frame.buffer_from_cl(input_img_cl).reshape((1, MODEL_WIDTH*MODEL_HEIGHT)), dtype=dtypes.uint8).realize()
self.tensor_inputs['input_img'] = Tensor(self.frame.buffer_from_cl(input_img_cl).reshape(self.input_shapes['input_img']), dtype=dtypes.uint8).realize()
output = self.model_run(**self.tensor_inputs).numpy().flatten()
output = self.model_run(**self.tensor_inputs).contiguous().realize().uop.base.buffer.numpy()
t2 = time.perf_counter()
return output, t2 - t1
def slice_outputs(model_outputs, output_slices):
return {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()}
def fill_driver_state(msg, ds_result: DriverStateResult):
msg.faceOrientation = list(ds_result.face_orientation)
msg.faceOrientationStd = [math.exp(x) for x in ds_result.face_orientation_std]
msg.facePosition = list(ds_result.face_position[:2])
msg.facePositionStd = [math.exp(x) for x in ds_result.face_position_std[:2]]
msg.faceProb = float(sigmoid(ds_result.face_prob))
msg.leftEyeProb = float(sigmoid(ds_result.left_eye_prob))
msg.rightEyeProb = float(sigmoid(ds_result.right_eye_prob))
msg.leftBlinkProb = float(sigmoid(ds_result.left_blink_prob))
msg.rightBlinkProb = float(sigmoid(ds_result.right_blink_prob))
msg.sunglassesProb = float(sigmoid(ds_result.sunglasses_prob))
msg.occludedProb = float(sigmoid(ds_result.occluded_prob))
msg.readyProb = [float(sigmoid(x)) for x in ds_result.ready_prob]
msg.notReadyProb = [float(sigmoid(x)) for x in ds_result.not_ready_prob]
def parse_model_output(model_output):
parsed = {}
parsed['wheel_on_right'] = sigmoid(model_output['wheel_on_right'])
for ds_suffix in ['lhd', 'rhd']:
face_descs = model_output[f'face_descs_{ds_suffix}']
parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6]
parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:])
for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']:
parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
return parsed
def fill_driver_data(msg, model_output, ds_suffix):
msg.faceOrientation = model_output[f'face_descs_{ds_suffix}'][0, :3].tolist()
msg.faceOrientationStd = model_output[f'face_descs_{ds_suffix}_std'][0, :3].tolist()
msg.facePosition = model_output[f'face_descs_{ds_suffix}'][0, 3:5].tolist()
msg.facePositionStd = model_output[f'face_descs_{ds_suffix}_std'][0, 3:5].tolist()
msg.faceProb = model_output[f'face_prob_{ds_suffix}'][0, 0].item()
msg.leftEyeProb = model_output[f'left_eye_prob_{ds_suffix}'][0, 0].item()
msg.rightEyeProb = model_output[f'right_eye_prob_{ds_suffix}'][0, 0].item()
msg.leftBlinkProb = model_output[f'left_blink_prob_{ds_suffix}'][0, 0].item()
msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item()
msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item()
msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item()
def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: int, execution_time: float, gpu_execution_time: float):
model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(DMonitoringModelResult)).contents
def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float):
msg = messaging.new_message('driverStateV2', valid=True)
ds = msg.driverStateV2
ds.frameId = frame_id
ds.modelExecutionTime = execution_time
ds.gpuExecutionTime = gpu_execution_time
ds.poorVisionProb = float(sigmoid(model_result.poor_vision_prob))
ds.wheelOnRightProb = float(sigmoid(model_result.wheel_on_right_prob))
ds.rawPredictions = model_output.tobytes() if SEND_RAW_PRED else b''
fill_driver_state(ds.leftDriverData, model_result.driver_state_lhd)
fill_driver_state(ds.rightDriverData, model_result.driver_state_rhd)
ds.modelExecutionTime = exec_time
ds.gpuExecutionTime = gpu_exec_time
ds.rawPredictions = model_output['raw_pred']
ds.wheelOnRightProb = model_output['wheel_on_right'][0, 0].item()
fill_driver_data(ds.leftDriverData, model_output, 'lhd')
fill_driver_data(ds.rightDriverData, model_output, 'rhd')
return msg
@@ -153,7 +127,7 @@ def main():
sm = SubMaster(["liveCalibration"])
pm = PubMaster(["driverStateV2"])
calib = np.zeros(CALIB_LEN, dtype=np.float32)
calib = np.zeros(model.numpy_inputs['calib'].size, dtype=np.float32)
model_transform = None
while True:
@@ -172,8 +146,12 @@ def main():
t1 = time.perf_counter()
model_output, gpu_execution_time = model.run(buf, calib, model_transform)
t2 = time.perf_counter()
pm.send("driverStateV2", get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, gpu_execution_time))
raw_pred = model_output.tobytes() if SEND_RAW_PRED else b''
model_output = slice_outputs(model_output, model.output_slices)
model_output = parse_model_output(model_output)
model_output['raw_pred'] = raw_pred
msg = get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, gpu_execution_time)
pm.send("driverStateV2", msg)
if __name__ == "__main__":
selfdrive/modeld/fill_model_msg.py
+28 -6
View File
@@ -1,6 +1,7 @@
import os
import capnp
import numpy as np
import math
from cereal import log
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
@@ -102,21 +103,42 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
LINE_T_IDXS = [np.nan] * ModelConstants.IDX_N
LINE_T_IDXS[0] = 0.0
plan_x = net_output_data['plan'][0, :, Plan.POSITION][:, 0].tolist()
Tmax = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1]
for xidx in range(1, ModelConstants.IDX_N):
tidx = 0
# increment tidx until we find an element that's further away than the current xidx
while tidx < ModelConstants.IDX_N - 1 and plan_x[tidx + 1] < ModelConstants.X_IDXS[xidx]:
tidx += 1
if tidx == ModelConstants.IDX_N - 1:
# if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break
LINE_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1]
break
for k in range(xidx, ModelConstants.IDX_N):
LINE_T_IDXS[k] = Tmax
break
# interpolate to find `t` for the current xidx
current_x_val = plan_x[tidx]
next_x_val = plan_x[tidx + 1]
p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(
next_x_val - current_x_val) > 1e-9 else float('nan')
LINE_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx + 1] + (1 - p) * ModelConstants.T_IDXS[tidx]
dx = next_x_val - current_x_val
if dx <= 1e-9:
LINE_T_IDXS[xidx] = ModelConstants.T_IDXS[tidx]
else:
p = (ModelConstants.X_IDXS[xidx] - current_x_val) / dx
if p < 0.0: p = 0.0
elif p > 1.0: p = 1.0
LINE_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx + 1] + (1.0 - p) * ModelConstants.T_IDXS[tidx]
#p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(
# next_x_val - current_x_val) > 1e-9 else float('nan')
#LINE_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx + 1] + (1 - p) * ModelConstants.T_IDXS[tidx]
LINE_T_IDXS = [float(Tmax if math.isnan(float(v)) else float(v)) for v in LINE_T_IDXS]
# 비내림(monotonic non-decreasing) 보정 (순수 파이썬, numpy 불사용)
running = LINE_T_IDXS[0]
for i in range(1, len(LINE_T_IDXS)):
if LINE_T_IDXS[i] < running:
LINE_T_IDXS[i] = running
else:
running = LINE_T_IDXS[i]
# lane lines
modelV2.init('laneLines', 4)
selfdrive/modeld/models/README.md
+1 -2
View File
@@ -62,6 +62,5 @@ Refer to **slice_outputs** and **parse_vision_outputs/parse_policy_outputs** in
* (deprecated) distracted probabilities: 2
* using phone probability: 1
* distracted probability: 1
* common outputs 2
* poor camera vision probability: 1
* common outputs 1
* left hand drive probability: 1
selfdrive/modeld/models/dmonitoring_model.current
-2
View File
@@ -1,2 +0,0 @@
fa69be01-b430-4504-9d72-7dcb058eb6dd
d9fb22d1c4fa3ca3d201dbc8edf1d0f0918e53e6
selfdrive/modeld/models/dmonitoring_model.onnx
BIN
View File
Binary file not shown.
selfdrive/modeld/models/driving_policy.onnx
BIN
View File
Binary file not shown.
selfdrive/modeld/models/driving_vision.onnx
BIN
View File
Binary file not shown.
selfdrive/ui/carrot.cc
+2 -3
View File
@@ -2400,9 +2400,8 @@ public:
if (strcmp(driving_mode_str, driving_mode_str_last)) ui_draw_text_a(s, dx, dy, driving_mode_str, 30, COLOR_WHITE, BOLD);
strcpy(driving_mode_str_last, driving_mode_str);
auto locationd = sm["liveLocationKalman"].getLiveLocationKalman();
bool is_gps_valid = sm.valid("liveLocationKalman") && locationd.getGpsOK();
if (is_gps_valid) {
auto gps = (s->ublox_avaliable) ? sm["gpsLocationExternal"].getGpsLocationExternal() : sm["gpsLocation"].getGpsLocation();
if (gps.getHasFix()) {
ui_draw_text(s, dx, dy - 45, "GPS", 30, COLOR_GREEN, BOLD);
}
selfdrive/ui/qt/maps/map.cc
+110 -5
View File
@@ -4,6 +4,9 @@
#include <eigen3/Eigen/Dense>
#include <QDebug>
#include <QJsonDocument>
#include <QJsonObject>
#include <QJsonArray>
#include "selfdrive/ui/qt/maps/map_helpers.h"
#include "selfdrive/ui/qt/util.h"
@@ -13,7 +16,7 @@
const int INTERACTION_TIMEOUT = 100;
//const float MAX_ZOOM = 20;// 17;
const float MIN_ZOOM = 14;
const float MIN_ZOOM = 15; // 14;
const float MAX_PITCH = 50;
const float MIN_PITCH = 0;
const float MAP_SCALE = 2;
@@ -147,6 +150,40 @@ void MapWindow::initLayers() {
20, 0
};
if (!m_map->sourceExists("carrotSpeedSource")) {
qDebug() << "Initializing carrotSpeedSource";
// 빈 FeatureCollection GeoJSON (QVariantMap 버전)
QVariantMap fc;
fc["type"] = "FeatureCollection";
fc["features"] = QVariantList{}; // 빈 리스트
QVariantMap src;
src["type"] = "geojson";
src["data"] = fc;
m_map->addSource("carrotSpeedSource", src);
}
if (!m_map->layerExists("carrotSpeedLayer")) {
qDebug() << "Initializing carrotSpeedLayer";
QVariantMap layer;
layer["type"] = "symbol";
layer["source"] = "carrotSpeedSource";
m_map->addLayer("carrotSpeedLayer", layer);
// properties.speed 를 텍스트로 표시 (토큰 방식)
// "{speed}" 라고 쓰면 properties.speed 값을 문자열로 넣어줌
m_map->setLayoutProperty("carrotSpeedLayer", "text-field", "{speed}");
m_map->setLayoutProperty("carrotSpeedLayer", "text-size", 16.0);
m_map->setLayoutProperty("carrotSpeedLayer", "text-offset", QVariantList{ 0.0, -1.5 });
m_map->setLayoutProperty("carrotSpeedLayer", "text-anchor", "top");
m_map->setLayoutProperty("carrotSpeedLayer", "icon-allow-overlap", true);
m_map->setPaintProperty("carrotSpeedLayer", "text-color", QColor("white"));
m_map->setPaintProperty("carrotSpeedLayer", "text-halo-color", QColor("black"));
m_map->setPaintProperty("carrotSpeedLayer", "text-halo-width", 1.0);
m_map->setLayoutProperty("carrotSpeedLayer", "text-allow-overlap", true);
}
m_map->setPaintProperty("buildingsLayer", "fill-extrusion-color", QColor("grey"));
m_map->setPaintProperty("buildingsLayer", "fill-extrusion-opacity", fillExtrusionOpacity);
m_map->setPaintProperty("buildingsLayer", "fill-extrusion-height", fillExtrusionHight);
@@ -228,6 +265,74 @@ void MapWindow::updateState(const UIState &s) {
initLayers();
{
std::string raw = params_memory.get("CarrotSpeedViz");
if (!raw.empty()) {
QString qraw = QString::fromStdString(raw);
//printf("%s\n", qraw.toStdString().c_str());
if (qraw != last_viz_raw) {
last_viz_raw = qraw;
QJsonParseError err;
QJsonDocument doc = QJsonDocument::fromJson(qraw.toUtf8(), &err);
if (err.error == QJsonParseError::NoError && doc.isObject()) {
QJsonObject obj = doc.object();
QJsonArray pts = obj["pts"].toArray();
// GeoJSON FeatureCollection → QVariantMap 트리로 만들기
QVariantList features; // Feature 리스트
for (const QJsonValue& v : pts) {
QJsonArray arr = v.toArray();
if (arr.size() < 3) continue;
double plat = arr[0].toDouble();
double plon = arr[1].toDouble();
double spd = arr[2].toDouble();
// geometry: Point (GeoJSON: [lon, lat] 순서)
QVariantList coords;
coords.append(plon);
coords.append(plat);
QVariantMap geom;
geom["type"] = "Point";
geom["coordinates"] = coords;
// properties: speed
QVariantMap props;
props["speed"] = static_cast<int>(std::round(spd));
// Feature
QVariantMap feature;
feature["type"] = "Feature";
feature["geometry"] = geom;
feature["properties"] = props;
features.append(feature);
}
QVariantMap fc;
fc["type"] = "FeatureCollection";
fc["features"] = features;
QJsonDocument fc_doc = QJsonDocument::fromVariant(fc);
QByteArray fc_bytes = fc_doc.toJson(QJsonDocument::Compact);
QVariantMap src;
src["type"] = "geojson";
src["data"] = fc_bytes;
m_map->updateSource("carrotSpeedSource", src);
m_map->setLayoutProperty("carrotSpeedLayer", "visibility", "visible");
}
}
}
else {
// 필요하면 숨기기
// m_map->setLayoutProperty("carrotSpeedLayer", "visibility", "none");
}
}
if (!locationd_valid) {
setError(tr("Waiting for GPS(APN)"));
} else if (routing_problem) {
@@ -295,17 +400,17 @@ void MapWindow::updateState(const UIState &s) {
updateDestinationMarker();
}
if (loaded_once && (sm.rcv_frame("modelV2") != model_rcv_frame)) {
auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman();
if (locationd_location.getGpsOK()) {
//auto carrot_man = sm["carrotMan"].getCarrotMan();
/*
gps = (ublox_avaliable)? sm[gps_service].getGpsLocationExternal() : sm[gps_service].getGpsLocation();
if (gps.getHasFix()) {
auto model_path = model_to_collection(locationd_location.getCalibratedOrientationECEF(), locationd_location.getPositionECEF(), sm["modelV2"].getModelV2().getPosition(), carrotMan.getXPosLat(), carrotMan.getXPosLon());
//auto model_path = model_to_collection(sm["modelV2"].getModelV2().getPosition(), carrotMan.getXPosLat(), carrotMan.getXPosLon(), carrotMan.getXPosAngle());
QMapLibre::Feature model_path_feature(QMapLibre::Feature::LineStringType, model_path, {}, {});
QVariantMap modelV2Path;
modelV2Path["type"] = "geojson";
modelV2Path["data"] = QVariant::fromValue<QMapLibre::Feature>(model_path_feature);
m_map->updateSource("modelPathSource", modelV2Path);
}
*/
model_rcv_frame = sm.rcv_frame("modelV2");
}
}
selfdrive/ui/qt/maps/map.h
+4 -1
View File
@@ -69,6 +69,7 @@ private:
MapInstructions* map_instructions;
MapETA* map_eta;
// Blue with normal nav, green when nav is input into the model
QColor getNavPathColor(bool nav_enabled) {
return nav_enabled ? QColor("#31ee73") : QColor("#31a1ee");
@@ -78,10 +79,12 @@ private:
void updateDestinationMarker();
uint64_t route_rcv_frame = 0;
// FrogPilot variables
Params params;
uint64_t model_rcv_frame = 0;
Params params_memory{ "/dev/shm/params" };
QString last_viz_raw;
float MAX_ZOOM = 17; // carrot
private slots:
void updateState(const UIState &s);
selfdrive/ui/qt/offroad/settings.cc
+15 -9
View File
@@ -215,19 +215,24 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
//QObject::connect(init_btn, &QPushButton::clicked, this, &DevicePanel::reboot);
QObject::connect(init_btn, &QPushButton::clicked, [&]() {
if (ConfirmationDialog::confirm(tr("Git pull & Reboot?"), tr("Yes"), this)) {
QString cmd =
"bash -c 'cd /data/openpilot && "
"git fetch && "
"if git status -uno | grep -q \"Your branch is behind\"; then "
"git pull && reboot; "
QString pullscript = "cd /data/openpilot && "
"git fetch origin && "
"LOCAL=$(git rev-parse HEAD) && "
"BRANCH=$(git branch --show-current) && "
"REMOTE=$(git rev-parse origin/$BRANCH) && "
"if [ $LOCAL != $REMOTE ]; then "
"echo 'Local is behind. Pulling updates...' && "
"git pull --ff-only && "
"sudo reboot; "
"else "
"echo \"Already up to date.\"; "
"echo 'Already up to date.'; "
"fi'";
if (!QProcess::startDetached(cmd)) {
bool success = QProcess::startDetached("/bin/sh", QStringList() << "-c" << pullscript);
if (!success) {
ConfirmationDialog::alert(tr("Failed to start update process."), this);
}
else {
} else {
ConfirmationDialog::alert(tr("Update process started. Device will reboot if updates are applied."), this);
}
}
@@ -858,6 +863,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
speedToggles->addItem(new CValueControl("AutoNaviSpeedBumpSpeed", tr("SpeedBumpSpeed(35Km/h)"), "", 10, 100, 5));
speedToggles->addItem(new CValueControl("AutoNaviCountDownMode", tr("NaviCountDown mode(2)"), tr("0: off, 1:tbt+camera, 2:tbt+camera+bump"), 0, 2, 1));
speedToggles->addItem(new CValueControl("TurnSpeedControlMode", tr("Turn Speed control mode(1)"), tr("0: off, 1:vision, 2:vision+route, 3: route"), 0, 3, 1));
speedToggles->addItem(new CValueControl("CarrotSmartSpeedControl", tr("Smart Speed Control(0)"), tr("0: off, 1:accel, 2:decel, 3: all"), 0, 3, 1));
speedToggles->addItem(new CValueControl("MapTurnSpeedFactor", tr("Map TurnSpeed Factor(100)"), "", 50, 300, 5));
speedToggles->addItem(new CValueControl("ModelTurnSpeedFactor", tr("Model TurnSpeed Factor(0)"), "", 0, 80, 10));
speedToggles->addItem(new CValueControl("AutoTurnControl", tr("ATC: Auto turn control(0)"), tr("0:None, 1: lane change, 2: lane change + speed, 3: speed"), 0, 3, 1));
selfdrive/ui/ui.cc
+3 -1
View File
@@ -98,13 +98,15 @@ void UIState::updateStatus() {
}
UIState::UIState(QObject *parent) : QObject(parent) {
ublox_avaliable = Params().getBool("UbloxAvailable");
auto gps_service = (ublox_avaliable) ? "gpsLocationExternal" : "gpsLocation";
sm = std::make_unique<SubMaster>(std::vector<const char*>{
"modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
"pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2",
"wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
"longitudinalPlan",
"carControl", "carrotMan", "liveTorqueParameters", "lateralPlan", "liveParameters",
"navRoute", "navInstruction", "navInstructionCarrot", "liveLocationKalman", "liveDelay",
"navRoute", "navInstruction", "navInstructionCarrot", gps_service, "liveDelay",
"peripheralState",
});
prime_state = new PrimeState(this);
selfdrive/ui/ui.h
+2
View File
@@ -98,6 +98,8 @@ public:
float show_brightness_ratio = 1.0;
int show_brightness_timer = 20;
bool ublox_avaliable = true;
signals:
void uiUpdate(const UIState &s);
void offroadTransition(bool offroad);
system/camerad/SConscript
+2 -2
View File
@@ -1,6 +1,6 @@
Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc')
Import('env', 'arch', 'messaging', 'common', 'visionipc')
libs = [common, 'OpenCL', messaging, visionipc, gpucommon]
libs = [common, 'OpenCL', messaging, visionipc]
if arch != "Darwin":
camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/spectra.cc',
system/manager/manager.py
+1
View File
@@ -86,6 +86,7 @@ def get_default_params():
("AutoRoadSpeedLimitOffset", "-1"),
("AutoNaviCountDownMode", "2"),
("TurnSpeedControlMode", "1"),
("CarrotSmartSpeedControl", "0"),
("MapTurnSpeedFactor", "90"),
("ModelTurnSpeedFactor", "0"),
("StoppingAccel", "0"),
system/manager/process_config.py
+4 -7
View File
@@ -93,17 +93,14 @@ procs = [
PythonProcess("micd", "system.micd", iscar),
PythonProcess("timed", "system.timed", always_run, enabled=not PC),
# TODO: Make python process once TG allows opening QCOM from child pro
# https://github.com/tinygrad/tinygrad/blob/ac9c96dae1656dc220ee4acc39cef4dd449aa850/tinygrad/device.py#L26
NativeProcess("modeld", "selfdrive/modeld", ["./modeld.py"], only_onroad),
NativeProcess("dmonitoringmodeld", "selfdrive/modeld", ["./dmonitoringmodeld.py"], enable_dm, enabled=(WEBCAM or not PC)),
PythonProcess("modeld", "selfdrive.modeld.modeld", only_onroad),
PythonProcess("dmonitoringmodeld", "selfdrive.modeld.dmonitoringmodeld", enable_dm, enabled=(WEBCAM or not PC)),
#NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"], only_onroad),
#NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"], always_run),
#PythonProcess("navmodeld", "selfdrive.modeld.navmodeld", only_onroad),
NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC),
PythonProcess("sensord", "system.sensord.sensord", only_onroad, enabled=not PC),
NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)),
PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad),
NativeProcess("locationd2", "selfdrive/locationd", ["./locationd"], only_onroad),
PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad),
NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False),
PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),
@@ -119,7 +116,7 @@ procs = [
PythonProcess("pandad", "selfdrive.pandad.pandad", always_run),
PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad),
PythonProcess("lagd", "selfdrive.locationd.lagd", only_onroad),
NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI),
PythonProcess("ubloxd", "system.ubloxd.ubloxd", ublox, enabled=TICI),
PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI),
PythonProcess("plannerd", "selfdrive.controls.plannerd", not_long_maneuver),
PythonProcess("maneuversd", "tools.longitudinal_maneuvers.maneuversd", long_maneuver),
system/qcomgpsd/qcomgpsd.py
+1 -1
View File
@@ -16,7 +16,7 @@ from struct import unpack_from, calcsize, pack
from cereal import log
import cereal.messaging as messaging
from openpilot.common.gpio import gpio_init, gpio_set
from openpilot.common.retry import retry
from openpilot.common.utils import retry
from openpilot.common.time_helpers import system_time_valid
from openpilot.system.hardware.tici.pins import GPIO
from openpilot.common.swaglog import cloudlog
system/sensord/.gitignore
-1
View File
@@ -1 +0,0 @@
sensord
system/sensord/SConscript
-17
View File
@@ -1,17 +0,0 @@
Import('env', 'arch', 'common', 'messaging')
sensors = [
'sensors/i2c_sensor.cc',
'sensors/bmx055_accel.cc',
'sensors/bmx055_gyro.cc',
'sensors/bmx055_magn.cc',
'sensors/bmx055_temp.cc',
'sensors/lsm6ds3_accel.cc',
'sensors/lsm6ds3_gyro.cc',
'sensors/lsm6ds3_temp.cc',
'sensors/mmc5603nj_magn.cc',
]
libs = [common, messaging, 'pthread']
if arch == "larch64":
libs.append('i2c')
env.Program('sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs)
system/sensord/sensord.py
+150
View File
@@ -0,0 +1,150 @@
#!/usr/bin/env python3
import os
import time
import ctypes
import select
import threading
import cereal.messaging as messaging
from cereal.services import SERVICE_LIST
from openpilot.common.util import sudo_write
from openpilot.common.realtime import config_realtime_process, Ratekeeper
from openpilot.common.swaglog import cloudlog
from openpilot.common.gpio import gpiochip_get_ro_value_fd, gpioevent_data
from openpilot.system.hardware import HARDWARE
from openpilot.system.sensord.sensors.i2c_sensor import Sensor
from openpilot.system.sensord.sensors.lsm6ds3_accel import LSM6DS3_Accel
from openpilot.system.sensord.sensors.lsm6ds3_gyro import LSM6DS3_Gyro
from openpilot.system.sensord.sensors.lsm6ds3_temp import LSM6DS3_Temp
from openpilot.system.sensord.sensors.mmc5603nj_magn import MMC5603NJ_Magn
I2C_BUS_IMU = 1
def interrupt_loop(sensors: list[tuple[Sensor, str, bool]], event) -> None:
pm = messaging.PubMaster([service for sensor, service, interrupt in sensors if interrupt])
# Requesting both edges as the data ready pulse from the lsm6ds sensor is
# very short (75us) and is mostly detected as falling edge instead of rising.
# So if it is detected as rising the following falling edge is skipped.
fd = gpiochip_get_ro_value_fd("sensord", 0, 84)
# Configure IRQ affinity
irq_path = "/proc/irq/336/smp_affinity_list"
if not os.path.exists(irq_path):
irq_path = "/proc/irq/335/smp_affinity_list"
if os.path.exists(irq_path):
sudo_write('1\n', irq_path)
offset = time.time_ns() - time.monotonic_ns()
poller = select.poll()
poller.register(fd, select.POLLIN | select.POLLPRI)
while not event.is_set():
events = poller.poll(100)
if not events:
cloudlog.error("poll timed out")
continue
if not (events[0][1] & (select.POLLIN | select.POLLPRI)):
cloudlog.error("no poll events set")
continue
dat = os.read(fd, ctypes.sizeof(gpioevent_data)*16)
evd = gpioevent_data.from_buffer_copy(dat)
cur_offset = time.time_ns() - time.monotonic_ns()
if abs(cur_offset - offset) > 10 * 1e6: # ms
cloudlog.warning(f"time jumped: {cur_offset} {offset}")
offset = cur_offset
continue
ts = evd.timestamp - cur_offset
for sensor, service, interrupt in sensors:
if interrupt:
try:
evt = sensor.get_event(ts)
if not sensor.is_data_valid():
continue
msg = messaging.new_message(service, valid=True)
setattr(msg, service, evt)
pm.send(service, msg)
except Sensor.DataNotReady:
pass
except Exception:
cloudlog.exception(f"Error processing {service}")
def polling_loop(sensor: Sensor, service: str, event: threading.Event) -> None:
pm = messaging.PubMaster([service])
rk = Ratekeeper(SERVICE_LIST[service].frequency, print_delay_threshold=None)
while not event.is_set():
try:
evt = sensor.get_event()
if not sensor.is_data_valid():
continue
msg = messaging.new_message(service, valid=True)
setattr(msg, service, evt)
pm.send(service, msg)
except Exception:
cloudlog.exception(f"Error in {service} polling loop")
rk.keep_time()
def main() -> None:
config_realtime_process([1, ], 1)
sensors_cfg = [
(LSM6DS3_Accel(I2C_BUS_IMU), "accelerometer", True),
(LSM6DS3_Gyro(I2C_BUS_IMU), "gyroscope", True),
(LSM6DS3_Temp(I2C_BUS_IMU), "temperatureSensor", False),
]
if HARDWARE.get_device_type() == "tizi":
sensors_cfg.append(
(MMC5603NJ_Magn(I2C_BUS_IMU), "magnetometer", False),
)
# Reset sensors
for sensor, _, _ in sensors_cfg:
try:
sensor.reset()
except Exception:
cloudlog.exception(f"Error initializing {sensor} sensor")
# Initialize sensors
exit_event = threading.Event()
threads = [
threading.Thread(target=interrupt_loop, args=(sensors_cfg, exit_event), daemon=True)
]
for sensor, service, interrupt in sensors_cfg:
try:
sensor.init()
if not interrupt:
# Start polling thread for sensors without interrupts
threads.append(threading.Thread(
target=polling_loop,
args=(sensor, service, exit_event),
daemon=True
))
except Exception:
cloudlog.exception(f"Error initializing {service} sensor")
try:
for t in threads:
t.start()
while any(t.is_alive() for t in threads):
time.sleep(1)
except KeyboardInterrupt:
pass
finally:
exit_event.set()
for t in threads:
if t.is_alive():
t.join()
for sensor, _, _ in sensors_cfg:
try:
sensor.shutdown()
except Exception:
cloudlog.exception("Error shutting down sensor")
if __name__ == "__main__":
main()
system/sensord/sensors/__init__.py
View File
system/sensord/sensors/bmx055_accel.cc
-85
View File
@@ -1,85 +0,0 @@
#include "system/sensord/sensors/bmx055_accel.h"
#include <cassert>
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Accel::init() {
int ret = verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID});
if (ret == -1) {
goto fail;
}
ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_NORMAL_MODE);
if (ret < 0) {
goto fail;
}
// bmx055 accel has a 1.3ms wakeup time from deep suspend mode
util::sleep_for(10);
// 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;
}
enabled = true;
fail:
return ret;
}
int BMX055_Accel::shutdown() {
if (!enabled) return 0;
// enter deep suspend mode (lowest power mode)
int ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_DEEP_SUSPEND);
if (ret < 0) {
LOGE("Could not move BMX055 ACCEL in deep suspend mode!");
}
return ret;
}
bool BMX055_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
uint8_t buffer[6];
int len = read_register(BMX055_ACCEL_I2C_REG_X_LSB, 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;
auto event = msg.initEvent().initAccelerometer2();
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};
auto svec = event.initAcceleration();
svec.setV(xyz);
svec.setStatus(true);
return true;
}
system/sensord/sensors/bmx055_accel.h
-41
View File
@@ -1,41 +0,0 @@
#pragma once
#include "system/sensord/sensors/i2c_sensor.h"
// 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_X_LSB 0x02
#define BMX055_ACCEL_I2C_REG_TEMP 0x08
#define BMX055_ACCEL_I2C_REG_BW 0x10
#define BMX055_ACCEL_I2C_REG_PMU 0x11
#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_DEEP_SUSPEND 0b00100000
#define BMX055_ACCEL_NORMAL_MODE 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();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};
system/sensord/sensors/bmx055_gyro.cc
-92
View File
@@ -1,92 +0,0 @@
#include "system/sensord/sensors/bmx055_gyro.h"
#include <cassert>
#include <cmath>
#include "common/swaglog.h"
#include "common/util.h"
#define DEG2RAD(x) ((x) * M_PI / 180.0)
BMX055_Gyro::BMX055_Gyro(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Gyro::init() {
int ret = verify_chip_id(BMX055_GYRO_I2C_REG_ID, {BMX055_GYRO_CHIP_ID});
if (ret == -1) return -1;
ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_NORMAL_MODE);
if (ret < 0) {
goto fail;
}
// bmx055 gyro has a 30ms wakeup time from deep suspend mode
util::sleep_for(50);
// 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;
}
enabled = true;
fail:
return ret;
}
int BMX055_Gyro::shutdown() {
if (!enabled) return 0;
// enter deep suspend mode (lowest power mode)
int ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_DEEP_SUSPEND);
if (ret < 0) {
LOGE("Could not move BMX055 GYRO in deep suspend mode!");
}
return ret;
}
bool BMX055_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
uint8_t buffer[6];
int len = read_register(BMX055_GYRO_I2C_REG_RATE_X_LSB, 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);
auto event = msg.initEvent().initGyroscope2();
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};
auto svec = event.initGyroUncalibrated();
svec.setV(xyz);
svec.setStatus(true);
return true;
}
system/sensord/sensors/bmx055_gyro.h
-41
View File
@@ -1,41 +0,0 @@
#pragma once
#include "system/sensord/sensors/i2c_sensor.h"
// 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_RATE_X_LSB 0x02
#define BMX055_GYRO_I2C_REG_RANGE 0x0F
#define BMX055_GYRO_I2C_REG_BW 0x10
#define BMX055_GYRO_I2C_REG_LPM1 0x11
#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_DEEP_SUSPEND 0b00100000
#define BMX055_GYRO_NORMAL_MODE 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();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};
system/sensord/sensors/bmx055_magn.cc
-258
View File
@@ -1,258 +0,0 @@
#include "system/sensord/sensors/bmx055_magn.h"
#include <unistd.h>
#include <algorithm>
#include <cassert>
#include <cstdio>
#include "common/swaglog.h"
#include "common/util.h"
static int16_t compensate_x(trim_data_t trim_data, int16_t mag_data_x, uint16_t data_rhall) {
uint16_t process_comp_x0 = data_rhall;
int32_t process_comp_x1 = ((int32_t)trim_data.dig_xyz1) * 16384;
uint16_t process_comp_x2 = ((uint16_t)(process_comp_x1 / process_comp_x0)) - ((uint16_t)0x4000);
int16_t retval = ((int16_t)process_comp_x2);
int32_t process_comp_x3 = (((int32_t)retval) * ((int32_t)retval));
int32_t process_comp_x4 = (((int32_t)trim_data.dig_xy2) * (process_comp_x3 / 128));
int32_t process_comp_x5 = (int32_t)(((int16_t)trim_data.dig_xy1) * 128);
int32_t process_comp_x6 = ((int32_t)retval) * process_comp_x5;
int32_t process_comp_x7 = (((process_comp_x4 + process_comp_x6) / 512) + ((int32_t)0x100000));
int32_t process_comp_x8 = ((int32_t)(((int16_t)trim_data.dig_x2) + ((int16_t)0xA0)));
int32_t process_comp_x9 = ((process_comp_x7 * process_comp_x8) / 4096);
int32_t process_comp_x10 = ((int32_t)mag_data_x) * process_comp_x9;
retval = ((int16_t)(process_comp_x10 / 8192));
retval = (retval + (((int16_t)trim_data.dig_x1) * 8)) / 16;
return retval;
}
static int16_t compensate_y(trim_data_t trim_data, int16_t mag_data_y, uint16_t data_rhall) {
uint16_t process_comp_y0 = trim_data.dig_xyz1;
int32_t process_comp_y1 = (((int32_t)trim_data.dig_xyz1) * 16384) / process_comp_y0;
uint16_t process_comp_y2 = ((uint16_t)process_comp_y1) - ((uint16_t)0x4000);
int16_t retval = ((int16_t)process_comp_y2);
int32_t process_comp_y3 = ((int32_t) retval) * ((int32_t)retval);
int32_t process_comp_y4 = ((int32_t)trim_data.dig_xy2) * (process_comp_y3 / 128);
int32_t process_comp_y5 = ((int32_t)(((int16_t)trim_data.dig_xy1) * 128));
int32_t process_comp_y6 = ((process_comp_y4 + (((int32_t)retval) * process_comp_y5)) / 512);
int32_t process_comp_y7 = ((int32_t)(((int16_t)trim_data.dig_y2) + ((int16_t)0xA0)));
int32_t process_comp_y8 = (((process_comp_y6 + ((int32_t)0x100000)) * process_comp_y7) / 4096);
int32_t process_comp_y9 = (((int32_t)mag_data_y) * process_comp_y8);
retval = (int16_t)(process_comp_y9 / 8192);
retval = (retval + (((int16_t)trim_data.dig_y1) * 8)) / 16;
return retval;
}
static int16_t compensate_z(trim_data_t trim_data, int16_t mag_data_z, uint16_t data_rhall) {
int16_t process_comp_z0 = ((int16_t)data_rhall) - ((int16_t) trim_data.dig_xyz1);
int32_t process_comp_z1 = (((int32_t)trim_data.dig_z3) * ((int32_t)(process_comp_z0))) / 4;
int32_t process_comp_z2 = (((int32_t)(mag_data_z - trim_data.dig_z4)) * 32768);
int32_t process_comp_z3 = ((int32_t)trim_data.dig_z1) * (((int16_t)data_rhall) * 2);
int16_t process_comp_z4 = (int16_t)((process_comp_z3 + (32768)) / 65536);
int32_t retval = ((process_comp_z2 - process_comp_z1) / (trim_data.dig_z2 + process_comp_z4));
/* saturate result to +/- 2 micro-tesla */
retval = std::clamp(retval, -32767, 32767);
/* Conversion of LSB to micro-tesla*/
retval = retval / 16;
return (int16_t)retval;
}
BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Magn::init() {
uint8_t trim_x1y1[2] = {0};
uint8_t trim_x2y2[2] = {0};
uint8_t trim_xy1xy2[2] = {0};
uint8_t trim_z1[2] = {0};
uint8_t trim_z2[2] = {0};
uint8_t trim_z3[2] = {0};
uint8_t trim_z4[2] = {0};
uint8_t trim_xyz1[2] = {0};
// suspend -> sleep
int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01);
if (ret < 0) {
LOGD("Enabling power failed: %d", ret);
goto fail;
}
util::sleep_for(5); // wait until the chip is powered on
ret = verify_chip_id(BMX055_MAGN_I2C_REG_ID, {BMX055_MAGN_CHIP_ID});
if (ret == -1) {
goto fail;
}
// Load magnetometer trim
ret = read_register(BMX055_MAGN_I2C_REG_DIG_X1, trim_x1y1, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_X2, trim_x2y2, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_XY2, trim_xy1xy2, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z1_LSB, trim_z1, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z2_LSB, trim_z2, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z3_LSB, trim_z3, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z4_LSB, trim_z4, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB, trim_xyz1, 2);
if (ret < 0) goto fail;
// Read trim data
trim_data.dig_x1 = trim_x1y1[0];
trim_data.dig_y1 = trim_x1y1[1];
trim_data.dig_x2 = trim_x2y2[0];
trim_data.dig_y2 = trim_x2y2[1];
trim_data.dig_xy1 = trim_xy1xy2[1]; // NB: MSB/LSB swapped
trim_data.dig_xy2 = trim_xy1xy2[0];
trim_data.dig_z1 = read_16_bit(trim_z1[0], trim_z1[1]);
trim_data.dig_z2 = read_16_bit(trim_z2[0], trim_z2[1]);
trim_data.dig_z3 = read_16_bit(trim_z3[0], trim_z3[1]);
trim_data.dig_z4 = read_16_bit(trim_z4[0], trim_z4[1]);
trim_data.dig_xyz1 = read_16_bit(trim_xyz1[0], trim_xyz1[1] & 0x7f);
assert(trim_data.dig_xyz1 != 0);
perform_self_test();
// f_max = 1 / (145us * nXY + 500us * NZ + 980us)
// Chose NXY = 7, NZ = 12, which gives 125 Hz,
// and has the same ratio as the high accuracy preset
ret = set_register(BMX055_MAGN_I2C_REG_REPXY, (7 - 1) / 2);
if (ret < 0) {
goto fail;
}
ret = set_register(BMX055_MAGN_I2C_REG_REPZ, 12 - 1);
if (ret < 0) {
goto fail;
}
enabled = true;
return 0;
fail:
return ret;
}
int BMX055_Magn::shutdown() {
if (!enabled) return 0;
// move to suspend mode
int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0);
if (ret < 0) {
LOGE("Could not move BMX055 MAGN in suspend mode!");
}
return ret;
}
bool BMX055_Magn::perform_self_test() {
uint8_t buffer[8];
int16_t x, y;
int16_t neg_z, pos_z;
// Increase z reps for less false positives (~30 Hz ODR)
set_register(BMX055_MAGN_I2C_REG_REPXY, 1);
set_register(BMX055_MAGN_I2C_REG_REPZ, 64 - 1);
// Clean existing measurement
read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
uint8_t forced = BMX055_MAGN_FORCED;
// Negative current
set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b10) << 6));
util::sleep_for(100);
read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
parse_xyz(buffer, &x, &y, &neg_z);
// Positive current
set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b11) << 6));
util::sleep_for(100);
read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
parse_xyz(buffer, &x, &y, &pos_z);
// Put back in normal mode
set_register(BMX055_MAGN_I2C_REG_MAG, 0);
int16_t diff = pos_z - neg_z;
bool passed = (diff > 180) && (diff < 240);
if (!passed) {
LOGE("self test failed: neg %d pos %d diff %d", neg_z, pos_z, diff);
}
return passed;
}
bool BMX055_Magn::parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z) {
bool ready = buffer[6] & 0x1;
if (ready) {
int16_t mdata_x = (int16_t) (((int16_t)buffer[1] << 8) | buffer[0]) >> 3;
int16_t mdata_y = (int16_t) (((int16_t)buffer[3] << 8) | buffer[2]) >> 3;
int16_t mdata_z = (int16_t) (((int16_t)buffer[5] << 8) | buffer[4]) >> 1;
uint16_t data_r = (uint16_t) (((uint16_t)buffer[7] << 8) | buffer[6]) >> 2;
assert(data_r != 0);
*x = compensate_x(trim_data, mdata_x, data_r);
*y = compensate_y(trim_data, mdata_y, data_r);
*z = compensate_z(trim_data, mdata_z, data_r);
}
return ready;
}
bool BMX055_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
uint8_t buffer[8];
int16_t _x, _y, x, y, z;
int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
bool parsed = parse_xyz(buffer, &_x, &_y, &z);
if (parsed) {
auto event = msg.initEvent().initMagnetometer();
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
event.setVersion(2);
event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
event.setTimestamp(start_time);
// Move magnetometer into same reference frame as accel/gryo
x = -_y;
y = _x;
// Axis convention
x = -x;
y = -y;
float xyz[] = {(float)x, (float)y, (float)z};
auto svec = event.initMagneticUncalibrated();
svec.setV(xyz);
svec.setStatus(true);
}
// The BMX055 Magnetometer has no FIFO mode. Self running mode only goes
// up to 30 Hz. Therefore we put in forced mode, and request measurements
// at a 100 Hz. When reading the registers we have to check the ready bit
// To verify the measurement was completed this cycle.
set_register(BMX055_MAGN_I2C_REG_MAG, BMX055_MAGN_FORCED);
return parsed;
}
system/sensord/sensors/bmx055_magn.h
-64
View File
@@ -1,64 +0,0 @@
#pragma once
#include <tuple>
#include "system/sensord/sensors/i2c_sensor.h"
// 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
#define BMX055_MAGN_I2C_REG_MAG 0x4C
#define BMX055_MAGN_I2C_REG_DATAX_LSB 0x42
#define BMX055_MAGN_I2C_REG_RHALL_LSB 0x48
#define BMX055_MAGN_I2C_REG_REPXY 0x51
#define BMX055_MAGN_I2C_REG_REPZ 0x52
#define BMX055_MAGN_I2C_REG_DIG_X1 0x5D
#define BMX055_MAGN_I2C_REG_DIG_Y1 0x5E
#define BMX055_MAGN_I2C_REG_DIG_Z4_LSB 0x62
#define BMX055_MAGN_I2C_REG_DIG_Z4_MSB 0x63
#define BMX055_MAGN_I2C_REG_DIG_X2 0x64
#define BMX055_MAGN_I2C_REG_DIG_Y2 0x65
#define BMX055_MAGN_I2C_REG_DIG_Z2_LSB 0x68
#define BMX055_MAGN_I2C_REG_DIG_Z2_MSB 0x69
#define BMX055_MAGN_I2C_REG_DIG_Z1_LSB 0x6A
#define BMX055_MAGN_I2C_REG_DIG_Z1_MSB 0x6B
#define BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB 0x6C
#define BMX055_MAGN_I2C_REG_DIG_XYZ1_MSB 0x6D
#define BMX055_MAGN_I2C_REG_DIG_Z3_LSB 0x6E
#define BMX055_MAGN_I2C_REG_DIG_Z3_MSB 0x6F
#define BMX055_MAGN_I2C_REG_DIG_XY2 0x70
#define BMX055_MAGN_I2C_REG_DIG_XY1 0x71
// Constants
#define BMX055_MAGN_CHIP_ID 0x32
#define BMX055_MAGN_FORCED (0b01 << 1)
struct trim_data_t {
int8_t dig_x1;
int8_t dig_y1;
int8_t dig_x2;
int8_t dig_y2;
uint16_t dig_z1;
int16_t dig_z2;
int16_t dig_z3;
int16_t dig_z4;
uint8_t dig_xy1;
int8_t dig_xy2;
uint16_t dig_xyz1;
};
class BMX055_Magn : public I2CSensor{
uint8_t get_device_address() {return BMX055_MAGN_I2C_ADDR;}
trim_data_t trim_data = {0};
bool perform_self_test();
bool parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z);
public:
BMX055_Magn(I2CBus *bus);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};
system/sensord/sensors/bmx055_temp.cc
-31
View File
@@ -1,31 +0,0 @@
#include "system/sensord/sensors/bmx055_temp.h"
#include <cassert>
#include "system/sensord/sensors/bmx055_accel.h"
#include "common/swaglog.h"
#include "common/timing.h"
BMX055_Temp::BMX055_Temp(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Temp::init() {
return verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}) == -1 ? -1 : 0;
}
bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
uint8_t buffer[1];
int len = read_register(BMX055_ACCEL_I2C_REG_TEMP, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float temp = 23.0f + int8_t(buffer[0]) / 2.0f;
auto event = msg.initEvent().initTemperatureSensor();
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
event.setVersion(1);
event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
event.setTimestamp(start_time);
event.setTemperature(temp);
return true;
}
system/sensord/sensors/bmx055_temp.h
-13
View File
@@ -1,13 +0,0 @@
#pragma once
#include "system/sensord/sensors/bmx055_accel.h"
#include "system/sensord/sensors/i2c_sensor.h"
class BMX055_Temp : public I2CSensor {
uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;}
public:
BMX055_Temp(I2CBus *bus);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown() { return 0; }
};
system/sensord/sensors/constants.h
-18
View File
@@ -1,18 +0,0 @@
#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_AMBIENT_TEMPERATURE 13
#define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED 14
#define SENSOR_TYPE_MAGNETIC_FIELD SENSOR_TYPE_GEOMAGNETIC_FIELD
#define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED 16
system/sensord/sensors/i2c_sensor.cc
-50
View File
@@ -1,50 +0,0 @@
#include "system/sensord/sensors/i2c_sensor.h"
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);
}
int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0) {
uint32_t combined = (uint32_t(b0) << 16) | (uint32_t(b1) << 8) | uint32_t(b2);
return int32_t(combined) / (1 << 4);
}
I2CSensor::I2CSensor(I2CBus *bus, int gpio_nr, bool shared_gpio) :
bus(bus), gpio_nr(gpio_nr), shared_gpio(shared_gpio) {}
I2CSensor::~I2CSensor() {
if (gpio_fd != -1) {
close(gpio_fd);
}
}
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);
}
int I2CSensor::init_gpio() {
if (shared_gpio || gpio_nr == 0) {
return 0;
}
gpio_fd = gpiochip_get_ro_value_fd("sensord", GPIOCHIP_INT, gpio_nr);
if (gpio_fd < 0) {
return -1;
}
return 0;
}
bool I2CSensor::has_interrupt_enabled() {
return gpio_nr != 0;
}
system/sensord/sensors/i2c_sensor.h
-51
View File
@@ -1,51 +0,0 @@
#pragma once
#include <cstdint>
#include <unistd.h>
#include <vector>
#include "cereal/gen/cpp/log.capnp.h"
#include "common/i2c.h"
#include "common/gpio.h"
#include "common/swaglog.h"
#include "system/sensord/sensors/constants.h"
#include "system/sensord/sensors/sensor.h"
int16_t read_12_bit(uint8_t lsb, uint8_t msb);
int16_t read_16_bit(uint8_t lsb, uint8_t msb);
int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0);
class I2CSensor : public Sensor {
private:
I2CBus *bus;
int gpio_nr;
bool shared_gpio;
virtual uint8_t get_device_address() = 0;
public:
I2CSensor(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
~I2CSensor();
int read_register(uint register_address, uint8_t *buffer, uint8_t len);
int set_register(uint register_address, uint8_t data);
int init_gpio();
bool has_interrupt_enabled();
virtual int init() = 0;
virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
virtual int shutdown() = 0;
int verify_chip_id(uint8_t address, const std::vector<uint8_t> &expected_ids) {
uint8_t chip_id = 0;
int ret = read_register(address, &chip_id, 1);
if (ret < 0) {
LOGD("Reading chip ID failed: %d", ret);
return -1;
}
for (int i = 0; i < expected_ids.size(); ++i) {
if (chip_id == expected_ids[i]) return chip_id;
}
LOGE("Chip ID wrong. Got: %d, Expected %d", chip_id, expected_ids[0]);
return -1;
}
};
system/sensord/sensors/i2c_sensor.py
+77
View File
@@ -0,0 +1,77 @@
import time
import smbus2
import ctypes
from collections.abc import Iterable
from cereal import log
class Sensor:
class SensorException(Exception):
pass
class DataNotReady(SensorException):
pass
def __init__(self, bus: int) -> None:
self.bus = smbus2.SMBus(bus)
self.source = log.SensorEventData.SensorSource.velodyne # unknown
self.start_ts = 0.
def __del__(self):
self.bus.close()
def read(self, addr: int, length: int) -> bytes:
return bytes(self.bus.read_i2c_block_data(self.device_address, addr, length))
def write(self, addr: int, data: int) -> None:
self.bus.write_byte_data(self.device_address, addr, data)
def writes(self, writes: Iterable[tuple[int, int]]) -> None:
for addr, data in writes:
self.write(addr, data)
def verify_chip_id(self, address: int, expected_ids: list[int]) -> int:
chip_id = self.read(address, 1)[0]
assert chip_id in expected_ids
return chip_id
# Abstract methods that must be implemented by subclasses
@property
def device_address(self) -> int:
raise NotImplementedError
def reset(self) -> None:
# optional.
# not part of init due to shared registers
pass
def init(self) -> None:
raise NotImplementedError
def get_event(self, ts: int | None = None) -> log.SensorEventData:
raise NotImplementedError
def shutdown(self) -> None:
raise NotImplementedError
def is_data_valid(self) -> bool:
if self.start_ts == 0:
self.start_ts = time.monotonic()
# unclear whether we need this...
return (time.monotonic() - self.start_ts) > 0.5
# *** helpers ***
@staticmethod
def wait():
# a standard small sleep
time.sleep(0.005)
@staticmethod
def parse_16bit(lsb: int, msb: int) -> int:
return ctypes.c_int16((msb << 8) | lsb).value
@staticmethod
def parse_20bit(b2: int, b1: int, b0: int) -> int:
combined = ctypes.c_uint32((b0 << 16) | (b1 << 8) | b2).value
return ctypes.c_int32(combined).value // (1 << 4)
system/sensord/sensors/lsm6ds3_accel.cc
-250
View File
@@ -1,250 +0,0 @@
#include "system/sensord/sensors/lsm6ds3_accel.h"
#include <cassert>
#include <cmath>
#include <cstring>
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) :
I2CSensor(bus, gpio_nr, shared_gpio) {}
void LSM6DS3_Accel::wait_for_data_ready() {
uint8_t drdy = 0;
uint8_t buffer[6];
do {
read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
drdy &= LSM6DS3_ACCEL_DRDY_XLDA;
} while (drdy == 0);
read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
}
void LSM6DS3_Accel::read_and_avg_data(float* out_buf) {
uint8_t drdy = 0;
uint8_t buffer[6];
float scaling = 0.061f;
if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) {
scaling = 0.122f;
}
for (int i = 0; i < 5; i++) {
do {
read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
drdy &= LSM6DS3_ACCEL_DRDY_XLDA;
} while (drdy == 0);
int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
for (int j = 0; j < 3; j++) {
out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * scaling;
}
}
for (int i = 0; i < 3; i++) {
out_buf[i] /= 5.0f;
}
}
int LSM6DS3_Accel::self_test(int test_type) {
float val_st_off[3] = {0};
float val_st_on[3] = {0};
float test_val[3] = {0};
uint8_t ODR_FS_MO = LSM6DS3_ACCEL_ODR_52HZ; // full scale: +-2g, ODR: 52Hz
// prepare sensor for self-test
// enable block data update and automatic increment
int ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC_BDU);
if (ret < 0) {
return ret;
}
if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) {
ODR_FS_MO = LSM6DS3_ACCEL_FS_4G | LSM6DS3_ACCEL_ODR_52HZ;
}
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, ODR_FS_MO);
if (ret < 0) {
return ret;
}
// wait for stable output, and discard first values
util::sleep_for(100);
wait_for_data_ready();
read_and_avg_data(val_st_off);
// enable Self Test positive (or negative)
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type);
if (ret < 0) {
return ret;
}
// wait for stable output, and discard first values
util::sleep_for(100);
wait_for_data_ready();
read_and_avg_data(val_st_on);
// disable sensor
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0);
if (ret < 0) {
return ret;
}
// disable self test
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0);
if (ret < 0) {
return ret;
}
// calculate the mg values for self test
for (int i = 0; i < 3; i++) {
test_val[i] = fabs(val_st_on[i] - val_st_off[i]);
}
// verify test result
for (int i = 0; i < 3; i++) {
if ((LSM6DS3_ACCEL_MIN_ST_LIMIT_mg > test_val[i]) ||
(test_val[i] > LSM6DS3_ACCEL_MAX_ST_LIMIT_mg)) {
return -1;
}
}
return ret;
}
int LSM6DS3_Accel::init() {
uint8_t value = 0;
bool do_self_test = false;
const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST");
if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) {
do_self_test = true;
}
int ret = verify_chip_id(LSM6DS3_ACCEL_I2C_REG_ID, {LSM6DS3_ACCEL_CHIP_ID, LSM6DS3TRC_ACCEL_CHIP_ID});
if (ret == -1) return -1;
if (ret == LSM6DS3TRC_ACCEL_CHIP_ID) {
source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
}
ret = self_test(LSM6DS3_ACCEL_POSITIVE_TEST);
if (ret < 0) {
LOGE("LSM6DS3 accel positive self-test failed!");
if (do_self_test) goto fail;
}
ret = self_test(LSM6DS3_ACCEL_NEGATIVE_TEST);
if (ret < 0) {
LOGE("LSM6DS3 accel negative self-test failed!");
if (do_self_test) goto fail;
}
ret = init_gpio();
if (ret < 0) {
goto fail;
}
// enable continuous update, and automatic increase
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC);
if (ret < 0) {
goto fail;
}
// TODO: set scale and bandwidth. Default is +- 2G, 50 Hz
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, LSM6DS3_ACCEL_ODR_104HZ);
if (ret < 0) {
goto fail;
}
ret = set_register(LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, LSM6DS3_ACCEL_DRDY_PULSE_MODE);
if (ret < 0) {
goto fail;
}
// enable data ready interrupt for accel on INT1
// (without resetting existing interrupts)
ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1);
if (ret < 0) {
goto fail;
}
value |= LSM6DS3_ACCEL_INT1_DRDY_XL;
ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value);
fail:
return ret;
}
int LSM6DS3_Accel::shutdown() {
int ret = 0;
// disable data ready interrupt for accel on INT1
uint8_t value = 0;
ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1);
if (ret < 0) {
goto fail;
}
value &= ~(LSM6DS3_ACCEL_INT1_DRDY_XL);
ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value);
if (ret < 0) {
LOGE("Could not disable lsm6ds3 acceleration interrupt!");
goto fail;
}
// enable power-down mode
value = 0;
ret = read_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, &value, 1);
if (ret < 0) {
goto fail;
}
value &= 0x0F;
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value);
if (ret < 0) {
LOGE("Could not power-down lsm6ds3 accelerometer!");
goto fail;
}
fail:
return ret;
}
bool LSM6DS3_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
// INT1 shared with gyro, check STATUS_REG who triggered
uint8_t status_reg = 0;
read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
if ((status_reg & LSM6DS3_ACCEL_DRDY_XLDA) == 0) {
return false;
}
uint8_t buffer[6];
int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float scale = 9.81 * 2.0f / (1 << 15);
float x = read_16_bit(buffer[0], buffer[1]) * scale;
float y = read_16_bit(buffer[2], buffer[3]) * scale;
float z = read_16_bit(buffer[4], buffer[5]) * scale;
auto event = msg.initEvent().initAccelerometer();
event.setSource(source);
event.setVersion(1);
event.setSensor(SENSOR_ACCELEROMETER);
event.setType(SENSOR_TYPE_ACCELEROMETER);
event.setTimestamp(ts);
float xyz[] = {y, -x, z};
auto svec = event.initAcceleration();
svec.setV(xyz);
svec.setStatus(true);
return true;
}
system/sensord/sensors/lsm6ds3_accel.h
-49
View File
@@ -1,49 +0,0 @@
#pragma once
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define LSM6DS3_ACCEL_I2C_ADDR 0x6A
// Registers of the chip
#define LSM6DS3_ACCEL_I2C_REG_DRDY_CFG 0x0B
#define LSM6DS3_ACCEL_I2C_REG_ID 0x0F
#define LSM6DS3_ACCEL_I2C_REG_INT1_CTRL 0x0D
#define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL 0x10
#define LSM6DS3_ACCEL_I2C_REG_CTRL3_C 0x12
#define LSM6DS3_ACCEL_I2C_REG_CTRL5_C 0x14
#define LSM6DS3_ACCEL_I2C_REG_CTR9_XL 0x18
#define LSM6DS3_ACCEL_I2C_REG_STAT_REG 0x1E
#define LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL 0x28
// Constants
#define LSM6DS3_ACCEL_CHIP_ID 0x69
#define LSM6DS3TRC_ACCEL_CHIP_ID 0x6A
#define LSM6DS3_ACCEL_FS_4G (0b10 << 2)
#define LSM6DS3_ACCEL_ODR_52HZ (0b0011 << 4)
#define LSM6DS3_ACCEL_ODR_104HZ (0b0100 << 4)
#define LSM6DS3_ACCEL_INT1_DRDY_XL 0b1
#define LSM6DS3_ACCEL_DRDY_XLDA 0b1
#define LSM6DS3_ACCEL_DRDY_PULSE_MODE (1 << 7)
#define LSM6DS3_ACCEL_IF_INC 0b00000100
#define LSM6DS3_ACCEL_IF_INC_BDU 0b01000100
#define LSM6DS3_ACCEL_XYZ_DEN 0b11100000
#define LSM6DS3_ACCEL_POSITIVE_TEST 0b01
#define LSM6DS3_ACCEL_NEGATIVE_TEST 0b10
#define LSM6DS3_ACCEL_MIN_ST_LIMIT_mg 90.0f
#define LSM6DS3_ACCEL_MAX_ST_LIMIT_mg 1700.0f
class LSM6DS3_Accel : public I2CSensor {
uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;}
cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
// self test functions
int self_test(int test_type);
void wait_for_data_ready();
void read_and_avg_data(float* val_st_off);
public:
LSM6DS3_Accel(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};
system/sensord/sensors/lsm6ds3_accel.py
+161
View File
@@ -0,0 +1,161 @@
import os
import time
from cereal import log
from openpilot.system.sensord.sensors.i2c_sensor import Sensor
class LSM6DS3_Accel(Sensor):
LSM6DS3_ACCEL_I2C_REG_DRDY_CFG = 0x0B
LSM6DS3_ACCEL_I2C_REG_INT1_CTRL = 0x0D
LSM6DS3_ACCEL_I2C_REG_CTRL1_XL = 0x10
LSM6DS3_ACCEL_I2C_REG_CTRL3_C = 0x12
LSM6DS3_ACCEL_I2C_REG_CTRL5_C = 0x14
LSM6DS3_ACCEL_I2C_REG_STAT_REG = 0x1E
LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL = 0x28
LSM6DS3_ACCEL_ODR_104HZ = (0b0100 << 4)
LSM6DS3_ACCEL_INT1_DRDY_XL = 0b1
LSM6DS3_ACCEL_DRDY_XLDA = 0b1
LSM6DS3_ACCEL_DRDY_PULSE_MODE = (1 << 7)
LSM6DS3_ACCEL_IF_INC = 0b00000100
LSM6DS3_ACCEL_ODR_52HZ = (0b0011 << 4)
LSM6DS3_ACCEL_FS_4G = (0b10 << 2)
LSM6DS3_ACCEL_IF_INC_BDU = 0b01000100
LSM6DS3_ACCEL_POSITIVE_TEST = 0b01
LSM6DS3_ACCEL_NEGATIVE_TEST = 0b10
LSM6DS3_ACCEL_MIN_ST_LIMIT_mg = 90.0
LSM6DS3_ACCEL_MAX_ST_LIMIT_mg = 1700.0
@property
def device_address(self) -> int:
return 0x6A
def reset(self):
self.write(0x12, 0x1)
time.sleep(0.1)
def init(self):
chip_id = self.verify_chip_id(0x0F, [0x69, 0x6A])
if chip_id == 0x6A:
self.source = log.SensorEventData.SensorSource.lsm6ds3trc
else:
self.source = log.SensorEventData.SensorSource.lsm6ds3
# self-test
if os.getenv("LSM_SELF_TEST") == "1":
self.self_test(self.LSM6DS3_ACCEL_POSITIVE_TEST)
self.self_test(self.LSM6DS3_ACCEL_NEGATIVE_TEST)
# actual init
int1 = self.read(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, 1)[0]
int1 |= self.LSM6DS3_ACCEL_INT1_DRDY_XL
self.writes((
# Enable continuous update and automatic address increment
(self.LSM6DS3_ACCEL_I2C_REG_CTRL3_C, self.LSM6DS3_ACCEL_IF_INC),
# Set ODR to 104 Hz, FS to ±2g (default)
(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, self.LSM6DS3_ACCEL_ODR_104HZ),
# Configure data ready signal to pulse mode
(self.LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, self.LSM6DS3_ACCEL_DRDY_PULSE_MODE),
# Enable data ready interrupt on INT1 without resetting existing interrupts
(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, int1),
))
def get_event(self, ts: int | None = None) -> log.SensorEventData:
assert ts is not None # must come from the IRQ event
# Check if data is ready since IRQ is shared with gyro
status_reg = self.read(self.LSM6DS3_ACCEL_I2C_REG_STAT_REG, 1)[0]
if (status_reg & self.LSM6DS3_ACCEL_DRDY_XLDA) == 0:
raise self.DataNotReady
scale = 9.81 * 2.0 / (1 << 15)
b = self.read(self.LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, 6)
x = self.parse_16bit(b[0], b[1]) * scale
y = self.parse_16bit(b[2], b[3]) * scale
z = self.parse_16bit(b[4], b[5]) * scale
event = log.SensorEventData.new_message()
event.timestamp = ts
event.version = 1
event.sensor = 1 # SENSOR_ACCELEROMETER
event.type = 1 # SENSOR_TYPE_ACCELEROMETER
event.source = self.source
a = event.init('acceleration')
a.v = [y, -x, z]
a.status = 1
return event
def shutdown(self) -> None:
# Disable data ready interrupt on INT1
value = self.read(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, 1)[0]
value &= ~self.LSM6DS3_ACCEL_INT1_DRDY_XL
self.write(self.LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value)
# Power down by clearing ODR bits
value = self.read(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 1)[0]
value &= 0x0F
self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value)
# *** self-test stuff ***
def _wait_for_data_ready(self):
while True:
drdy = self.read(self.LSM6DS3_ACCEL_I2C_REG_STAT_REG, 1)[0]
if drdy & self.LSM6DS3_ACCEL_DRDY_XLDA:
break
def _read_and_avg_data(self, scaling: float) -> list[float]:
out_buf = [0.0, 0.0, 0.0]
for _ in range(5):
self._wait_for_data_ready()
b = self.read(self.LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, 6)
for j in range(3):
val = self.parse_16bit(b[j*2], b[j*2+1]) * scaling
out_buf[j] += val
return [x / 5.0 for x in out_buf]
def self_test(self, test_type: int) -> None:
# Prepare sensor for self-test
self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL3_C, self.LSM6DS3_ACCEL_IF_INC_BDU)
# Configure ODR and full scale based on sensor type
if self.source == log.SensorEventData.SensorSource.lsm6ds3trc:
odr_fs = self.LSM6DS3_ACCEL_FS_4G | self.LSM6DS3_ACCEL_ODR_52HZ
scaling = 0.122 # mg/LSB for ±4g
else:
odr_fs = self.LSM6DS3_ACCEL_ODR_52HZ
scaling = 0.061 # mg/LSB for ±2g
self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, odr_fs)
# Wait for stable output
time.sleep(0.1)
self._wait_for_data_ready()
val_st_off = self._read_and_avg_data(scaling)
# Enable self-test
self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type)
# Wait for stable output
time.sleep(0.1)
self._wait_for_data_ready()
val_st_on = self._read_and_avg_data(scaling)
# Disable sensor and self-test
self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0)
self.write(self.LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0)
# Calculate differences and check limits
test_val = [abs(on - off) for on, off in zip(val_st_on, val_st_off, strict=False)]
for val in test_val:
if val < self.LSM6DS3_ACCEL_MIN_ST_LIMIT_mg or val > self.LSM6DS3_ACCEL_MAX_ST_LIMIT_mg:
raise self.SensorException(f"Accelerometer self-test failed for test type {test_type}")
if __name__ == "__main__":
import numpy as np
s = LSM6DS3_Accel(1)
s.init()
time.sleep(0.2)
e = s.get_event(0)
print(e)
print(np.linalg.norm(e.acceleration.v))
s.shutdown()
system/sensord/sensors/lsm6ds3_gyro.cc
-233
View File
@@ -1,233 +0,0 @@
#include "system/sensord/sensors/lsm6ds3_gyro.h"
#include <cassert>
#include <cmath>
#include <cstring>
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
#define DEG2RAD(x) ((x) * M_PI / 180.0)
LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus, int gpio_nr, bool shared_gpio) :
I2CSensor(bus, gpio_nr, shared_gpio) {}
void LSM6DS3_Gyro::wait_for_data_ready() {
uint8_t drdy = 0;
uint8_t buffer[6];
do {
read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
drdy &= LSM6DS3_GYRO_DRDY_GDA;
} while (drdy == 0);
read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
}
void LSM6DS3_Gyro::read_and_avg_data(float* out_buf) {
uint8_t drdy = 0;
uint8_t buffer[6];
for (int i = 0; i < 5; i++) {
do {
read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
drdy &= LSM6DS3_GYRO_DRDY_GDA;
} while (drdy == 0);
int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
for (int j = 0; j < 3; j++) {
out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * 70.0f;
}
}
// calculate the mg average values
for (int i = 0; i < 3; i++) {
out_buf[i] /= 5.0f;
}
}
int LSM6DS3_Gyro::self_test(int test_type) {
float val_st_off[3] = {0};
float val_st_on[3] = {0};
float test_val[3] = {0};
// prepare sensor for self-test
// full scale: 2000dps, ODR: 208Hz
int ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_208HZ | LSM6DS3_GYRO_FS_2000dps);
if (ret < 0) {
return ret;
}
// wait for stable output, and discard first values
util::sleep_for(150);
wait_for_data_ready();
read_and_avg_data(val_st_off);
// enable Self Test positive (or negative)
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type);
if (ret < 0) {
return ret;
}
// wait for stable output, and discard first values
util::sleep_for(50);
wait_for_data_ready();
read_and_avg_data(val_st_on);
// disable sensor
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0);
if (ret < 0) {
return ret;
}
// disable self test
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0);
if (ret < 0) {
return ret;
}
// calculate the mg values for self test
for (int i = 0; i < 3; i++) {
test_val[i] = fabs(val_st_on[i] - val_st_off[i]);
}
// verify test result
for (int i = 0; i < 3; i++) {
if ((LSM6DS3_GYRO_MIN_ST_LIMIT_mdps > test_val[i]) ||
(test_val[i] > LSM6DS3_GYRO_MAX_ST_LIMIT_mdps)) {
return -1;
}
}
return ret;
}
int LSM6DS3_Gyro::init() {
uint8_t value = 0;
bool do_self_test = false;
const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST");
if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) {
do_self_test = true;
}
int ret = verify_chip_id(LSM6DS3_GYRO_I2C_REG_ID, {LSM6DS3_GYRO_CHIP_ID, LSM6DS3TRC_GYRO_CHIP_ID});
if (ret == -1) return -1;
if (ret == LSM6DS3TRC_GYRO_CHIP_ID) {
source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
}
ret = init_gpio();
if (ret < 0) {
goto fail;
}
ret = self_test(LSM6DS3_GYRO_POSITIVE_TEST);
if (ret < 0) {
LOGE("LSM6DS3 gyro positive self-test failed!");
if (do_self_test) goto fail;
}
ret = self_test(LSM6DS3_GYRO_NEGATIVE_TEST);
if (ret < 0) {
LOGE("LSM6DS3 gyro negative self-test failed!");
if (do_self_test) goto fail;
}
// TODO: set scale. Default is +- 250 deg/s
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_104HZ);
if (ret < 0) {
goto fail;
}
ret = set_register(LSM6DS3_GYRO_I2C_REG_DRDY_CFG, LSM6DS3_GYRO_DRDY_PULSE_MODE);
if (ret < 0) {
goto fail;
}
// enable data ready interrupt for gyro on INT1
// (without resetting existing interrupts)
ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1);
if (ret < 0) {
goto fail;
}
value |= LSM6DS3_GYRO_INT1_DRDY_G;
ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value);
fail:
return ret;
}
int LSM6DS3_Gyro::shutdown() {
int ret = 0;
// disable data ready interrupt for gyro on INT1
uint8_t value = 0;
ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1);
if (ret < 0) {
goto fail;
}
value &= ~(LSM6DS3_GYRO_INT1_DRDY_G);
ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value);
if (ret < 0) {
LOGE("Could not disable lsm6ds3 gyroscope interrupt!");
goto fail;
}
// enable power-down mode
value = 0;
ret = read_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, &value, 1);
if (ret < 0) {
goto fail;
}
value &= 0x0F;
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, value);
if (ret < 0) {
LOGE("Could not power-down lsm6ds3 gyroscope!");
goto fail;
}
fail:
return ret;
}
bool LSM6DS3_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
// INT1 shared with accel, check STATUS_REG who triggered
uint8_t status_reg = 0;
read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
if ((status_reg & LSM6DS3_GYRO_DRDY_GDA) == 0) {
return false;
}
uint8_t buffer[6];
int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float scale = 8.75 / 1000.0;
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);
auto event = msg.initEvent().initGyroscope();
event.setSource(source);
event.setVersion(2);
event.setSensor(SENSOR_GYRO_UNCALIBRATED);
event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
event.setTimestamp(ts);
float xyz[] = {y, -x, z};
auto svec = event.initGyroUncalibrated();
svec.setV(xyz);
svec.setStatus(true);
return true;
}
system/sensord/sensors/lsm6ds3_gyro.h
-45
View File
@@ -1,45 +0,0 @@
#pragma once
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define LSM6DS3_GYRO_I2C_ADDR 0x6A
// Registers of the chip
#define LSM6DS3_GYRO_I2C_REG_DRDY_CFG 0x0B
#define LSM6DS3_GYRO_I2C_REG_ID 0x0F
#define LSM6DS3_GYRO_I2C_REG_INT1_CTRL 0x0D
#define LSM6DS3_GYRO_I2C_REG_CTRL2_G 0x11
#define LSM6DS3_GYRO_I2C_REG_CTRL5_C 0x14
#define LSM6DS3_GYRO_I2C_REG_STAT_REG 0x1E
#define LSM6DS3_GYRO_I2C_REG_OUTX_L_G 0x22
#define LSM6DS3_GYRO_POSITIVE_TEST (0b01 << 2)
#define LSM6DS3_GYRO_NEGATIVE_TEST (0b11 << 2)
// Constants
#define LSM6DS3_GYRO_CHIP_ID 0x69
#define LSM6DS3TRC_GYRO_CHIP_ID 0x6A
#define LSM6DS3_GYRO_FS_2000dps (0b11 << 2)
#define LSM6DS3_GYRO_ODR_104HZ (0b0100 << 4)
#define LSM6DS3_GYRO_ODR_208HZ (0b0101 << 4)
#define LSM6DS3_GYRO_INT1_DRDY_G 0b10
#define LSM6DS3_GYRO_DRDY_GDA 0b10
#define LSM6DS3_GYRO_DRDY_PULSE_MODE (1 << 7)
#define LSM6DS3_GYRO_MIN_ST_LIMIT_mdps 150000.0f
#define LSM6DS3_GYRO_MAX_ST_LIMIT_mdps 700000.0f
class LSM6DS3_Gyro : public I2CSensor {
uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;}
cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
// self test functions
int self_test(int test_type);
void wait_for_data_ready();
void read_and_avg_data(float* val_st_off);
public:
LSM6DS3_Gyro(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};
system/sensord/sensors/lsm6ds3_gyro.py
+145
View File
@@ -0,0 +1,145 @@
import os
import math
import time
from cereal import log
from openpilot.system.sensord.sensors.i2c_sensor import Sensor
class LSM6DS3_Gyro(Sensor):
LSM6DS3_GYRO_I2C_REG_DRDY_CFG = 0x0B
LSM6DS3_GYRO_I2C_REG_INT1_CTRL = 0x0D
LSM6DS3_GYRO_I2C_REG_CTRL2_G = 0x11
LSM6DS3_GYRO_I2C_REG_CTRL5_C = 0x14
LSM6DS3_GYRO_I2C_REG_STAT_REG = 0x1E
LSM6DS3_GYRO_I2C_REG_OUTX_L_G = 0x22
LSM6DS3_GYRO_ODR_104HZ = (0b0100 << 4)
LSM6DS3_GYRO_INT1_DRDY_G = 0b10
LSM6DS3_GYRO_DRDY_GDA = 0b10
LSM6DS3_GYRO_DRDY_PULSE_MODE = (1 << 7)
LSM6DS3_GYRO_ODR_208HZ = (0b0101 << 4)
LSM6DS3_GYRO_FS_2000dps = (0b11 << 2)
LSM6DS3_GYRO_POSITIVE_TEST = (0b01 << 2)
LSM6DS3_GYRO_NEGATIVE_TEST = (0b11 << 2)
LSM6DS3_GYRO_MIN_ST_LIMIT_mdps = 150000.0
LSM6DS3_GYRO_MAX_ST_LIMIT_mdps = 700000.0
@property
def device_address(self) -> int:
return 0x6A
def reset(self):
self.write(0x12, 0x1)
time.sleep(0.1)
def init(self):
chip_id = self.verify_chip_id(0x0F, [0x69, 0x6A])
if chip_id == 0x6A:
self.source = log.SensorEventData.SensorSource.lsm6ds3trc
else:
self.source = log.SensorEventData.SensorSource.lsm6ds3
# self-test
if "LSM_SELF_TEST" in os.environ:
self.self_test(self.LSM6DS3_GYRO_POSITIVE_TEST)
self.self_test(self.LSM6DS3_GYRO_NEGATIVE_TEST)
# actual init
self.writes((
# TODO: set scale. Default is +- 250 deg/s
(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, self.LSM6DS3_GYRO_ODR_104HZ),
# Configure data ready signal to pulse mode
(self.LSM6DS3_GYRO_I2C_REG_DRDY_CFG, self.LSM6DS3_GYRO_DRDY_PULSE_MODE),
))
value = self.read(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, 1)[0]
value |= self.LSM6DS3_GYRO_INT1_DRDY_G
self.write(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value)
def get_event(self, ts: int | None = None) -> log.SensorEventData:
assert ts is not None # must come from the IRQ event
# Check if gyroscope data is ready, since it's shared with accelerometer
status_reg = self.read(self.LSM6DS3_GYRO_I2C_REG_STAT_REG, 1)[0]
if not (status_reg & self.LSM6DS3_GYRO_DRDY_GDA):
raise self.DataNotReady
b = self.read(self.LSM6DS3_GYRO_I2C_REG_OUTX_L_G, 6)
x = self.parse_16bit(b[0], b[1])
y = self.parse_16bit(b[2], b[3])
z = self.parse_16bit(b[4], b[5])
scale = (8.75 / 1000.0) * (math.pi / 180.0)
xyz = [y * scale, -x * scale, z * scale]
event = log.SensorEventData.new_message()
event.timestamp = ts
event.version = 2
event.sensor = 5 # SENSOR_GYRO_UNCALIBRATED
event.type = 16 # SENSOR_TYPE_GYROSCOPE_UNCALIBRATED
event.source = self.source
g = event.init('gyroUncalibrated')
g.v = xyz
g.status = 1
return event
def shutdown(self) -> None:
# Disable data ready interrupt on INT1
value = self.read(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, 1)[0]
value &= ~self.LSM6DS3_GYRO_INT1_DRDY_G
self.write(self.LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value)
# Power down by clearing ODR bits
value = self.read(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, 1)[0]
value &= 0x0F
self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, value)
# *** self-test stuff ***
def _wait_for_data_ready(self):
while True:
drdy = self.read(self.LSM6DS3_GYRO_I2C_REG_STAT_REG, 1)[0]
if drdy & self.LSM6DS3_GYRO_DRDY_GDA:
break
def _read_and_avg_data(self) -> list[float]:
out_buf = [0.0, 0.0, 0.0]
for _ in range(5):
self._wait_for_data_ready()
b = self.read(self.LSM6DS3_GYRO_I2C_REG_OUTX_L_G, 6)
for j in range(3):
val = self.parse_16bit(b[j*2], b[j*2+1]) * 70.0 # mdps/LSB for 2000 dps
out_buf[j] += val
return [x / 5.0 for x in out_buf]
def self_test(self, test_type: int):
# Set ODR to 208Hz, FS to 2000dps
self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, self.LSM6DS3_GYRO_ODR_208HZ | self.LSM6DS3_GYRO_FS_2000dps)
# Wait for stable output
time.sleep(0.15)
self._wait_for_data_ready()
val_st_off = self._read_and_avg_data()
# Enable self-test
self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type)
# Wait for stable output
time.sleep(0.05)
self._wait_for_data_ready()
val_st_on = self._read_and_avg_data()
# Disable sensor and self-test
self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0)
self.write(self.LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0)
# Calculate differences and check limits
test_val = [abs(on - off) for on, off in zip(val_st_on, val_st_off, strict=False)]
for val in test_val:
if val < self.LSM6DS3_GYRO_MIN_ST_LIMIT_mdps or val > self.LSM6DS3_GYRO_MAX_ST_LIMIT_mdps:
raise Exception(f"Gyroscope self-test failed for test type {test_type}")
if __name__ == "__main__":
s = LSM6DS3_Gyro(1)
s.init()
time.sleep(0.1)
print(s.get_event(0))
s.shutdown()
system/sensord/sensors/lsm6ds3_temp.cc
-37
View File
@@ -1,37 +0,0 @@
#include "system/sensord/sensors/lsm6ds3_temp.h"
#include <cassert>
#include "common/swaglog.h"
#include "common/timing.h"
LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {}
int LSM6DS3_Temp::init() {
int ret = verify_chip_id(LSM6DS3_TEMP_I2C_REG_ID, {LSM6DS3_TEMP_CHIP_ID, LSM6DS3TRC_TEMP_CHIP_ID});
if (ret == -1) return -1;
if (ret == LSM6DS3TRC_TEMP_CHIP_ID) {
source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
}
return 0;
}
bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
uint8_t buffer[2];
int len = read_register(LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float scale = (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) ? 256.0f : 16.0f;
float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / scale;
auto event = msg.initEvent().initTemperatureSensor();
event.setSource(source);
event.setVersion(1);
event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
event.setTimestamp(start_time);
event.setTemperature(temp);
return true;
}
system/sensord/sensors/lsm6ds3_temp.h
-26
View File
@@ -1,26 +0,0 @@
#pragma once
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define LSM6DS3_TEMP_I2C_ADDR 0x6A
// Registers of the chip
#define LSM6DS3_TEMP_I2C_REG_ID 0x0F
#define LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L 0x20
// Constants
#define LSM6DS3_TEMP_CHIP_ID 0x69
#define LSM6DS3TRC_TEMP_CHIP_ID 0x6A
class LSM6DS3_Temp : public I2CSensor {
uint8_t get_device_address() {return LSM6DS3_TEMP_I2C_ADDR;}
cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
public:
LSM6DS3_Temp(I2CBus *bus);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown() { return 0; }
};
system/sensord/sensors/lsm6ds3_temp.py
+33
View File
@@ -0,0 +1,33 @@
import time
from cereal import log
from openpilot.system.sensord.sensors.i2c_sensor import Sensor
# https://content.arduino.cc/assets/st_imu_lsm6ds3_datasheet.pdf
class LSM6DS3_Temp(Sensor):
@property
def device_address(self) -> int:
return 0x6A
def _read_temperature(self) -> float:
scale = 16.0 if self.source == log.SensorEventData.SensorSource.lsm6ds3 else 256.0
data = self.read(0x20, 2)
return 25 + (self.parse_16bit(data[0], data[1]) / scale)
def init(self):
chip_id = self.verify_chip_id(0x0F, [0x69, 0x6A])
if chip_id == 0x6A:
self.source = log.SensorEventData.SensorSource.lsm6ds3trc
else:
self.source = log.SensorEventData.SensorSource.lsm6ds3
def get_event(self, ts: int | None = None) -> log.SensorEventData:
event = log.SensorEventData.new_message()
event.version = 1
event.timestamp = int(time.monotonic() * 1e9)
event.source = self.source
event.temperature = self._read_temperature()
return event
def shutdown(self) -> None:
pass
system/sensord/sensors/mmc5603nj_magn.cc
-108
View File
@@ -1,108 +0,0 @@
#include "system/sensord/sensors/mmc5603nj_magn.h"
#include <algorithm>
#include <cassert>
#include <vector>
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {}
int MMC5603NJ_Magn::init() {
int ret = verify_chip_id(MMC5603NJ_I2C_REG_ID, {MMC5603NJ_CHIP_ID});
if (ret == -1) return -1;
// Set ODR to 0
ret = set_register(MMC5603NJ_I2C_REG_ODR, 0);
if (ret < 0) {
goto fail;
}
// Set BW to 0b01 for 1-150 Hz operation
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_1, 0b01);
if (ret < 0) {
goto fail;
}
fail:
return ret;
}
int MMC5603NJ_Magn::shutdown() {
int ret = 0;
// disable auto reset of measurements
uint8_t value = 0;
ret = read_register(MMC5603NJ_I2C_REG_INTERNAL_0, &value, 1);
if (ret < 0) {
goto fail;
}
value &= ~(MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN);
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, value);
if (ret < 0) {
goto fail;
}
// set ODR to 0 to leave continuous mode
ret = set_register(MMC5603NJ_I2C_REG_ODR, 0);
if (ret < 0) {
goto fail;
}
return ret;
fail:
LOGE("Could not disable mmc5603nj auto set reset");
return ret;
}
void MMC5603NJ_Magn::start_measurement() {
set_register(MMC5603NJ_I2C_REG_INTERNAL_0, 0b01);
util::sleep_for(5);
}
std::vector<float> MMC5603NJ_Magn::read_measurement() {
int len;
uint8_t buffer[9];
len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float scale = 1.0 / 16384.0;
float x = (read_20_bit(buffer[6], buffer[1], buffer[0]) * scale) - 32.0;
float y = (read_20_bit(buffer[7], buffer[3], buffer[2]) * scale) - 32.0;
float z = (read_20_bit(buffer[8], buffer[5], buffer[4]) * scale) - 32.0;
std::vector<float> xyz = {x, y, z};
return xyz;
}
bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
// SET - RESET cycle
set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_SET);
util::sleep_for(5);
MMC5603NJ_Magn::start_measurement();
std::vector<float> xyz = MMC5603NJ_Magn::read_measurement();
set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_RESET);
util::sleep_for(5);
MMC5603NJ_Magn::start_measurement();
std::vector<float> reset_xyz = MMC5603NJ_Magn::read_measurement();
auto event = msg.initEvent().initMagnetometer();
event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ);
event.setVersion(1);
event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
event.setTimestamp(start_time);
float vals[] = {xyz[0], xyz[1], xyz[2], reset_xyz[0], reset_xyz[1], reset_xyz[2]};
bool valid = true;
if (std::any_of(std::begin(vals), std::end(vals), [](float val) { return val == -32.0; })) {
valid = false;
}
auto svec = event.initMagneticUncalibrated();
svec.setV(vals);
svec.setStatus(valid);
return true;
}
system/sensord/sensors/mmc5603nj_magn.h
-37
View File
@@ -1,37 +0,0 @@
#pragma once
#include <vector>
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define MMC5603NJ_I2C_ADDR 0x30
// Registers of the chip
#define MMC5603NJ_I2C_REG_XOUT0 0x00
#define MMC5603NJ_I2C_REG_ODR 0x1A
#define MMC5603NJ_I2C_REG_INTERNAL_0 0x1B
#define MMC5603NJ_I2C_REG_INTERNAL_1 0x1C
#define MMC5603NJ_I2C_REG_INTERNAL_2 0x1D
#define MMC5603NJ_I2C_REG_ID 0x39
// Constants
#define MMC5603NJ_CHIP_ID 0x10
#define MMC5603NJ_CMM_FREQ_EN (1 << 7)
#define MMC5603NJ_AUTO_SR_EN (1 << 5)
#define MMC5603NJ_CMM_EN (1 << 4)
#define MMC5603NJ_EN_PRD_SET (1 << 3)
#define MMC5603NJ_SET (1 << 3)
#define MMC5603NJ_RESET (1 << 4)
class MMC5603NJ_Magn : public I2CSensor {
private:
uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;}
void start_measurement();
std::vector<float> read_measurement();
public:
MMC5603NJ_Magn(I2CBus *bus);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};
system/sensord/sensors/mmc5603nj_magn.py
+76
View File
@@ -0,0 +1,76 @@
import time
from cereal import log
from openpilot.system.sensord.sensors.i2c_sensor import Sensor
# https://www.mouser.com/datasheet/2/821/Memsic_09102019_Datasheet_Rev.B-1635324.pdf
# Register addresses
REG_ODR = 0x1A
REG_INTERNAL_0 = 0x1B
REG_INTERNAL_1 = 0x1C
# Control register settings
CMM_FREQ_EN = (1 << 7)
AUTO_SR_EN = (1 << 5)
SET = (1 << 3)
RESET = (1 << 4)
class MMC5603NJ_Magn(Sensor):
@property
def device_address(self) -> int:
return 0x30
def init(self):
self.verify_chip_id(0x39, [0x10, ])
self.writes((
(REG_ODR, 0),
# Set BW to 0b01 for 1-150 Hz operation
(REG_INTERNAL_1, 0b01),
))
def _read_data(self, cycle) -> list[float]:
# start measurement
self.write(REG_INTERNAL_0, cycle)
self.wait()
# read out XYZ
scale = 1.0 / 16384.0
b = self.read(0x00, 9)
return [
(self.parse_20bit(b[6], b[1], b[0]) * scale) - 32.0,
(self.parse_20bit(b[7], b[3], b[2]) * scale) - 32.0,
(self.parse_20bit(b[8], b[5], b[4]) * scale) - 32.0,
]
def get_event(self, ts: int | None = None) -> log.SensorEventData:
ts = time.monotonic_ns()
# SET - RESET cycle
xyz = self._read_data(SET)
reset_xyz = self._read_data(RESET)
vals = [*xyz, *reset_xyz]
event = log.SensorEventData.new_message()
event.timestamp = ts
event.version = 1
event.sensor = 3 # SENSOR_MAGNETOMETER_UNCALIBRATED
event.type = 14 # SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED
event.source = log.SensorEventData.SensorSource.mmc5603nj
m = event.init('magneticUncalibrated')
m.v = vals
m.status = int(all(int(v) != -32 for v in vals))
return event
def shutdown(self) -> None:
v = self.read(REG_INTERNAL_0, 1)[0]
self.writes((
# disable auto-reset of measurements
(REG_INTERNAL_0, (v & (~(CMM_FREQ_EN | AUTO_SR_EN)))),
# disable continuous mode
(REG_ODR, 0),
))
system/sensord/sensors/sensor.h
-24
View File
@@ -1,24 +0,0 @@
#pragma once
#include "cereal/messaging/messaging.h"
class Sensor {
public:
int gpio_fd = -1;
bool enabled = false;
uint64_t start_ts = 0;
uint64_t init_delay = 500e6; // default dealy 500ms
virtual ~Sensor() {}
virtual int init() = 0;
virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
virtual bool has_interrupt_enabled() = 0;
virtual int shutdown() = 0;
virtual bool is_data_valid(uint64_t current_ts) {
if (start_ts == 0) {
start_ts = current_ts;
}
return (current_ts - start_ts) > init_delay;
}
};
system/sensord/sensors_qcom2.cc
-179
View File
@@ -1,179 +0,0 @@
#include <sys/resource.h>
#include <chrono>
#include <thread>
#include <vector>
#include <map>
#include <poll.h>
#include <linux/gpio.h>
#include "cereal/services.h"
#include "cereal/messaging/messaging.h"
#include "common/i2c.h"
#include "common/ratekeeper.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
#include "system/sensord/sensors/bmx055_accel.h"
#include "system/sensord/sensors/bmx055_gyro.h"
#include "system/sensord/sensors/bmx055_magn.h"
#include "system/sensord/sensors/bmx055_temp.h"
#include "system/sensord/sensors/constants.h"
#include "system/sensord/sensors/lsm6ds3_accel.h"
#include "system/sensord/sensors/lsm6ds3_gyro.h"
#include "system/sensord/sensors/lsm6ds3_temp.h"
#include "system/sensord/sensors/mmc5603nj_magn.h"
#define I2C_BUS_IMU 1
ExitHandler do_exit;
void interrupt_loop(std::vector<std::tuple<Sensor *, std::string>> sensors) {
PubMaster pm({"gyroscope", "accelerometer"});
int fd = -1;
for (auto &[sensor, msg_name] : sensors) {
if (sensor->has_interrupt_enabled()) {
fd = sensor->gpio_fd;
break;
}
}
uint64_t offset = nanos_since_epoch() - nanos_since_boot();
struct pollfd fd_list[1] = {0};
fd_list[0].fd = fd;
fd_list[0].events = POLLIN | POLLPRI;
while (!do_exit) {
int err = poll(fd_list, 1, 100);
if (err == -1) {
if (errno == EINTR) {
continue;
}
return;
} else if (err == 0) {
LOGE("poll timed out");
continue;
}
if ((fd_list[0].revents & (POLLIN | POLLPRI)) == 0) {
LOGE("no poll events set");
continue;
}
// Read all events
struct gpioevent_data evdata[16];
err = HANDLE_EINTR(read(fd, evdata, sizeof(evdata)));
if (err < 0 || err % sizeof(*evdata) != 0) {
LOGE("error reading event data %d", err);
continue;
}
uint64_t cur_offset = nanos_since_epoch() - nanos_since_boot();
uint64_t diff = cur_offset > offset ? cur_offset - offset : offset - cur_offset;
if (diff > 10*1e6) { // 10ms
LOGW("time jumped: %lu %lu", cur_offset, offset);
offset = cur_offset;
// we don't have a valid timestamp since the
// time jumped, so throw out this measurement.
continue;
}
int num_events = err / sizeof(*evdata);
uint64_t ts = evdata[num_events - 1].timestamp - cur_offset;
for (auto &[sensor, msg_name] : sensors) {
if (!sensor->has_interrupt_enabled()) {
continue;
}
MessageBuilder msg;
if (!sensor->get_event(msg, ts)) {
continue;
}
if (!sensor->is_data_valid(ts)) {
continue;
}
pm.send(msg_name.c_str(), msg);
}
}
}
void polling_loop(Sensor *sensor, std::string msg_name) {
PubMaster pm({msg_name.c_str()});
RateKeeper rk(msg_name, services.at(msg_name).frequency);
while (!do_exit) {
MessageBuilder msg;
if (sensor->get_event(msg) && sensor->is_data_valid(nanos_since_boot())) {
pm.send(msg_name.c_str(), msg);
}
rk.keepTime();
}
}
int sensor_loop(I2CBus *i2c_bus_imu) {
// Sensor init
std::vector<std::tuple<Sensor *, std::string>> sensors_init = {
{new BMX055_Accel(i2c_bus_imu), "accelerometer2"},
{new BMX055_Gyro(i2c_bus_imu), "gyroscope2"},
{new BMX055_Magn(i2c_bus_imu), "magnetometer"},
{new BMX055_Temp(i2c_bus_imu), "temperatureSensor2"},
{new LSM6DS3_Accel(i2c_bus_imu, GPIO_LSM_INT), "accelerometer"},
{new LSM6DS3_Gyro(i2c_bus_imu, GPIO_LSM_INT, true), "gyroscope"},
{new LSM6DS3_Temp(i2c_bus_imu), "temperatureSensor"},
{new MMC5603NJ_Magn(i2c_bus_imu), "magnetometer"},
};
// Initialize sensors
std::vector<std::thread> threads;
for (auto &[sensor, msg_name] : sensors_init) {
int err = sensor->init();
if (err < 0) {
continue;
}
if (!sensor->has_interrupt_enabled()) {
threads.emplace_back(polling_loop, sensor, msg_name);
}
}
// increase interrupt quality by pinning interrupt and process to core 1
setpriority(PRIO_PROCESS, 0, -18);
util::set_core_affinity({1});
// TODO: get the IRQ number from gpiochip
std::string irq_path = "/proc/irq/336/smp_affinity_list";
if (!util::file_exists(irq_path)) {
irq_path = "/proc/irq/335/smp_affinity_list";
}
std::system(util::string_format("sudo su -c 'echo 1 > %s'", irq_path.c_str()).c_str());
// thread for reading events via interrupts
threads.emplace_back(&interrupt_loop, std::ref(sensors_init));
// wait for all threads to finish
for (auto &t : threads) {
t.join();
}
for (auto &[sensor, msg_name] : sensors_init) {
sensor->shutdown();
delete sensor;
}
return 0;
}
int main(int argc, char *argv[]) {
try {
auto i2c_bus_imu = std::make_unique<I2CBus>(I2C_BUS_IMU);
return sensor_loop(i2c_bus_imu.get());
} catch (std::exception &e) {
LOGE("I2CBus init failed");
return -1;
}
}
system/ubloxd/.gitignore
-2
View File
@@ -1,2 +0,0 @@
ubloxd
tests/test_glonass_runner
system/ubloxd/SConscript
+8 -17
View File
@@ -1,20 +1,11 @@
Import('env', 'common', 'messaging')
loc_libs = [messaging, common, 'kaitai', 'pthread']
Import('env')
if GetOption('kaitai'):
generated = Dir('generated').srcnode().abspath
cmd = f"kaitai-struct-compiler --target cpp_stl --outdir {generated} $SOURCES"
env.Command(['generated/ubx.cpp', 'generated/ubx.h'], 'ubx.ksy', cmd)
env.Command(['generated/gps.cpp', 'generated/gps.h'], 'gps.ksy', cmd)
glonass = env.Command(['generated/glonass.cpp', 'generated/glonass.h'], 'glonass.ksy', cmd)
current_dir = Dir('./generated/').srcnode().abspath
python_cmd = f"kaitai-struct-compiler --target python --outdir {current_dir} $SOURCES"
env.Command(File('./generated/ubx.py'), 'ubx.ksy', python_cmd)
env.Command(File('./generated/gps.py'), 'gps.ksy', python_cmd)
env.Command(File('./generated/glonass.py'), 'glonass.ksy', python_cmd)
# kaitai issue: https://github.com/kaitai-io/kaitai_struct/issues/910
patch = env.Command(None, 'glonass_fix.patch', 'git apply $SOURCES')
env.Depends(patch, glonass)
glonass_obj = env.Object('generated/glonass.cpp')
env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp", glonass_obj], LIBS=loc_libs)
if GetOption('extras'):
env.Program("tests/test_glonass_runner", ['tests/test_glonass_runner.cc', 'tests/test_glonass_kaitai.cc', glonass_obj], LIBS=[loc_libs])
py_glonass_fix = env.Command(None, File('./generated/glonass.py'), "sed -i 's/self._io.align_to_byte()/# self._io.align_to_byte()/' $SOURCES")
env.Depends(py_glonass_fix, File('./generated/glonass.py'))
system/ubloxd/generated/glonass.cpp
-353
View File
@@ -1,353 +0,0 @@
// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
#include "glonass.h"
glonass_t::glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = this;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void glonass_t::_read() {
m_idle_chip = m__io->read_bits_int_be(1);
m_string_number = m__io->read_bits_int_be(4);
//m__io->align_to_byte();
switch (string_number()) {
case 4: {
m_data = new string_4_t(m__io, this, m__root);
break;
}
case 1: {
m_data = new string_1_t(m__io, this, m__root);
break;
}
case 3: {
m_data = new string_3_t(m__io, this, m__root);
break;
}
case 5: {
m_data = new string_5_t(m__io, this, m__root);
break;
}
case 2: {
m_data = new string_2_t(m__io, this, m__root);
break;
}
default: {
m_data = new string_non_immediate_t(m__io, this, m__root);
break;
}
}
m_hamming_code = m__io->read_bits_int_be(8);
m_pad_1 = m__io->read_bits_int_be(11);
m_superframe_number = m__io->read_bits_int_be(16);
m_pad_2 = m__io->read_bits_int_be(8);
m_frame_number = m__io->read_bits_int_be(8);
}
glonass_t::~glonass_t() {
_clean_up();
}
void glonass_t::_clean_up() {
if (m_data) {
delete m_data; m_data = 0;
}
}
glonass_t::string_4_t::string_4_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
f_tau_n = false;
f_delta_tau_n = false;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void glonass_t::string_4_t::_read() {
m_tau_n_sign = m__io->read_bits_int_be(1);
m_tau_n_value = m__io->read_bits_int_be(21);
m_delta_tau_n_sign = m__io->read_bits_int_be(1);
m_delta_tau_n_value = m__io->read_bits_int_be(4);
m_e_n = m__io->read_bits_int_be(5);
m_not_used_1 = m__io->read_bits_int_be(14);
m_p4 = m__io->read_bits_int_be(1);
m_f_t = m__io->read_bits_int_be(4);
m_not_used_2 = m__io->read_bits_int_be(3);
m_n_t = m__io->read_bits_int_be(11);
m_n = m__io->read_bits_int_be(5);
m_m = m__io->read_bits_int_be(2);
}
glonass_t::string_4_t::~string_4_t() {
_clean_up();
}
void glonass_t::string_4_t::_clean_up() {
}
int32_t glonass_t::string_4_t::tau_n() {
if (f_tau_n)
return m_tau_n;
m_tau_n = ((tau_n_sign()) ? ((tau_n_value() * -1)) : (tau_n_value()));
f_tau_n = true;
return m_tau_n;
}
int32_t glonass_t::string_4_t::delta_tau_n() {
if (f_delta_tau_n)
return m_delta_tau_n;
m_delta_tau_n = ((delta_tau_n_sign()) ? ((delta_tau_n_value() * -1)) : (delta_tau_n_value()));
f_delta_tau_n = true;
return m_delta_tau_n;
}
glonass_t::string_non_immediate_t::string_non_immediate_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void glonass_t::string_non_immediate_t::_read() {
m_data_1 = m__io->read_bits_int_be(64);
m_data_2 = m__io->read_bits_int_be(8);
}
glonass_t::string_non_immediate_t::~string_non_immediate_t() {
_clean_up();
}
void glonass_t::string_non_immediate_t::_clean_up() {
}
glonass_t::string_5_t::string_5_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void glonass_t::string_5_t::_read() {
m_n_a = m__io->read_bits_int_be(11);
m_tau_c = m__io->read_bits_int_be(32);
m_not_used = m__io->read_bits_int_be(1);
m_n_4 = m__io->read_bits_int_be(5);
m_tau_gps = m__io->read_bits_int_be(22);
m_l_n = m__io->read_bits_int_be(1);
}
glonass_t::string_5_t::~string_5_t() {
_clean_up();
}
void glonass_t::string_5_t::_clean_up() {
}
glonass_t::string_1_t::string_1_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
f_x_vel = false;
f_x_accel = false;
f_x = false;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void glonass_t::string_1_t::_read() {
m_not_used = m__io->read_bits_int_be(2);
m_p1 = m__io->read_bits_int_be(2);
m_t_k = m__io->read_bits_int_be(12);
m_x_vel_sign = m__io->read_bits_int_be(1);
m_x_vel_value = m__io->read_bits_int_be(23);
m_x_accel_sign = m__io->read_bits_int_be(1);
m_x_accel_value = m__io->read_bits_int_be(4);
m_x_sign = m__io->read_bits_int_be(1);
m_x_value = m__io->read_bits_int_be(26);
}
glonass_t::string_1_t::~string_1_t() {
_clean_up();
}
void glonass_t::string_1_t::_clean_up() {
}
int32_t glonass_t::string_1_t::x_vel() {
if (f_x_vel)
return m_x_vel;
m_x_vel = ((x_vel_sign()) ? ((x_vel_value() * -1)) : (x_vel_value()));
f_x_vel = true;
return m_x_vel;
}
int32_t glonass_t::string_1_t::x_accel() {
if (f_x_accel)
return m_x_accel;
m_x_accel = ((x_accel_sign()) ? ((x_accel_value() * -1)) : (x_accel_value()));
f_x_accel = true;
return m_x_accel;
}
int32_t glonass_t::string_1_t::x() {
if (f_x)
return m_x;
m_x = ((x_sign()) ? ((x_value() * -1)) : (x_value()));
f_x = true;
return m_x;
}
glonass_t::string_2_t::string_2_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
f_y_vel = false;
f_y_accel = false;
f_y = false;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void glonass_t::string_2_t::_read() {
m_b_n = m__io->read_bits_int_be(3);
m_p2 = m__io->read_bits_int_be(1);
m_t_b = m__io->read_bits_int_be(7);
m_not_used = m__io->read_bits_int_be(5);
m_y_vel_sign = m__io->read_bits_int_be(1);
m_y_vel_value = m__io->read_bits_int_be(23);
m_y_accel_sign = m__io->read_bits_int_be(1);
m_y_accel_value = m__io->read_bits_int_be(4);
m_y_sign = m__io->read_bits_int_be(1);
m_y_value = m__io->read_bits_int_be(26);
}
glonass_t::string_2_t::~string_2_t() {
_clean_up();
}
void glonass_t::string_2_t::_clean_up() {
}
int32_t glonass_t::string_2_t::y_vel() {
if (f_y_vel)
return m_y_vel;
m_y_vel = ((y_vel_sign()) ? ((y_vel_value() * -1)) : (y_vel_value()));
f_y_vel = true;
return m_y_vel;
}
int32_t glonass_t::string_2_t::y_accel() {
if (f_y_accel)
return m_y_accel;
m_y_accel = ((y_accel_sign()) ? ((y_accel_value() * -1)) : (y_accel_value()));
f_y_accel = true;
return m_y_accel;
}
int32_t glonass_t::string_2_t::y() {
if (f_y)
return m_y;
m_y = ((y_sign()) ? ((y_value() * -1)) : (y_value()));
f_y = true;
return m_y;
}
glonass_t::string_3_t::string_3_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
f_gamma_n = false;
f_z_vel = false;
f_z_accel = false;
f_z = false;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void glonass_t::string_3_t::_read() {
m_p3 = m__io->read_bits_int_be(1);
m_gamma_n_sign = m__io->read_bits_int_be(1);
m_gamma_n_value = m__io->read_bits_int_be(10);
m_not_used = m__io->read_bits_int_be(1);
m_p = m__io->read_bits_int_be(2);
m_l_n = m__io->read_bits_int_be(1);
m_z_vel_sign = m__io->read_bits_int_be(1);
m_z_vel_value = m__io->read_bits_int_be(23);
m_z_accel_sign = m__io->read_bits_int_be(1);
m_z_accel_value = m__io->read_bits_int_be(4);
m_z_sign = m__io->read_bits_int_be(1);
m_z_value = m__io->read_bits_int_be(26);
}
glonass_t::string_3_t::~string_3_t() {
_clean_up();
}
void glonass_t::string_3_t::_clean_up() {
}
int32_t glonass_t::string_3_t::gamma_n() {
if (f_gamma_n)
return m_gamma_n;
m_gamma_n = ((gamma_n_sign()) ? ((gamma_n_value() * -1)) : (gamma_n_value()));
f_gamma_n = true;
return m_gamma_n;
}
int32_t glonass_t::string_3_t::z_vel() {
if (f_z_vel)
return m_z_vel;
m_z_vel = ((z_vel_sign()) ? ((z_vel_value() * -1)) : (z_vel_value()));
f_z_vel = true;
return m_z_vel;
}
int32_t glonass_t::string_3_t::z_accel() {
if (f_z_accel)
return m_z_accel;
m_z_accel = ((z_accel_sign()) ? ((z_accel_value() * -1)) : (z_accel_value()));
f_z_accel = true;
return m_z_accel;
}
int32_t glonass_t::string_3_t::z() {
if (f_z)
return m_z;
m_z = ((z_sign()) ? ((z_value() * -1)) : (z_value()));
f_z = true;
return m_z;
}
system/ubloxd/generated/glonass.h
-375
View File
@@ -1,375 +0,0 @@
#ifndef GLONASS_H_
#define GLONASS_H_
// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
#include "kaitai/kaitaistruct.h"
#include <stdint.h>
#if KAITAI_STRUCT_VERSION < 9000L
#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required"
#endif
class glonass_t : public kaitai::kstruct {
public:
class string_4_t;
class string_non_immediate_t;
class string_5_t;
class string_1_t;
class string_2_t;
class string_3_t;
glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, glonass_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~glonass_t();
class string_4_t : public kaitai::kstruct {
public:
string_4_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~string_4_t();
private:
bool f_tau_n;
int32_t m_tau_n;
public:
int32_t tau_n();
private:
bool f_delta_tau_n;
int32_t m_delta_tau_n;
public:
int32_t delta_tau_n();
private:
bool m_tau_n_sign;
uint64_t m_tau_n_value;
bool m_delta_tau_n_sign;
uint64_t m_delta_tau_n_value;
uint64_t m_e_n;
uint64_t m_not_used_1;
bool m_p4;
uint64_t m_f_t;
uint64_t m_not_used_2;
uint64_t m_n_t;
uint64_t m_n;
uint64_t m_m;
glonass_t* m__root;
glonass_t* m__parent;
public:
bool tau_n_sign() const { return m_tau_n_sign; }
uint64_t tau_n_value() const { return m_tau_n_value; }
bool delta_tau_n_sign() const { return m_delta_tau_n_sign; }
uint64_t delta_tau_n_value() const { return m_delta_tau_n_value; }
uint64_t e_n() const { return m_e_n; }
uint64_t not_used_1() const { return m_not_used_1; }
bool p4() const { return m_p4; }
uint64_t f_t() const { return m_f_t; }
uint64_t not_used_2() const { return m_not_used_2; }
uint64_t n_t() const { return m_n_t; }
uint64_t n() const { return m_n; }
uint64_t m() const { return m_m; }
glonass_t* _root() const { return m__root; }
glonass_t* _parent() const { return m__parent; }
};
class string_non_immediate_t : public kaitai::kstruct {
public:
string_non_immediate_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~string_non_immediate_t();
private:
uint64_t m_data_1;
uint64_t m_data_2;
glonass_t* m__root;
glonass_t* m__parent;
public:
uint64_t data_1() const { return m_data_1; }
uint64_t data_2() const { return m_data_2; }
glonass_t* _root() const { return m__root; }
glonass_t* _parent() const { return m__parent; }
};
class string_5_t : public kaitai::kstruct {
public:
string_5_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~string_5_t();
private:
uint64_t m_n_a;
uint64_t m_tau_c;
bool m_not_used;
uint64_t m_n_4;
uint64_t m_tau_gps;
bool m_l_n;
glonass_t* m__root;
glonass_t* m__parent;
public:
uint64_t n_a() const { return m_n_a; }
uint64_t tau_c() const { return m_tau_c; }
bool not_used() const { return m_not_used; }
uint64_t n_4() const { return m_n_4; }
uint64_t tau_gps() const { return m_tau_gps; }
bool l_n() const { return m_l_n; }
glonass_t* _root() const { return m__root; }
glonass_t* _parent() const { return m__parent; }
};
class string_1_t : public kaitai::kstruct {
public:
string_1_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~string_1_t();
private:
bool f_x_vel;
int32_t m_x_vel;
public:
int32_t x_vel();
private:
bool f_x_accel;
int32_t m_x_accel;
public:
int32_t x_accel();
private:
bool f_x;
int32_t m_x;
public:
int32_t x();
private:
uint64_t m_not_used;
uint64_t m_p1;
uint64_t m_t_k;
bool m_x_vel_sign;
uint64_t m_x_vel_value;
bool m_x_accel_sign;
uint64_t m_x_accel_value;
bool m_x_sign;
uint64_t m_x_value;
glonass_t* m__root;
glonass_t* m__parent;
public:
uint64_t not_used() const { return m_not_used; }
uint64_t p1() const { return m_p1; }
uint64_t t_k() const { return m_t_k; }
bool x_vel_sign() const { return m_x_vel_sign; }
uint64_t x_vel_value() const { return m_x_vel_value; }
bool x_accel_sign() const { return m_x_accel_sign; }
uint64_t x_accel_value() const { return m_x_accel_value; }
bool x_sign() const { return m_x_sign; }
uint64_t x_value() const { return m_x_value; }
glonass_t* _root() const { return m__root; }
glonass_t* _parent() const { return m__parent; }
};
class string_2_t : public kaitai::kstruct {
public:
string_2_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~string_2_t();
private:
bool f_y_vel;
int32_t m_y_vel;
public:
int32_t y_vel();
private:
bool f_y_accel;
int32_t m_y_accel;
public:
int32_t y_accel();
private:
bool f_y;
int32_t m_y;
public:
int32_t y();
private:
uint64_t m_b_n;
bool m_p2;
uint64_t m_t_b;
uint64_t m_not_used;
bool m_y_vel_sign;
uint64_t m_y_vel_value;
bool m_y_accel_sign;
uint64_t m_y_accel_value;
bool m_y_sign;
uint64_t m_y_value;
glonass_t* m__root;
glonass_t* m__parent;
public:
uint64_t b_n() const { return m_b_n; }
bool p2() const { return m_p2; }
uint64_t t_b() const { return m_t_b; }
uint64_t not_used() const { return m_not_used; }
bool y_vel_sign() const { return m_y_vel_sign; }
uint64_t y_vel_value() const { return m_y_vel_value; }
bool y_accel_sign() const { return m_y_accel_sign; }
uint64_t y_accel_value() const { return m_y_accel_value; }
bool y_sign() const { return m_y_sign; }
uint64_t y_value() const { return m_y_value; }
glonass_t* _root() const { return m__root; }
glonass_t* _parent() const { return m__parent; }
};
class string_3_t : public kaitai::kstruct {
public:
string_3_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~string_3_t();
private:
bool f_gamma_n;
int32_t m_gamma_n;
public:
int32_t gamma_n();
private:
bool f_z_vel;
int32_t m_z_vel;
public:
int32_t z_vel();
private:
bool f_z_accel;
int32_t m_z_accel;
public:
int32_t z_accel();
private:
bool f_z;
int32_t m_z;
public:
int32_t z();
private:
bool m_p3;
bool m_gamma_n_sign;
uint64_t m_gamma_n_value;
bool m_not_used;
uint64_t m_p;
bool m_l_n;
bool m_z_vel_sign;
uint64_t m_z_vel_value;
bool m_z_accel_sign;
uint64_t m_z_accel_value;
bool m_z_sign;
uint64_t m_z_value;
glonass_t* m__root;
glonass_t* m__parent;
public:
bool p3() const { return m_p3; }
bool gamma_n_sign() const { return m_gamma_n_sign; }
uint64_t gamma_n_value() const { return m_gamma_n_value; }
bool not_used() const { return m_not_used; }
uint64_t p() const { return m_p; }
bool l_n() const { return m_l_n; }
bool z_vel_sign() const { return m_z_vel_sign; }
uint64_t z_vel_value() const { return m_z_vel_value; }
bool z_accel_sign() const { return m_z_accel_sign; }
uint64_t z_accel_value() const { return m_z_accel_value; }
bool z_sign() const { return m_z_sign; }
uint64_t z_value() const { return m_z_value; }
glonass_t* _root() const { return m__root; }
glonass_t* _parent() const { return m__parent; }
};
private:
bool m_idle_chip;
uint64_t m_string_number;
kaitai::kstruct* m_data;
uint64_t m_hamming_code;
uint64_t m_pad_1;
uint64_t m_superframe_number;
uint64_t m_pad_2;
uint64_t m_frame_number;
glonass_t* m__root;
kaitai::kstruct* m__parent;
public:
bool idle_chip() const { return m_idle_chip; }
uint64_t string_number() const { return m_string_number; }
kaitai::kstruct* data() const { return m_data; }
uint64_t hamming_code() const { return m_hamming_code; }
uint64_t pad_1() const { return m_pad_1; }
uint64_t superframe_number() const { return m_superframe_number; }
uint64_t pad_2() const { return m_pad_2; }
uint64_t frame_number() const { return m_frame_number; }
glonass_t* _root() const { return m__root; }
kaitai::kstruct* _parent() const { return m__parent; }
};
#endif // GLONASS_H_
system/ubloxd/generated/glonass.py
+247
View File
@@ -0,0 +1,247 @@
# This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
import kaitaistruct
from kaitaistruct import KaitaiStruct, KaitaiStream, BytesIO
if getattr(kaitaistruct, 'API_VERSION', (0, 9)) < (0, 9):
raise Exception("Incompatible Kaitai Struct Python API: 0.9 or later is required, but you have %s" % (kaitaistruct.__version__))
class Glonass(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.idle_chip = self._io.read_bits_int_be(1) != 0
self.string_number = self._io.read_bits_int_be(4)
# workaround for kaitai bit alignment issue (see glonass_fix.patch for C++)
# self._io.align_to_byte()
_on = self.string_number
if _on == 4:
self.data = Glonass.String4(self._io, self, self._root)
elif _on == 1:
self.data = Glonass.String1(self._io, self, self._root)
elif _on == 3:
self.data = Glonass.String3(self._io, self, self._root)
elif _on == 5:
self.data = Glonass.String5(self._io, self, self._root)
elif _on == 2:
self.data = Glonass.String2(self._io, self, self._root)
else:
self.data = Glonass.StringNonImmediate(self._io, self, self._root)
self.hamming_code = self._io.read_bits_int_be(8)
self.pad_1 = self._io.read_bits_int_be(11)
self.superframe_number = self._io.read_bits_int_be(16)
self.pad_2 = self._io.read_bits_int_be(8)
self.frame_number = self._io.read_bits_int_be(8)
class String4(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.tau_n_sign = self._io.read_bits_int_be(1) != 0
self.tau_n_value = self._io.read_bits_int_be(21)
self.delta_tau_n_sign = self._io.read_bits_int_be(1) != 0
self.delta_tau_n_value = self._io.read_bits_int_be(4)
self.e_n = self._io.read_bits_int_be(5)
self.not_used_1 = self._io.read_bits_int_be(14)
self.p4 = self._io.read_bits_int_be(1) != 0
self.f_t = self._io.read_bits_int_be(4)
self.not_used_2 = self._io.read_bits_int_be(3)
self.n_t = self._io.read_bits_int_be(11)
self.n = self._io.read_bits_int_be(5)
self.m = self._io.read_bits_int_be(2)
@property
def tau_n(self):
if hasattr(self, '_m_tau_n'):
return self._m_tau_n
self._m_tau_n = ((self.tau_n_value * -1) if self.tau_n_sign else self.tau_n_value)
return getattr(self, '_m_tau_n', None)
@property
def delta_tau_n(self):
if hasattr(self, '_m_delta_tau_n'):
return self._m_delta_tau_n
self._m_delta_tau_n = ((self.delta_tau_n_value * -1) if self.delta_tau_n_sign else self.delta_tau_n_value)
return getattr(self, '_m_delta_tau_n', None)
class StringNonImmediate(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.data_1 = self._io.read_bits_int_be(64)
self.data_2 = self._io.read_bits_int_be(8)
class String5(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.n_a = self._io.read_bits_int_be(11)
self.tau_c = self._io.read_bits_int_be(32)
self.not_used = self._io.read_bits_int_be(1) != 0
self.n_4 = self._io.read_bits_int_be(5)
self.tau_gps = self._io.read_bits_int_be(22)
self.l_n = self._io.read_bits_int_be(1) != 0
class String1(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.not_used = self._io.read_bits_int_be(2)
self.p1 = self._io.read_bits_int_be(2)
self.t_k = self._io.read_bits_int_be(12)
self.x_vel_sign = self._io.read_bits_int_be(1) != 0
self.x_vel_value = self._io.read_bits_int_be(23)
self.x_accel_sign = self._io.read_bits_int_be(1) != 0
self.x_accel_value = self._io.read_bits_int_be(4)
self.x_sign = self._io.read_bits_int_be(1) != 0
self.x_value = self._io.read_bits_int_be(26)
@property
def x_vel(self):
if hasattr(self, '_m_x_vel'):
return self._m_x_vel
self._m_x_vel = ((self.x_vel_value * -1) if self.x_vel_sign else self.x_vel_value)
return getattr(self, '_m_x_vel', None)
@property
def x_accel(self):
if hasattr(self, '_m_x_accel'):
return self._m_x_accel
self._m_x_accel = ((self.x_accel_value * -1) if self.x_accel_sign else self.x_accel_value)
return getattr(self, '_m_x_accel', None)
@property
def x(self):
if hasattr(self, '_m_x'):
return self._m_x
self._m_x = ((self.x_value * -1) if self.x_sign else self.x_value)
return getattr(self, '_m_x', None)
class String2(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.b_n = self._io.read_bits_int_be(3)
self.p2 = self._io.read_bits_int_be(1) != 0
self.t_b = self._io.read_bits_int_be(7)
self.not_used = self._io.read_bits_int_be(5)
self.y_vel_sign = self._io.read_bits_int_be(1) != 0
self.y_vel_value = self._io.read_bits_int_be(23)
self.y_accel_sign = self._io.read_bits_int_be(1) != 0
self.y_accel_value = self._io.read_bits_int_be(4)
self.y_sign = self._io.read_bits_int_be(1) != 0
self.y_value = self._io.read_bits_int_be(26)
@property
def y_vel(self):
if hasattr(self, '_m_y_vel'):
return self._m_y_vel
self._m_y_vel = ((self.y_vel_value * -1) if self.y_vel_sign else self.y_vel_value)
return getattr(self, '_m_y_vel', None)
@property
def y_accel(self):
if hasattr(self, '_m_y_accel'):
return self._m_y_accel
self._m_y_accel = ((self.y_accel_value * -1) if self.y_accel_sign else self.y_accel_value)
return getattr(self, '_m_y_accel', None)
@property
def y(self):
if hasattr(self, '_m_y'):
return self._m_y
self._m_y = ((self.y_value * -1) if self.y_sign else self.y_value)
return getattr(self, '_m_y', None)
class String3(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.p3 = self._io.read_bits_int_be(1) != 0
self.gamma_n_sign = self._io.read_bits_int_be(1) != 0
self.gamma_n_value = self._io.read_bits_int_be(10)
self.not_used = self._io.read_bits_int_be(1) != 0
self.p = self._io.read_bits_int_be(2)
self.l_n = self._io.read_bits_int_be(1) != 0
self.z_vel_sign = self._io.read_bits_int_be(1) != 0
self.z_vel_value = self._io.read_bits_int_be(23)
self.z_accel_sign = self._io.read_bits_int_be(1) != 0
self.z_accel_value = self._io.read_bits_int_be(4)
self.z_sign = self._io.read_bits_int_be(1) != 0
self.z_value = self._io.read_bits_int_be(26)
@property
def gamma_n(self):
if hasattr(self, '_m_gamma_n'):
return self._m_gamma_n
self._m_gamma_n = ((self.gamma_n_value * -1) if self.gamma_n_sign else self.gamma_n_value)
return getattr(self, '_m_gamma_n', None)
@property
def z_vel(self):
if hasattr(self, '_m_z_vel'):
return self._m_z_vel
self._m_z_vel = ((self.z_vel_value * -1) if self.z_vel_sign else self.z_vel_value)
return getattr(self, '_m_z_vel', None)
@property
def z_accel(self):
if hasattr(self, '_m_z_accel'):
return self._m_z_accel
self._m_z_accel = ((self.z_accel_value * -1) if self.z_accel_sign else self.z_accel_value)
return getattr(self, '_m_z_accel', None)
@property
def z(self):
if hasattr(self, '_m_z'):
return self._m_z
self._m_z = ((self.z_value * -1) if self.z_sign else self.z_value)
return getattr(self, '_m_z', None)
system/ubloxd/generated/gps.cpp
-325
View File
@@ -1,325 +0,0 @@
// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
#include "gps.h"
#include "kaitai/exceptions.h"
gps_t::gps_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = this;
m_tlm = 0;
m_how = 0;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void gps_t::_read() {
m_tlm = new tlm_t(m__io, this, m__root);
m_how = new how_t(m__io, this, m__root);
n_body = true;
switch (how()->subframe_id()) {
case 1: {
n_body = false;
m_body = new subframe_1_t(m__io, this, m__root);
break;
}
case 2: {
n_body = false;
m_body = new subframe_2_t(m__io, this, m__root);
break;
}
case 3: {
n_body = false;
m_body = new subframe_3_t(m__io, this, m__root);
break;
}
case 4: {
n_body = false;
m_body = new subframe_4_t(m__io, this, m__root);
break;
}
}
}
gps_t::~gps_t() {
_clean_up();
}
void gps_t::_clean_up() {
if (m_tlm) {
delete m_tlm; m_tlm = 0;
}
if (m_how) {
delete m_how; m_how = 0;
}
if (!n_body) {
if (m_body) {
delete m_body; m_body = 0;
}
}
}
gps_t::subframe_1_t::subframe_1_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
f_af_0 = false;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void gps_t::subframe_1_t::_read() {
m_week_no = m__io->read_bits_int_be(10);
m_code = m__io->read_bits_int_be(2);
m_sv_accuracy = m__io->read_bits_int_be(4);
m_sv_health = m__io->read_bits_int_be(6);
m_iodc_msb = m__io->read_bits_int_be(2);
m_l2_p_data_flag = m__io->read_bits_int_be(1);
m_reserved1 = m__io->read_bits_int_be(23);
m_reserved2 = m__io->read_bits_int_be(24);
m_reserved3 = m__io->read_bits_int_be(24);
m_reserved4 = m__io->read_bits_int_be(16);
m__io->align_to_byte();
m_t_gd = m__io->read_s1();
m_iodc_lsb = m__io->read_u1();
m_t_oc = m__io->read_u2be();
m_af_2 = m__io->read_s1();
m_af_1 = m__io->read_s2be();
m_af_0_sign = m__io->read_bits_int_be(1);
m_af_0_value = m__io->read_bits_int_be(21);
m_reserved5 = m__io->read_bits_int_be(2);
}
gps_t::subframe_1_t::~subframe_1_t() {
_clean_up();
}
void gps_t::subframe_1_t::_clean_up() {
}
int32_t gps_t::subframe_1_t::af_0() {
if (f_af_0)
return m_af_0;
m_af_0 = ((af_0_sign()) ? ((af_0_value() - (1 << 21))) : (af_0_value()));
f_af_0 = true;
return m_af_0;
}
gps_t::subframe_3_t::subframe_3_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
f_omega_dot = false;
f_idot = false;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void gps_t::subframe_3_t::_read() {
m_c_ic = m__io->read_s2be();
m_omega_0 = m__io->read_s4be();
m_c_is = m__io->read_s2be();
m_i_0 = m__io->read_s4be();
m_c_rc = m__io->read_s2be();
m_omega = m__io->read_s4be();
m_omega_dot_sign = m__io->read_bits_int_be(1);
m_omega_dot_value = m__io->read_bits_int_be(23);
m__io->align_to_byte();
m_iode = m__io->read_u1();
m_idot_sign = m__io->read_bits_int_be(1);
m_idot_value = m__io->read_bits_int_be(13);
m_reserved = m__io->read_bits_int_be(2);
}
gps_t::subframe_3_t::~subframe_3_t() {
_clean_up();
}
void gps_t::subframe_3_t::_clean_up() {
}
int32_t gps_t::subframe_3_t::omega_dot() {
if (f_omega_dot)
return m_omega_dot;
m_omega_dot = ((omega_dot_sign()) ? ((omega_dot_value() - (1 << 23))) : (omega_dot_value()));
f_omega_dot = true;
return m_omega_dot;
}
int32_t gps_t::subframe_3_t::idot() {
if (f_idot)
return m_idot;
m_idot = ((idot_sign()) ? ((idot_value() - (1 << 13))) : (idot_value()));
f_idot = true;
return m_idot;
}
gps_t::subframe_4_t::subframe_4_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void gps_t::subframe_4_t::_read() {
m_data_id = m__io->read_bits_int_be(2);
m_page_id = m__io->read_bits_int_be(6);
m__io->align_to_byte();
n_body = true;
switch (page_id()) {
case 56: {
n_body = false;
m_body = new ionosphere_data_t(m__io, this, m__root);
break;
}
}
}
gps_t::subframe_4_t::~subframe_4_t() {
_clean_up();
}
void gps_t::subframe_4_t::_clean_up() {
if (!n_body) {
if (m_body) {
delete m_body; m_body = 0;
}
}
}
gps_t::subframe_4_t::ionosphere_data_t::ionosphere_data_t(kaitai::kstream* p__io, gps_t::subframe_4_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void gps_t::subframe_4_t::ionosphere_data_t::_read() {
m_a0 = m__io->read_s1();
m_a1 = m__io->read_s1();
m_a2 = m__io->read_s1();
m_a3 = m__io->read_s1();
m_b0 = m__io->read_s1();
m_b1 = m__io->read_s1();
m_b2 = m__io->read_s1();
m_b3 = m__io->read_s1();
}
gps_t::subframe_4_t::ionosphere_data_t::~ionosphere_data_t() {
_clean_up();
}
void gps_t::subframe_4_t::ionosphere_data_t::_clean_up() {
}
gps_t::how_t::how_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void gps_t::how_t::_read() {
m_tow_count = m__io->read_bits_int_be(17);
m_alert = m__io->read_bits_int_be(1);
m_anti_spoof = m__io->read_bits_int_be(1);
m_subframe_id = m__io->read_bits_int_be(3);
m_reserved = m__io->read_bits_int_be(2);
}
gps_t::how_t::~how_t() {
_clean_up();
}
void gps_t::how_t::_clean_up() {
}
gps_t::tlm_t::tlm_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void gps_t::tlm_t::_read() {
m_preamble = m__io->read_bytes(1);
if (!(preamble() == std::string("\x8B", 1))) {
throw kaitai::validation_not_equal_error<std::string>(std::string("\x8B", 1), preamble(), _io(), std::string("/types/tlm/seq/0"));
}
m_tlm = m__io->read_bits_int_be(14);
m_integrity_status = m__io->read_bits_int_be(1);
m_reserved = m__io->read_bits_int_be(1);
}
gps_t::tlm_t::~tlm_t() {
_clean_up();
}
void gps_t::tlm_t::_clean_up() {
}
gps_t::subframe_2_t::subframe_2_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void gps_t::subframe_2_t::_read() {
m_iode = m__io->read_u1();
m_c_rs = m__io->read_s2be();
m_delta_n = m__io->read_s2be();
m_m_0 = m__io->read_s4be();
m_c_uc = m__io->read_s2be();
m_e = m__io->read_s4be();
m_c_us = m__io->read_s2be();
m_sqrt_a = m__io->read_u4be();
m_t_oe = m__io->read_u2be();
m_fit_interval_flag = m__io->read_bits_int_be(1);
m_aoda = m__io->read_bits_int_be(5);
m_reserved = m__io->read_bits_int_be(2);
}
gps_t::subframe_2_t::~subframe_2_t() {
_clean_up();
}
void gps_t::subframe_2_t::_clean_up() {
}
system/ubloxd/generated/gps.h
-359
View File
@@ -1,359 +0,0 @@
#ifndef GPS_H_
#define GPS_H_
// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
#include "kaitai/kaitaistruct.h"
#include <stdint.h>
#if KAITAI_STRUCT_VERSION < 9000L
#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required"
#endif
class gps_t : public kaitai::kstruct {
public:
class subframe_1_t;
class subframe_3_t;
class subframe_4_t;
class how_t;
class tlm_t;
class subframe_2_t;
gps_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~gps_t();
class subframe_1_t : public kaitai::kstruct {
public:
subframe_1_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~subframe_1_t();
private:
bool f_af_0;
int32_t m_af_0;
public:
int32_t af_0();
private:
uint64_t m_week_no;
uint64_t m_code;
uint64_t m_sv_accuracy;
uint64_t m_sv_health;
uint64_t m_iodc_msb;
bool m_l2_p_data_flag;
uint64_t m_reserved1;
uint64_t m_reserved2;
uint64_t m_reserved3;
uint64_t m_reserved4;
int8_t m_t_gd;
uint8_t m_iodc_lsb;
uint16_t m_t_oc;
int8_t m_af_2;
int16_t m_af_1;
bool m_af_0_sign;
uint64_t m_af_0_value;
uint64_t m_reserved5;
gps_t* m__root;
gps_t* m__parent;
public:
uint64_t week_no() const { return m_week_no; }
uint64_t code() const { return m_code; }
uint64_t sv_accuracy() const { return m_sv_accuracy; }
uint64_t sv_health() const { return m_sv_health; }
uint64_t iodc_msb() const { return m_iodc_msb; }
bool l2_p_data_flag() const { return m_l2_p_data_flag; }
uint64_t reserved1() const { return m_reserved1; }
uint64_t reserved2() const { return m_reserved2; }
uint64_t reserved3() const { return m_reserved3; }
uint64_t reserved4() const { return m_reserved4; }
int8_t t_gd() const { return m_t_gd; }
uint8_t iodc_lsb() const { return m_iodc_lsb; }
uint16_t t_oc() const { return m_t_oc; }
int8_t af_2() const { return m_af_2; }
int16_t af_1() const { return m_af_1; }
bool af_0_sign() const { return m_af_0_sign; }
uint64_t af_0_value() const { return m_af_0_value; }
uint64_t reserved5() const { return m_reserved5; }
gps_t* _root() const { return m__root; }
gps_t* _parent() const { return m__parent; }
};
class subframe_3_t : public kaitai::kstruct {
public:
subframe_3_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~subframe_3_t();
private:
bool f_omega_dot;
int32_t m_omega_dot;
public:
int32_t omega_dot();
private:
bool f_idot;
int32_t m_idot;
public:
int32_t idot();
private:
int16_t m_c_ic;
int32_t m_omega_0;
int16_t m_c_is;
int32_t m_i_0;
int16_t m_c_rc;
int32_t m_omega;
bool m_omega_dot_sign;
uint64_t m_omega_dot_value;
uint8_t m_iode;
bool m_idot_sign;
uint64_t m_idot_value;
uint64_t m_reserved;
gps_t* m__root;
gps_t* m__parent;
public:
int16_t c_ic() const { return m_c_ic; }
int32_t omega_0() const { return m_omega_0; }
int16_t c_is() const { return m_c_is; }
int32_t i_0() const { return m_i_0; }
int16_t c_rc() const { return m_c_rc; }
int32_t omega() const { return m_omega; }
bool omega_dot_sign() const { return m_omega_dot_sign; }
uint64_t omega_dot_value() const { return m_omega_dot_value; }
uint8_t iode() const { return m_iode; }
bool idot_sign() const { return m_idot_sign; }
uint64_t idot_value() const { return m_idot_value; }
uint64_t reserved() const { return m_reserved; }
gps_t* _root() const { return m__root; }
gps_t* _parent() const { return m__parent; }
};
class subframe_4_t : public kaitai::kstruct {
public:
class ionosphere_data_t;
subframe_4_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~subframe_4_t();
class ionosphere_data_t : public kaitai::kstruct {
public:
ionosphere_data_t(kaitai::kstream* p__io, gps_t::subframe_4_t* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~ionosphere_data_t();
private:
int8_t m_a0;
int8_t m_a1;
int8_t m_a2;
int8_t m_a3;
int8_t m_b0;
int8_t m_b1;
int8_t m_b2;
int8_t m_b3;
gps_t* m__root;
gps_t::subframe_4_t* m__parent;
public:
int8_t a0() const { return m_a0; }
int8_t a1() const { return m_a1; }
int8_t a2() const { return m_a2; }
int8_t a3() const { return m_a3; }
int8_t b0() const { return m_b0; }
int8_t b1() const { return m_b1; }
int8_t b2() const { return m_b2; }
int8_t b3() const { return m_b3; }
gps_t* _root() const { return m__root; }
gps_t::subframe_4_t* _parent() const { return m__parent; }
};
private:
uint64_t m_data_id;
uint64_t m_page_id;
ionosphere_data_t* m_body;
bool n_body;
public:
bool _is_null_body() { body(); return n_body; };
private:
gps_t* m__root;
gps_t* m__parent;
public:
uint64_t data_id() const { return m_data_id; }
uint64_t page_id() const { return m_page_id; }
ionosphere_data_t* body() const { return m_body; }
gps_t* _root() const { return m__root; }
gps_t* _parent() const { return m__parent; }
};
class how_t : public kaitai::kstruct {
public:
how_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~how_t();
private:
uint64_t m_tow_count;
bool m_alert;
bool m_anti_spoof;
uint64_t m_subframe_id;
uint64_t m_reserved;
gps_t* m__root;
gps_t* m__parent;
public:
uint64_t tow_count() const { return m_tow_count; }
bool alert() const { return m_alert; }
bool anti_spoof() const { return m_anti_spoof; }
uint64_t subframe_id() const { return m_subframe_id; }
uint64_t reserved() const { return m_reserved; }
gps_t* _root() const { return m__root; }
gps_t* _parent() const { return m__parent; }
};
class tlm_t : public kaitai::kstruct {
public:
tlm_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~tlm_t();
private:
std::string m_preamble;
uint64_t m_tlm;
bool m_integrity_status;
bool m_reserved;
gps_t* m__root;
gps_t* m__parent;
public:
std::string preamble() const { return m_preamble; }
uint64_t tlm() const { return m_tlm; }
bool integrity_status() const { return m_integrity_status; }
bool reserved() const { return m_reserved; }
gps_t* _root() const { return m__root; }
gps_t* _parent() const { return m__parent; }
};
class subframe_2_t : public kaitai::kstruct {
public:
subframe_2_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~subframe_2_t();
private:
uint8_t m_iode;
int16_t m_c_rs;
int16_t m_delta_n;
int32_t m_m_0;
int16_t m_c_uc;
int32_t m_e;
int16_t m_c_us;
uint32_t m_sqrt_a;
uint16_t m_t_oe;
bool m_fit_interval_flag;
uint64_t m_aoda;
uint64_t m_reserved;
gps_t* m__root;
gps_t* m__parent;
public:
uint8_t iode() const { return m_iode; }
int16_t c_rs() const { return m_c_rs; }
int16_t delta_n() const { return m_delta_n; }
int32_t m_0() const { return m_m_0; }
int16_t c_uc() const { return m_c_uc; }
int32_t e() const { return m_e; }
int16_t c_us() const { return m_c_us; }
uint32_t sqrt_a() const { return m_sqrt_a; }
uint16_t t_oe() const { return m_t_oe; }
bool fit_interval_flag() const { return m_fit_interval_flag; }
uint64_t aoda() const { return m_aoda; }
uint64_t reserved() const { return m_reserved; }
gps_t* _root() const { return m__root; }
gps_t* _parent() const { return m__parent; }
};
private:
tlm_t* m_tlm;
how_t* m_how;
kaitai::kstruct* m_body;
bool n_body;
public:
bool _is_null_body() { body(); return n_body; };
private:
gps_t* m__root;
kaitai::kstruct* m__parent;
public:
tlm_t* tlm() const { return m_tlm; }
how_t* how() const { return m_how; }
kaitai::kstruct* body() const { return m_body; }
gps_t* _root() const { return m__root; }
kaitai::kstruct* _parent() const { return m__parent; }
};
#endif // GPS_H_
system/ubloxd/generated/gps.py
+193
View File
@@ -0,0 +1,193 @@
# This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
import kaitaistruct
from kaitaistruct import KaitaiStruct, KaitaiStream, BytesIO
if getattr(kaitaistruct, 'API_VERSION', (0, 9)) < (0, 9):
raise Exception("Incompatible Kaitai Struct Python API: 0.9 or later is required, but you have %s" % (kaitaistruct.__version__))
class Gps(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.tlm = Gps.Tlm(self._io, self, self._root)
self.how = Gps.How(self._io, self, self._root)
_on = self.how.subframe_id
if _on == 1:
self.body = Gps.Subframe1(self._io, self, self._root)
elif _on == 2:
self.body = Gps.Subframe2(self._io, self, self._root)
elif _on == 3:
self.body = Gps.Subframe3(self._io, self, self._root)
elif _on == 4:
self.body = Gps.Subframe4(self._io, self, self._root)
class Subframe1(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.week_no = self._io.read_bits_int_be(10)
self.code = self._io.read_bits_int_be(2)
self.sv_accuracy = self._io.read_bits_int_be(4)
self.sv_health = self._io.read_bits_int_be(6)
self.iodc_msb = self._io.read_bits_int_be(2)
self.l2_p_data_flag = self._io.read_bits_int_be(1) != 0
self.reserved1 = self._io.read_bits_int_be(23)
self.reserved2 = self._io.read_bits_int_be(24)
self.reserved3 = self._io.read_bits_int_be(24)
self.reserved4 = self._io.read_bits_int_be(16)
self._io.align_to_byte()
self.t_gd = self._io.read_s1()
self.iodc_lsb = self._io.read_u1()
self.t_oc = self._io.read_u2be()
self.af_2 = self._io.read_s1()
self.af_1 = self._io.read_s2be()
self.af_0_sign = self._io.read_bits_int_be(1) != 0
self.af_0_value = self._io.read_bits_int_be(21)
self.reserved5 = self._io.read_bits_int_be(2)
@property
def af_0(self):
if hasattr(self, '_m_af_0'):
return self._m_af_0
self._m_af_0 = ((self.af_0_value - (1 << 21)) if self.af_0_sign else self.af_0_value)
return getattr(self, '_m_af_0', None)
class Subframe3(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.c_ic = self._io.read_s2be()
self.omega_0 = self._io.read_s4be()
self.c_is = self._io.read_s2be()
self.i_0 = self._io.read_s4be()
self.c_rc = self._io.read_s2be()
self.omega = self._io.read_s4be()
self.omega_dot_sign = self._io.read_bits_int_be(1) != 0
self.omega_dot_value = self._io.read_bits_int_be(23)
self._io.align_to_byte()
self.iode = self._io.read_u1()
self.idot_sign = self._io.read_bits_int_be(1) != 0
self.idot_value = self._io.read_bits_int_be(13)
self.reserved = self._io.read_bits_int_be(2)
@property
def omega_dot(self):
if hasattr(self, '_m_omega_dot'):
return self._m_omega_dot
self._m_omega_dot = ((self.omega_dot_value - (1 << 23)) if self.omega_dot_sign else self.omega_dot_value)
return getattr(self, '_m_omega_dot', None)
@property
def idot(self):
if hasattr(self, '_m_idot'):
return self._m_idot
self._m_idot = ((self.idot_value - (1 << 13)) if self.idot_sign else self.idot_value)
return getattr(self, '_m_idot', None)
class Subframe4(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.data_id = self._io.read_bits_int_be(2)
self.page_id = self._io.read_bits_int_be(6)
self._io.align_to_byte()
_on = self.page_id
if _on == 56:
self.body = Gps.Subframe4.IonosphereData(self._io, self, self._root)
class IonosphereData(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.a0 = self._io.read_s1()
self.a1 = self._io.read_s1()
self.a2 = self._io.read_s1()
self.a3 = self._io.read_s1()
self.b0 = self._io.read_s1()
self.b1 = self._io.read_s1()
self.b2 = self._io.read_s1()
self.b3 = self._io.read_s1()
class How(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.tow_count = self._io.read_bits_int_be(17)
self.alert = self._io.read_bits_int_be(1) != 0
self.anti_spoof = self._io.read_bits_int_be(1) != 0
self.subframe_id = self._io.read_bits_int_be(3)
self.reserved = self._io.read_bits_int_be(2)
class Tlm(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.preamble = self._io.read_bytes(1)
if not self.preamble == b"\x8B":
raise kaitaistruct.ValidationNotEqualError(b"\x8B", self.preamble, self._io, u"/types/tlm/seq/0")
self.tlm = self._io.read_bits_int_be(14)
self.integrity_status = self._io.read_bits_int_be(1) != 0
self.reserved = self._io.read_bits_int_be(1) != 0
class Subframe2(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.iode = self._io.read_u1()
self.c_rs = self._io.read_s2be()
self.delta_n = self._io.read_s2be()
self.m_0 = self._io.read_s4be()
self.c_uc = self._io.read_s2be()
self.e = self._io.read_s4be()
self.c_us = self._io.read_s2be()
self.sqrt_a = self._io.read_u4be()
self.t_oe = self._io.read_u2be()
self.fit_interval_flag = self._io.read_bits_int_be(1) != 0
self.aoda = self._io.read_bits_int_be(5)
self.reserved = self._io.read_bits_int_be(2)
system/ubloxd/generated/ubx.cpp
-424
View File
@@ -1,424 +0,0 @@
// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
#include "ubx.h"
#include "kaitai/exceptions.h"
ubx_t::ubx_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = this;
f_checksum = false;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void ubx_t::_read() {
m_magic = m__io->read_bytes(2);
if (!(magic() == std::string("\xB5\x62", 2))) {
throw kaitai::validation_not_equal_error<std::string>(std::string("\xB5\x62", 2), magic(), _io(), std::string("/seq/0"));
}
m_msg_type = m__io->read_u2be();
m_length = m__io->read_u2le();
n_body = true;
switch (msg_type()) {
case 2569: {
n_body = false;
m_body = new mon_hw_t(m__io, this, m__root);
break;
}
case 533: {
n_body = false;
m_body = new rxm_rawx_t(m__io, this, m__root);
break;
}
case 531: {
n_body = false;
m_body = new rxm_sfrbx_t(m__io, this, m__root);
break;
}
case 309: {
n_body = false;
m_body = new nav_sat_t(m__io, this, m__root);
break;
}
case 2571: {
n_body = false;
m_body = new mon_hw2_t(m__io, this, m__root);
break;
}
case 263: {
n_body = false;
m_body = new nav_pvt_t(m__io, this, m__root);
break;
}
}
}
ubx_t::~ubx_t() {
_clean_up();
}
void ubx_t::_clean_up() {
if (!n_body) {
if (m_body) {
delete m_body; m_body = 0;
}
}
if (f_checksum) {
}
}
ubx_t::rxm_rawx_t::rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
m_meas = 0;
m__raw_meas = 0;
m__io__raw_meas = 0;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void ubx_t::rxm_rawx_t::_read() {
m_rcv_tow = m__io->read_f8le();
m_week = m__io->read_u2le();
m_leap_s = m__io->read_s1();
m_num_meas = m__io->read_u1();
m_rec_stat = m__io->read_u1();
m_reserved1 = m__io->read_bytes(3);
m__raw_meas = new std::vector<std::string>();
m__io__raw_meas = new std::vector<kaitai::kstream*>();
m_meas = new std::vector<measurement_t*>();
const int l_meas = num_meas();
for (int i = 0; i < l_meas; i++) {
m__raw_meas->push_back(m__io->read_bytes(32));
kaitai::kstream* io__raw_meas = new kaitai::kstream(m__raw_meas->at(m__raw_meas->size() - 1));
m__io__raw_meas->push_back(io__raw_meas);
m_meas->push_back(new measurement_t(io__raw_meas, this, m__root));
}
}
ubx_t::rxm_rawx_t::~rxm_rawx_t() {
_clean_up();
}
void ubx_t::rxm_rawx_t::_clean_up() {
if (m__raw_meas) {
delete m__raw_meas; m__raw_meas = 0;
}
if (m__io__raw_meas) {
for (std::vector<kaitai::kstream*>::iterator it = m__io__raw_meas->begin(); it != m__io__raw_meas->end(); ++it) {
delete *it;
}
delete m__io__raw_meas; m__io__raw_meas = 0;
}
if (m_meas) {
for (std::vector<measurement_t*>::iterator it = m_meas->begin(); it != m_meas->end(); ++it) {
delete *it;
}
delete m_meas; m_meas = 0;
}
}
ubx_t::rxm_rawx_t::measurement_t::measurement_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void ubx_t::rxm_rawx_t::measurement_t::_read() {
m_pr_mes = m__io->read_f8le();
m_cp_mes = m__io->read_f8le();
m_do_mes = m__io->read_f4le();
m_gnss_id = static_cast<ubx_t::gnss_type_t>(m__io->read_u1());
m_sv_id = m__io->read_u1();
m_reserved2 = m__io->read_bytes(1);
m_freq_id = m__io->read_u1();
m_lock_time = m__io->read_u2le();
m_cno = m__io->read_u1();
m_pr_stdev = m__io->read_u1();
m_cp_stdev = m__io->read_u1();
m_do_stdev = m__io->read_u1();
m_trk_stat = m__io->read_u1();
m_reserved3 = m__io->read_bytes(1);
}
ubx_t::rxm_rawx_t::measurement_t::~measurement_t() {
_clean_up();
}
void ubx_t::rxm_rawx_t::measurement_t::_clean_up() {
}
ubx_t::rxm_sfrbx_t::rxm_sfrbx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
m_body = 0;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void ubx_t::rxm_sfrbx_t::_read() {
m_gnss_id = static_cast<ubx_t::gnss_type_t>(m__io->read_u1());
m_sv_id = m__io->read_u1();
m_reserved1 = m__io->read_bytes(1);
m_freq_id = m__io->read_u1();
m_num_words = m__io->read_u1();
m_reserved2 = m__io->read_bytes(1);
m_version = m__io->read_u1();
m_reserved3 = m__io->read_bytes(1);
m_body = new std::vector<uint32_t>();
const int l_body = num_words();
for (int i = 0; i < l_body; i++) {
m_body->push_back(m__io->read_u4le());
}
}
ubx_t::rxm_sfrbx_t::~rxm_sfrbx_t() {
_clean_up();
}
void ubx_t::rxm_sfrbx_t::_clean_up() {
if (m_body) {
delete m_body; m_body = 0;
}
}
ubx_t::nav_sat_t::nav_sat_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
m_svs = 0;
m__raw_svs = 0;
m__io__raw_svs = 0;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void ubx_t::nav_sat_t::_read() {
m_itow = m__io->read_u4le();
m_version = m__io->read_u1();
m_num_svs = m__io->read_u1();
m_reserved = m__io->read_bytes(2);
m__raw_svs = new std::vector<std::string>();
m__io__raw_svs = new std::vector<kaitai::kstream*>();
m_svs = new std::vector<nav_t*>();
const int l_svs = num_svs();
for (int i = 0; i < l_svs; i++) {
m__raw_svs->push_back(m__io->read_bytes(12));
kaitai::kstream* io__raw_svs = new kaitai::kstream(m__raw_svs->at(m__raw_svs->size() - 1));
m__io__raw_svs->push_back(io__raw_svs);
m_svs->push_back(new nav_t(io__raw_svs, this, m__root));
}
}
ubx_t::nav_sat_t::~nav_sat_t() {
_clean_up();
}
void ubx_t::nav_sat_t::_clean_up() {
if (m__raw_svs) {
delete m__raw_svs; m__raw_svs = 0;
}
if (m__io__raw_svs) {
for (std::vector<kaitai::kstream*>::iterator it = m__io__raw_svs->begin(); it != m__io__raw_svs->end(); ++it) {
delete *it;
}
delete m__io__raw_svs; m__io__raw_svs = 0;
}
if (m_svs) {
for (std::vector<nav_t*>::iterator it = m_svs->begin(); it != m_svs->end(); ++it) {
delete *it;
}
delete m_svs; m_svs = 0;
}
}
ubx_t::nav_sat_t::nav_t::nav_t(kaitai::kstream* p__io, ubx_t::nav_sat_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void ubx_t::nav_sat_t::nav_t::_read() {
m_gnss_id = static_cast<ubx_t::gnss_type_t>(m__io->read_u1());
m_sv_id = m__io->read_u1();
m_cno = m__io->read_u1();
m_elev = m__io->read_s1();
m_azim = m__io->read_s2le();
m_pr_res = m__io->read_s2le();
m_flags = m__io->read_u4le();
}
ubx_t::nav_sat_t::nav_t::~nav_t() {
_clean_up();
}
void ubx_t::nav_sat_t::nav_t::_clean_up() {
}
ubx_t::nav_pvt_t::nav_pvt_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void ubx_t::nav_pvt_t::_read() {
m_i_tow = m__io->read_u4le();
m_year = m__io->read_u2le();
m_month = m__io->read_u1();
m_day = m__io->read_u1();
m_hour = m__io->read_u1();
m_min = m__io->read_u1();
m_sec = m__io->read_u1();
m_valid = m__io->read_u1();
m_t_acc = m__io->read_u4le();
m_nano = m__io->read_s4le();
m_fix_type = m__io->read_u1();
m_flags = m__io->read_u1();
m_flags2 = m__io->read_u1();
m_num_sv = m__io->read_u1();
m_lon = m__io->read_s4le();
m_lat = m__io->read_s4le();
m_height = m__io->read_s4le();
m_h_msl = m__io->read_s4le();
m_h_acc = m__io->read_u4le();
m_v_acc = m__io->read_u4le();
m_vel_n = m__io->read_s4le();
m_vel_e = m__io->read_s4le();
m_vel_d = m__io->read_s4le();
m_g_speed = m__io->read_s4le();
m_head_mot = m__io->read_s4le();
m_s_acc = m__io->read_s4le();
m_head_acc = m__io->read_u4le();
m_p_dop = m__io->read_u2le();
m_flags3 = m__io->read_u1();
m_reserved1 = m__io->read_bytes(5);
m_head_veh = m__io->read_s4le();
m_mag_dec = m__io->read_s2le();
m_mag_acc = m__io->read_u2le();
}
ubx_t::nav_pvt_t::~nav_pvt_t() {
_clean_up();
}
void ubx_t::nav_pvt_t::_clean_up() {
}
ubx_t::mon_hw2_t::mon_hw2_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void ubx_t::mon_hw2_t::_read() {
m_ofs_i = m__io->read_s1();
m_mag_i = m__io->read_u1();
m_ofs_q = m__io->read_s1();
m_mag_q = m__io->read_u1();
m_cfg_source = static_cast<ubx_t::mon_hw2_t::config_source_t>(m__io->read_u1());
m_reserved1 = m__io->read_bytes(3);
m_low_lev_cfg = m__io->read_u4le();
m_reserved2 = m__io->read_bytes(8);
m_post_status = m__io->read_u4le();
m_reserved3 = m__io->read_bytes(4);
}
ubx_t::mon_hw2_t::~mon_hw2_t() {
_clean_up();
}
void ubx_t::mon_hw2_t::_clean_up() {
}
ubx_t::mon_hw_t::mon_hw_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
m__parent = p__parent;
m__root = p__root;
try {
_read();
} catch(...) {
_clean_up();
throw;
}
}
void ubx_t::mon_hw_t::_read() {
m_pin_sel = m__io->read_u4le();
m_pin_bank = m__io->read_u4le();
m_pin_dir = m__io->read_u4le();
m_pin_val = m__io->read_u4le();
m_noise_per_ms = m__io->read_u2le();
m_agc_cnt = m__io->read_u2le();
m_a_status = static_cast<ubx_t::mon_hw_t::antenna_status_t>(m__io->read_u1());
m_a_power = static_cast<ubx_t::mon_hw_t::antenna_power_t>(m__io->read_u1());
m_flags = m__io->read_u1();
m_reserved1 = m__io->read_bytes(1);
m_used_mask = m__io->read_u4le();
m_vp = m__io->read_bytes(17);
m_jam_ind = m__io->read_u1();
m_reserved2 = m__io->read_bytes(2);
m_pin_irq = m__io->read_u4le();
m_pull_h = m__io->read_u4le();
m_pull_l = m__io->read_u4le();
}
ubx_t::mon_hw_t::~mon_hw_t() {
_clean_up();
}
void ubx_t::mon_hw_t::_clean_up() {
}
uint16_t ubx_t::checksum() {
if (f_checksum)
return m_checksum;
std::streampos _pos = m__io->pos();
m__io->seek((length() + 6));
m_checksum = m__io->read_u2le();
m__io->seek(_pos);
f_checksum = true;
return m_checksum;
}
system/ubloxd/generated/ubx.h
-484
View File
@@ -1,484 +0,0 @@
#ifndef UBX_H_
#define UBX_H_
// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
#include "kaitai/kaitaistruct.h"
#include <stdint.h>
#include <vector>
#if KAITAI_STRUCT_VERSION < 9000L
#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required"
#endif
class ubx_t : public kaitai::kstruct {
public:
class rxm_rawx_t;
class rxm_sfrbx_t;
class nav_sat_t;
class nav_pvt_t;
class mon_hw2_t;
class mon_hw_t;
enum gnss_type_t {
GNSS_TYPE_GPS = 0,
GNSS_TYPE_SBAS = 1,
GNSS_TYPE_GALILEO = 2,
GNSS_TYPE_BEIDOU = 3,
GNSS_TYPE_IMES = 4,
GNSS_TYPE_QZSS = 5,
GNSS_TYPE_GLONASS = 6
};
ubx_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~ubx_t();
class rxm_rawx_t : public kaitai::kstruct {
public:
class measurement_t;
rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~rxm_rawx_t();
class measurement_t : public kaitai::kstruct {
public:
measurement_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~measurement_t();
private:
double m_pr_mes;
double m_cp_mes;
float m_do_mes;
gnss_type_t m_gnss_id;
uint8_t m_sv_id;
std::string m_reserved2;
uint8_t m_freq_id;
uint16_t m_lock_time;
uint8_t m_cno;
uint8_t m_pr_stdev;
uint8_t m_cp_stdev;
uint8_t m_do_stdev;
uint8_t m_trk_stat;
std::string m_reserved3;
ubx_t* m__root;
ubx_t::rxm_rawx_t* m__parent;
public:
double pr_mes() const { return m_pr_mes; }
double cp_mes() const { return m_cp_mes; }
float do_mes() const { return m_do_mes; }
gnss_type_t gnss_id() const { return m_gnss_id; }
uint8_t sv_id() const { return m_sv_id; }
std::string reserved2() const { return m_reserved2; }
uint8_t freq_id() const { return m_freq_id; }
uint16_t lock_time() const { return m_lock_time; }
uint8_t cno() const { return m_cno; }
uint8_t pr_stdev() const { return m_pr_stdev; }
uint8_t cp_stdev() const { return m_cp_stdev; }
uint8_t do_stdev() const { return m_do_stdev; }
uint8_t trk_stat() const { return m_trk_stat; }
std::string reserved3() const { return m_reserved3; }
ubx_t* _root() const { return m__root; }
ubx_t::rxm_rawx_t* _parent() const { return m__parent; }
};
private:
double m_rcv_tow;
uint16_t m_week;
int8_t m_leap_s;
uint8_t m_num_meas;
uint8_t m_rec_stat;
std::string m_reserved1;
std::vector<measurement_t*>* m_meas;
ubx_t* m__root;
ubx_t* m__parent;
std::vector<std::string>* m__raw_meas;
std::vector<kaitai::kstream*>* m__io__raw_meas;
public:
double rcv_tow() const { return m_rcv_tow; }
uint16_t week() const { return m_week; }
int8_t leap_s() const { return m_leap_s; }
uint8_t num_meas() const { return m_num_meas; }
uint8_t rec_stat() const { return m_rec_stat; }
std::string reserved1() const { return m_reserved1; }
std::vector<measurement_t*>* meas() const { return m_meas; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
std::vector<std::string>* _raw_meas() const { return m__raw_meas; }
std::vector<kaitai::kstream*>* _io__raw_meas() const { return m__io__raw_meas; }
};
class rxm_sfrbx_t : public kaitai::kstruct {
public:
rxm_sfrbx_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~rxm_sfrbx_t();
private:
gnss_type_t m_gnss_id;
uint8_t m_sv_id;
std::string m_reserved1;
uint8_t m_freq_id;
uint8_t m_num_words;
std::string m_reserved2;
uint8_t m_version;
std::string m_reserved3;
std::vector<uint32_t>* m_body;
ubx_t* m__root;
ubx_t* m__parent;
public:
gnss_type_t gnss_id() const { return m_gnss_id; }
uint8_t sv_id() const { return m_sv_id; }
std::string reserved1() const { return m_reserved1; }
uint8_t freq_id() const { return m_freq_id; }
uint8_t num_words() const { return m_num_words; }
std::string reserved2() const { return m_reserved2; }
uint8_t version() const { return m_version; }
std::string reserved3() const { return m_reserved3; }
std::vector<uint32_t>* body() const { return m_body; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
};
class nav_sat_t : public kaitai::kstruct {
public:
class nav_t;
nav_sat_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~nav_sat_t();
class nav_t : public kaitai::kstruct {
public:
nav_t(kaitai::kstream* p__io, ubx_t::nav_sat_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~nav_t();
private:
gnss_type_t m_gnss_id;
uint8_t m_sv_id;
uint8_t m_cno;
int8_t m_elev;
int16_t m_azim;
int16_t m_pr_res;
uint32_t m_flags;
ubx_t* m__root;
ubx_t::nav_sat_t* m__parent;
public:
gnss_type_t gnss_id() const { return m_gnss_id; }
uint8_t sv_id() const { return m_sv_id; }
uint8_t cno() const { return m_cno; }
int8_t elev() const { return m_elev; }
int16_t azim() const { return m_azim; }
int16_t pr_res() const { return m_pr_res; }
uint32_t flags() const { return m_flags; }
ubx_t* _root() const { return m__root; }
ubx_t::nav_sat_t* _parent() const { return m__parent; }
};
private:
uint32_t m_itow;
uint8_t m_version;
uint8_t m_num_svs;
std::string m_reserved;
std::vector<nav_t*>* m_svs;
ubx_t* m__root;
ubx_t* m__parent;
std::vector<std::string>* m__raw_svs;
std::vector<kaitai::kstream*>* m__io__raw_svs;
public:
uint32_t itow() const { return m_itow; }
uint8_t version() const { return m_version; }
uint8_t num_svs() const { return m_num_svs; }
std::string reserved() const { return m_reserved; }
std::vector<nav_t*>* svs() const { return m_svs; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
std::vector<std::string>* _raw_svs() const { return m__raw_svs; }
std::vector<kaitai::kstream*>* _io__raw_svs() const { return m__io__raw_svs; }
};
class nav_pvt_t : public kaitai::kstruct {
public:
nav_pvt_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~nav_pvt_t();
private:
uint32_t m_i_tow;
uint16_t m_year;
uint8_t m_month;
uint8_t m_day;
uint8_t m_hour;
uint8_t m_min;
uint8_t m_sec;
uint8_t m_valid;
uint32_t m_t_acc;
int32_t m_nano;
uint8_t m_fix_type;
uint8_t m_flags;
uint8_t m_flags2;
uint8_t m_num_sv;
int32_t m_lon;
int32_t m_lat;
int32_t m_height;
int32_t m_h_msl;
uint32_t m_h_acc;
uint32_t m_v_acc;
int32_t m_vel_n;
int32_t m_vel_e;
int32_t m_vel_d;
int32_t m_g_speed;
int32_t m_head_mot;
int32_t m_s_acc;
uint32_t m_head_acc;
uint16_t m_p_dop;
uint8_t m_flags3;
std::string m_reserved1;
int32_t m_head_veh;
int16_t m_mag_dec;
uint16_t m_mag_acc;
ubx_t* m__root;
ubx_t* m__parent;
public:
uint32_t i_tow() const { return m_i_tow; }
uint16_t year() const { return m_year; }
uint8_t month() const { return m_month; }
uint8_t day() const { return m_day; }
uint8_t hour() const { return m_hour; }
uint8_t min() const { return m_min; }
uint8_t sec() const { return m_sec; }
uint8_t valid() const { return m_valid; }
uint32_t t_acc() const { return m_t_acc; }
int32_t nano() const { return m_nano; }
uint8_t fix_type() const { return m_fix_type; }
uint8_t flags() const { return m_flags; }
uint8_t flags2() const { return m_flags2; }
uint8_t num_sv() const { return m_num_sv; }
int32_t lon() const { return m_lon; }
int32_t lat() const { return m_lat; }
int32_t height() const { return m_height; }
int32_t h_msl() const { return m_h_msl; }
uint32_t h_acc() const { return m_h_acc; }
uint32_t v_acc() const { return m_v_acc; }
int32_t vel_n() const { return m_vel_n; }
int32_t vel_e() const { return m_vel_e; }
int32_t vel_d() const { return m_vel_d; }
int32_t g_speed() const { return m_g_speed; }
int32_t head_mot() const { return m_head_mot; }
int32_t s_acc() const { return m_s_acc; }
uint32_t head_acc() const { return m_head_acc; }
uint16_t p_dop() const { return m_p_dop; }
uint8_t flags3() const { return m_flags3; }
std::string reserved1() const { return m_reserved1; }
int32_t head_veh() const { return m_head_veh; }
int16_t mag_dec() const { return m_mag_dec; }
uint16_t mag_acc() const { return m_mag_acc; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
};
class mon_hw2_t : public kaitai::kstruct {
public:
enum config_source_t {
CONFIG_SOURCE_FLASH = 102,
CONFIG_SOURCE_OTP = 111,
CONFIG_SOURCE_CONFIG_PINS = 112,
CONFIG_SOURCE_ROM = 113
};
mon_hw2_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~mon_hw2_t();
private:
int8_t m_ofs_i;
uint8_t m_mag_i;
int8_t m_ofs_q;
uint8_t m_mag_q;
config_source_t m_cfg_source;
std::string m_reserved1;
uint32_t m_low_lev_cfg;
std::string m_reserved2;
uint32_t m_post_status;
std::string m_reserved3;
ubx_t* m__root;
ubx_t* m__parent;
public:
int8_t ofs_i() const { return m_ofs_i; }
uint8_t mag_i() const { return m_mag_i; }
int8_t ofs_q() const { return m_ofs_q; }
uint8_t mag_q() const { return m_mag_q; }
config_source_t cfg_source() const { return m_cfg_source; }
std::string reserved1() const { return m_reserved1; }
uint32_t low_lev_cfg() const { return m_low_lev_cfg; }
std::string reserved2() const { return m_reserved2; }
uint32_t post_status() const { return m_post_status; }
std::string reserved3() const { return m_reserved3; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
};
class mon_hw_t : public kaitai::kstruct {
public:
enum antenna_status_t {
ANTENNA_STATUS_INIT = 0,
ANTENNA_STATUS_DONTKNOW = 1,
ANTENNA_STATUS_OK = 2,
ANTENNA_STATUS_SHORT = 3,
ANTENNA_STATUS_OPEN = 4
};
enum antenna_power_t {
ANTENNA_POWER_FALSE = 0,
ANTENNA_POWER_TRUE = 1,
ANTENNA_POWER_DONTKNOW = 2
};
mon_hw_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~mon_hw_t();
private:
uint32_t m_pin_sel;
uint32_t m_pin_bank;
uint32_t m_pin_dir;
uint32_t m_pin_val;
uint16_t m_noise_per_ms;
uint16_t m_agc_cnt;
antenna_status_t m_a_status;
antenna_power_t m_a_power;
uint8_t m_flags;
std::string m_reserved1;
uint32_t m_used_mask;
std::string m_vp;
uint8_t m_jam_ind;
std::string m_reserved2;
uint32_t m_pin_irq;
uint32_t m_pull_h;
uint32_t m_pull_l;
ubx_t* m__root;
ubx_t* m__parent;
public:
uint32_t pin_sel() const { return m_pin_sel; }
uint32_t pin_bank() const { return m_pin_bank; }
uint32_t pin_dir() const { return m_pin_dir; }
uint32_t pin_val() const { return m_pin_val; }
uint16_t noise_per_ms() const { return m_noise_per_ms; }
uint16_t agc_cnt() const { return m_agc_cnt; }
antenna_status_t a_status() const { return m_a_status; }
antenna_power_t a_power() const { return m_a_power; }
uint8_t flags() const { return m_flags; }
std::string reserved1() const { return m_reserved1; }
uint32_t used_mask() const { return m_used_mask; }
std::string vp() const { return m_vp; }
uint8_t jam_ind() const { return m_jam_ind; }
std::string reserved2() const { return m_reserved2; }
uint32_t pin_irq() const { return m_pin_irq; }
uint32_t pull_h() const { return m_pull_h; }
uint32_t pull_l() const { return m_pull_l; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
};
private:
bool f_checksum;
uint16_t m_checksum;
public:
uint16_t checksum();
private:
std::string m_magic;
uint16_t m_msg_type;
uint16_t m_length;
kaitai::kstruct* m_body;
bool n_body;
public:
bool _is_null_body() { body(); return n_body; };
private:
ubx_t* m__root;
kaitai::kstruct* m__parent;
public:
std::string magic() const { return m_magic; }
uint16_t msg_type() const { return m_msg_type; }
uint16_t length() const { return m_length; }
kaitai::kstruct* body() const { return m_body; }
ubx_t* _root() const { return m__root; }
kaitai::kstruct* _parent() const { return m__parent; }
};
#endif // UBX_H_
system/ubloxd/generated/ubx.py
+273
View File
@@ -0,0 +1,273 @@
# This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
import kaitaistruct
from kaitaistruct import KaitaiStruct, KaitaiStream, BytesIO
from enum import Enum
if getattr(kaitaistruct, 'API_VERSION', (0, 9)) < (0, 9):
raise Exception("Incompatible Kaitai Struct Python API: 0.9 or later is required, but you have %s" % (kaitaistruct.__version__))
class Ubx(KaitaiStruct):
class GnssType(Enum):
gps = 0
sbas = 1
galileo = 2
beidou = 3
imes = 4
qzss = 5
glonass = 6
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.magic = self._io.read_bytes(2)
if not self.magic == b"\xB5\x62":
raise kaitaistruct.ValidationNotEqualError(b"\xB5\x62", self.magic, self._io, u"/seq/0")
self.msg_type = self._io.read_u2be()
self.length = self._io.read_u2le()
_on = self.msg_type
if _on == 2569:
self.body = Ubx.MonHw(self._io, self, self._root)
elif _on == 533:
self.body = Ubx.RxmRawx(self._io, self, self._root)
elif _on == 531:
self.body = Ubx.RxmSfrbx(self._io, self, self._root)
elif _on == 309:
self.body = Ubx.NavSat(self._io, self, self._root)
elif _on == 2571:
self.body = Ubx.MonHw2(self._io, self, self._root)
elif _on == 263:
self.body = Ubx.NavPvt(self._io, self, self._root)
class RxmRawx(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.rcv_tow = self._io.read_f8le()
self.week = self._io.read_u2le()
self.leap_s = self._io.read_s1()
self.num_meas = self._io.read_u1()
self.rec_stat = self._io.read_u1()
self.reserved1 = self._io.read_bytes(3)
self._raw_meas = []
self.meas = []
for i in range(self.num_meas):
self._raw_meas.append(self._io.read_bytes(32))
_io__raw_meas = KaitaiStream(BytesIO(self._raw_meas[i]))
self.meas.append(Ubx.RxmRawx.Measurement(_io__raw_meas, self, self._root))
class Measurement(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.pr_mes = self._io.read_f8le()
self.cp_mes = self._io.read_f8le()
self.do_mes = self._io.read_f4le()
self.gnss_id = KaitaiStream.resolve_enum(Ubx.GnssType, self._io.read_u1())
self.sv_id = self._io.read_u1()
self.reserved2 = self._io.read_bytes(1)
self.freq_id = self._io.read_u1()
self.lock_time = self._io.read_u2le()
self.cno = self._io.read_u1()
self.pr_stdev = self._io.read_u1()
self.cp_stdev = self._io.read_u1()
self.do_stdev = self._io.read_u1()
self.trk_stat = self._io.read_u1()
self.reserved3 = self._io.read_bytes(1)
class RxmSfrbx(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.gnss_id = KaitaiStream.resolve_enum(Ubx.GnssType, self._io.read_u1())
self.sv_id = self._io.read_u1()
self.reserved1 = self._io.read_bytes(1)
self.freq_id = self._io.read_u1()
self.num_words = self._io.read_u1()
self.reserved2 = self._io.read_bytes(1)
self.version = self._io.read_u1()
self.reserved3 = self._io.read_bytes(1)
self.body = []
for i in range(self.num_words):
self.body.append(self._io.read_u4le())
class NavSat(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.itow = self._io.read_u4le()
self.version = self._io.read_u1()
self.num_svs = self._io.read_u1()
self.reserved = self._io.read_bytes(2)
self._raw_svs = []
self.svs = []
for i in range(self.num_svs):
self._raw_svs.append(self._io.read_bytes(12))
_io__raw_svs = KaitaiStream(BytesIO(self._raw_svs[i]))
self.svs.append(Ubx.NavSat.Nav(_io__raw_svs, self, self._root))
class Nav(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.gnss_id = KaitaiStream.resolve_enum(Ubx.GnssType, self._io.read_u1())
self.sv_id = self._io.read_u1()
self.cno = self._io.read_u1()
self.elev = self._io.read_s1()
self.azim = self._io.read_s2le()
self.pr_res = self._io.read_s2le()
self.flags = self._io.read_u4le()
class NavPvt(KaitaiStruct):
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.i_tow = self._io.read_u4le()
self.year = self._io.read_u2le()
self.month = self._io.read_u1()
self.day = self._io.read_u1()
self.hour = self._io.read_u1()
self.min = self._io.read_u1()
self.sec = self._io.read_u1()
self.valid = self._io.read_u1()
self.t_acc = self._io.read_u4le()
self.nano = self._io.read_s4le()
self.fix_type = self._io.read_u1()
self.flags = self._io.read_u1()
self.flags2 = self._io.read_u1()
self.num_sv = self._io.read_u1()
self.lon = self._io.read_s4le()
self.lat = self._io.read_s4le()
self.height = self._io.read_s4le()
self.h_msl = self._io.read_s4le()
self.h_acc = self._io.read_u4le()
self.v_acc = self._io.read_u4le()
self.vel_n = self._io.read_s4le()
self.vel_e = self._io.read_s4le()
self.vel_d = self._io.read_s4le()
self.g_speed = self._io.read_s4le()
self.head_mot = self._io.read_s4le()
self.s_acc = self._io.read_s4le()
self.head_acc = self._io.read_u4le()
self.p_dop = self._io.read_u2le()
self.flags3 = self._io.read_u1()
self.reserved1 = self._io.read_bytes(5)
self.head_veh = self._io.read_s4le()
self.mag_dec = self._io.read_s2le()
self.mag_acc = self._io.read_u2le()
class MonHw2(KaitaiStruct):
class ConfigSource(Enum):
flash = 102
otp = 111
config_pins = 112
rom = 113
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.ofs_i = self._io.read_s1()
self.mag_i = self._io.read_u1()
self.ofs_q = self._io.read_s1()
self.mag_q = self._io.read_u1()
self.cfg_source = KaitaiStream.resolve_enum(Ubx.MonHw2.ConfigSource, self._io.read_u1())
self.reserved1 = self._io.read_bytes(3)
self.low_lev_cfg = self._io.read_u4le()
self.reserved2 = self._io.read_bytes(8)
self.post_status = self._io.read_u4le()
self.reserved3 = self._io.read_bytes(4)
class MonHw(KaitaiStruct):
class AntennaStatus(Enum):
init = 0
dontknow = 1
ok = 2
short = 3
open = 4
class AntennaPower(Enum):
false = 0
true = 1
dontknow = 2
def __init__(self, _io, _parent=None, _root=None):
self._io = _io
self._parent = _parent
self._root = _root if _root else self
self._read()
def _read(self):
self.pin_sel = self._io.read_u4le()
self.pin_bank = self._io.read_u4le()
self.pin_dir = self._io.read_u4le()
self.pin_val = self._io.read_u4le()
self.noise_per_ms = self._io.read_u2le()
self.agc_cnt = self._io.read_u2le()
self.a_status = KaitaiStream.resolve_enum(Ubx.MonHw.AntennaStatus, self._io.read_u1())
self.a_power = KaitaiStream.resolve_enum(Ubx.MonHw.AntennaPower, self._io.read_u1())
self.flags = self._io.read_u1()
self.reserved1 = self._io.read_bytes(1)
self.used_mask = self._io.read_u4le()
self.vp = self._io.read_bytes(17)
self.jam_ind = self._io.read_u1()
self.reserved2 = self._io.read_bytes(2)
self.pin_irq = self._io.read_u4le()
self.pull_h = self._io.read_u4le()
self.pull_l = self._io.read_u4le()
@property
def checksum(self):
if hasattr(self, '_m_checksum'):
return self._m_checksum
_pos = self._io.pos()
self._io.seek((self.length + 6))
self._m_checksum = self._io.read_u2le()
self._io.seek(_pos)
return getattr(self, '_m_checksum', None)

Some files were not shown because too many files have changed in this diff Show More