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
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)
system/ubloxd/glonass_fix.patch
-13
View File
@@ -1,13 +0,0 @@
diff --git a/system/ubloxd/generated/glonass.cpp b/system/ubloxd/generated/glonass.cpp
index 5b17bc327..b5c6aa610 100644
--- a/system/ubloxd/generated/glonass.cpp
+++ b/system/ubloxd/generated/glonass.cpp
@@ -17,7 +17,7 @@ glonass_t::glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, glonass
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();
+ //m__io->align_to_byte();
switch (string_number()) {
case 4: {
m_data = new string_4_t(m__io, this, m__root);
system/ubloxd/pigeond.py
+30 -34
View File
@@ -41,7 +41,7 @@ def add_ubx_checksum(msg: bytes) -> bytes:
B = (B + A) % 256
return msg + bytes([A, B])
def get_assistnow_messages(token: bytes) -> list[bytes]:
def get_assistnow_messages(token: str) -> list[bytes]:
# make request
# TODO: implement adding the last known location
r = requests.get("https://online-live2.services.u-blox.com/GetOnlineData.ashx", params=urllib.parse.urlencode({
@@ -136,6 +136,17 @@ class TTYPigeon:
return True
return False
def save_almanac(pigeon: TTYPigeon) -> None:
# store almanac in flash
pigeon.send(b"\xB5\x62\x09\x14\x04\x00\x00\x00\x00\x00\x21\xEC")
try:
if pigeon.wait_for_ack(ack=UBLOX_SOS_ACK, nack=UBLOX_SOS_NACK):
cloudlog.info("Done storing almanac")
else:
cloudlog.error("Error storing almanac")
except TimeoutError:
pass
def init_baudrate(pigeon: TTYPigeon):
# ublox default setting on startup is 9600 baudrate
pigeon.set_baud(9600)
@@ -146,7 +157,7 @@ def init_baudrate(pigeon: TTYPigeon):
pigeon.set_baud(460800)
def initialize_pigeon(pigeon: TTYPigeon) -> bool:
def init_pigeon(pigeon: TTYPigeon) -> bool:
# try initializing a few times
for _ in range(10):
try:
@@ -239,32 +250,17 @@ def initialize_pigeon(pigeon: TTYPigeon) -> bool:
return True
def deinitialize_and_exit(pigeon: TTYPigeon | None):
cloudlog.warning("Storing almanac in ublox flash")
if pigeon is not None:
# controlled GNSS stop
pigeon.send(b"\xB5\x62\x06\x04\x04\x00\x00\x00\x08\x00\x16\x74")
# store almanac in flash
pigeon.send(b"\xB5\x62\x09\x14\x04\x00\x00\x00\x00\x00\x21\xEC")
try:
if pigeon.wait_for_ack(ack=UBLOX_SOS_ACK, nack=UBLOX_SOS_NACK):
cloudlog.warning("Done storing almanac")
else:
cloudlog.error("Error storing almanac")
except TimeoutError:
pass
# turn off power and exit cleanly
set_power(False)
sys.exit(0)
def create_pigeon() -> tuple[TTYPigeon, messaging.PubMaster]:
pigeon = None
def init(pigeon: TTYPigeon) -> None:
# register exit handler
signal.signal(signal.SIGINT, lambda sig, frame: deinitialize_and_exit(pigeon))
pm = messaging.PubMaster(['ubloxRaw'])
# power cycle ublox
set_power(False)
@@ -272,28 +268,34 @@ def create_pigeon() -> tuple[TTYPigeon, messaging.PubMaster]:
set_power(True)
time.sleep(0.5)
pigeon = TTYPigeon()
return pigeon, pm
init_baudrate(pigeon)
init_pigeon(pigeon)
def run_receiving(pigeon: TTYPigeon, pm: messaging.PubMaster, duration: int = 0):
def run_receiving(duration: int = 0):
pm = messaging.PubMaster(['ubloxRaw'])
pigeon = TTYPigeon()
init(pigeon)
start_time = time.monotonic()
def end_condition():
return True if duration == 0 else time.monotonic() - start_time < duration
while end_condition():
last_almanac_save = time.monotonic()
while (duration == 0) or (time.monotonic() - start_time < duration):
dat = pigeon.receive()
if len(dat) > 0:
if dat[0] == 0x00:
cloudlog.warning("received invalid data from ublox, re-initing!")
init_baudrate(pigeon)
initialize_pigeon(pigeon)
init(pigeon)
continue
# send out to socket
msg = messaging.new_message('ubloxRaw', len(dat), valid=True)
msg.ubloxRaw = dat[:]
pm.send('ubloxRaw', msg)
# save almanac every 5 minutes
if (time.monotonic() - last_almanac_save) > 60*5:
save_almanac(pigeon)
last_almanac_save = time.monotonic()
else:
# prevent locking up a CPU core if ublox disconnects
time.sleep(0.001)
@@ -301,13 +303,7 @@ def run_receiving(pigeon: TTYPigeon, pm: messaging.PubMaster, duration: int = 0)
def main():
assert TICI, "unsupported hardware for pigeond"
pigeon, pm = create_pigeon()
init_baudrate(pigeon)
initialize_pigeon(pigeon)
# start receiving data
run_receiving(pigeon, pm)
run_receiving()
if __name__ == "__main__":
main()
system/ubloxd/tests/print_gps_stats.py
-20
View File
@@ -1,20 +0,0 @@
#!/usr/bin/env python3
import time
import cereal.messaging as messaging
if __name__ == "__main__":
sm = messaging.SubMaster(['ubloxGnss', 'gpsLocationExternal'])
while 1:
ug = sm['ubloxGnss']
gle = sm['gpsLocationExternal']
try:
cnos = []
for m in ug.measurementReport.measurements:
cnos.append(m.cno)
print(f"Sats: {ug.measurementReport.numMeas} Accuracy: {gle.horizontalAccuracy:.2f} m cnos", sorted(cnos))
except Exception:
pass
sm.update()
time.sleep(0.1)
system/ubloxd/tests/test_glonass_kaitai.cc
-360
View File
@@ -1,360 +0,0 @@
#include <iostream>
#include <vector>
#include <bitset>
#include <cassert>
#include <cstdlib>
#include <ctime>
#include "catch2/catch.hpp"
#include "system/ubloxd/generated/glonass.h"
typedef std::vector<std::pair<int, int64_t>> string_data;
#define IDLE_CHIP_IDX 0
#define STRING_NUMBER_IDX 1
// string data 1-5
#define HC_IDX 0
#define PAD1_IDX 1
#define SUPERFRAME_IDX 2
#define PAD2_IDX 3
#define FRAME_IDX 4
// Indexes for string number 1
#define ST1_NU_IDX 2
#define ST1_P1_IDX 3
#define ST1_T_K_IDX 4
#define ST1_X_VEL_S_IDX 5
#define ST1_X_VEL_V_IDX 6
#define ST1_X_ACCEL_S_IDX 7
#define ST1_X_ACCEL_V_IDX 8
#define ST1_X_S_IDX 9
#define ST1_X_V_IDX 10
#define ST1_HC_OFF 11
// Indexes for string number 2
#define ST2_BN_IDX 2
#define ST2_P2_IDX 3
#define ST2_TB_IDX 4
#define ST2_NU_IDX 5
#define ST2_Y_VEL_S_IDX 6
#define ST2_Y_VEL_V_IDX 7
#define ST2_Y_ACCEL_S_IDX 8
#define ST2_Y_ACCEL_V_IDX 9
#define ST2_Y_S_IDX 10
#define ST2_Y_V_IDX 11
#define ST2_HC_OFF 12
// Indexes for string number 3
#define ST3_P3_IDX 2
#define ST3_GAMMA_N_S_IDX 3
#define ST3_GAMMA_N_V_IDX 4
#define ST3_NU_1_IDX 5
#define ST3_P_IDX 6
#define ST3_L_N_IDX 7
#define ST3_Z_VEL_S_IDX 8
#define ST3_Z_VEL_V_IDX 9
#define ST3_Z_ACCEL_S_IDX 10
#define ST3_Z_ACCEL_V_IDX 11
#define ST3_Z_S_IDX 12
#define ST3_Z_V_IDX 13
#define ST3_HC_OFF 14
// Indexes for string number 4
#define ST4_TAU_N_S_IDX 2
#define ST4_TAU_N_V_IDX 3
#define ST4_DELTA_TAU_N_S_IDX 4
#define ST4_DELTA_TAU_N_V_IDX 5
#define ST4_E_N_IDX 6
#define ST4_NU_1_IDX 7
#define ST4_P4_IDX 8
#define ST4_F_T_IDX 9
#define ST4_NU_2_IDX 10
#define ST4_N_T_IDX 11
#define ST4_N_IDX 12
#define ST4_M_IDX 13
#define ST4_HC_OFF 14
// Indexes for string number 5
#define ST5_N_A_IDX 2
#define ST5_TAU_C_IDX 3
#define ST5_NU_IDX 4
#define ST5_N_4_IDX 5
#define ST5_TAU_GPS_IDX 6
#define ST5_L_N_IDX 7
#define ST5_HC_OFF 8
// Indexes for non immediate
#define ST6_DATA_1_IDX 2
#define ST6_DATA_2_IDX 3
#define ST6_HC_OFF 4
std::string generate_inp_data(string_data& data) {
std::string inp_data = "";
for (auto& [b, v] : data) {
std::string tmp = std::bitset<64>(v).to_string();
inp_data += tmp.substr(64-b, b);
}
assert(inp_data.size() == 128);
std::string string_data;
string_data.reserve(16);
for (int i = 0; i < 128; i+=8) {
std::string substr = inp_data.substr(i, 8);
string_data.push_back((uint8_t)std::stoi(substr.c_str(), 0, 2));
}
return string_data;
}
string_data generate_string_data(uint8_t string_number) {
srand((unsigned)time(0));
string_data data; //<bit length, value>
data.push_back({1, 0}); // idle chip
data.push_back({4, string_number}); // string number
if (string_number == 1) {
data.push_back({2, 3}); // not_used
data.push_back({2, 1}); // p1
data.push_back({12, 113}); // t_k
data.push_back({1, rand() & 1}); // x_vel_sign
data.push_back({23, 7122}); // x_vel_value
data.push_back({1, rand() & 1}); // x_accel_sign
data.push_back({4, 3}); // x_accel_value
data.push_back({1, rand() & 1}); // x_sign
data.push_back({26, 33554431}); // x_value
} else if (string_number == 2) {
data.push_back({3, 3}); // b_n
data.push_back({1, 1}); // p2
data.push_back({7, 123}); // t_b
data.push_back({5, 31}); // not_used
data.push_back({1, rand() & 1}); // y_vel_sign
data.push_back({23, 7422}); // y_vel_value
data.push_back({1, rand() & 1}); // y_accel_sign
data.push_back({4, 3}); // y_accel_value
data.push_back({1, rand() & 1}); // y_sign
data.push_back({26, 67108863}); // y_value
} else if (string_number == 3) {
data.push_back({1, 0}); // p3
data.push_back({1, 1}); // gamma_n_sign
data.push_back({10, 123}); // gamma_n_value
data.push_back({1, 0}); // not_used
data.push_back({2, 2}); // p
data.push_back({1, 1}); // l_n
data.push_back({1, rand() & 1}); // z_vel_sign
data.push_back({23, 1337}); // z_vel_value
data.push_back({1, rand() & 1}); // z_accel_sign
data.push_back({4, 9}); // z_accel_value
data.push_back({1, rand() & 1}); // z_sign
data.push_back({26, 100023}); // z_value
} else if (string_number == 4) {
data.push_back({1, rand() & 1}); // tau_n_sign
data.push_back({21, 197152}); // tau_n_value
data.push_back({1, rand() & 1}); // delta_tau_n_sign
data.push_back({4, 4}); // delta_tau_n_value
data.push_back({5, 0}); // e_n
data.push_back({14, 2}); // not_used_1
data.push_back({1, 1}); // p4
data.push_back({4, 9}); // f_t
data.push_back({3, 3}); // not_used_2
data.push_back({11, 2047}); // n_t
data.push_back({5, 2}); // n
data.push_back({2, 1}); // m
} else if (string_number == 5) {
data.push_back({11, 2047}); // n_a
data.push_back({32, 4294767295}); // tau_c
data.push_back({1, 0}); // not_used_1
data.push_back({5, 2}); // n_4
data.push_back({22, 4114304}); // tau_gps
data.push_back({1, 0}); // l_n
} else { // non-immediate data is not parsed
data.push_back({64, rand()}); // data_1
data.push_back({8, 6}); // data_2
}
data.push_back({8, rand() & 0xFF}); // hamming code
data.push_back({11, rand() & 0x7FF}); // pad
data.push_back({16, rand() & 0xFFFF}); // superframe
data.push_back({8, rand() & 0xFF}); // pad
data.push_back({8, rand() & 0xFF}); // frame
return data;
}
TEST_CASE("parse_string_number_1"){
string_data data = generate_string_data(1);
std::string inp_data = generate_inp_data(data);
kaitai::kstream stream(inp_data);
glonass_t gl_string(&stream);
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
REQUIRE(gl_string.hamming_code() == data[ST1_HC_OFF + HC_IDX].second);
REQUIRE(gl_string.pad_1() == data[ST1_HC_OFF + PAD1_IDX].second);
REQUIRE(gl_string.superframe_number() == data[ST1_HC_OFF + SUPERFRAME_IDX].second);
REQUIRE(gl_string.pad_2() == data[ST1_HC_OFF + PAD2_IDX].second);
REQUIRE(gl_string.frame_number() == data[ST1_HC_OFF + FRAME_IDX].second);
kaitai::kstream str1(inp_data);
glonass_t str1_data(&str1);
glonass_t::string_1_t* s1 = static_cast<glonass_t::string_1_t*>(str1_data.data());
REQUIRE(s1->not_used() == data[ST1_NU_IDX].second);
REQUIRE(s1->p1() == data[ST1_P1_IDX].second);
REQUIRE(s1->t_k() == data[ST1_T_K_IDX].second);
int mul = s1->x_vel_sign() ? (-1) : 1;
REQUIRE(s1->x_vel() == (data[ST1_X_VEL_V_IDX].second * mul));
mul = s1->x_accel_sign() ? (-1) : 1;
REQUIRE(s1->x_accel() == (data[ST1_X_ACCEL_V_IDX].second * mul));
mul = s1->x_sign() ? (-1) : 1;
REQUIRE(s1->x() == (data[ST1_X_V_IDX].second * mul));
}
TEST_CASE("parse_string_number_2"){
string_data data = generate_string_data(2);
std::string inp_data = generate_inp_data(data);
kaitai::kstream stream(inp_data);
glonass_t gl_string(&stream);
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
REQUIRE(gl_string.hamming_code() == data[ST2_HC_OFF + HC_IDX].second);
REQUIRE(gl_string.pad_1() == data[ST2_HC_OFF + PAD1_IDX].second);
REQUIRE(gl_string.superframe_number() == data[ST2_HC_OFF + SUPERFRAME_IDX].second);
REQUIRE(gl_string.pad_2() == data[ST2_HC_OFF + PAD2_IDX].second);
REQUIRE(gl_string.frame_number() == data[ST2_HC_OFF + FRAME_IDX].second);
kaitai::kstream str2(inp_data);
glonass_t str2_data(&str2);
glonass_t::string_2_t* s2 = static_cast<glonass_t::string_2_t*>(str2_data.data());
REQUIRE(s2->b_n() == data[ST2_BN_IDX].second);
REQUIRE(s2->not_used() == data[ST2_NU_IDX].second);
REQUIRE(s2->p2() == data[ST2_P2_IDX].second);
REQUIRE(s2->t_b() == data[ST2_TB_IDX].second);
int mul = s2->y_vel_sign() ? (-1) : 1;
REQUIRE(s2->y_vel() == (data[ST2_Y_VEL_V_IDX].second * mul));
mul = s2->y_accel_sign() ? (-1) : 1;
REQUIRE(s2->y_accel() == (data[ST2_Y_ACCEL_V_IDX].second * mul));
mul = s2->y_sign() ? (-1) : 1;
REQUIRE(s2->y() == (data[ST2_Y_V_IDX].second * mul));
}
TEST_CASE("parse_string_number_3"){
string_data data = generate_string_data(3);
std::string inp_data = generate_inp_data(data);
kaitai::kstream stream(inp_data);
glonass_t gl_string(&stream);
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
REQUIRE(gl_string.hamming_code() == data[ST3_HC_OFF + HC_IDX].second);
REQUIRE(gl_string.pad_1() == data[ST3_HC_OFF + PAD1_IDX].second);
REQUIRE(gl_string.superframe_number() == data[ST3_HC_OFF + SUPERFRAME_IDX].second);
REQUIRE(gl_string.pad_2() == data[ST3_HC_OFF + PAD2_IDX].second);
REQUIRE(gl_string.frame_number() == data[ST3_HC_OFF + FRAME_IDX].second);
kaitai::kstream str3(inp_data);
glonass_t str3_data(&str3);
glonass_t::string_3_t* s3 = static_cast<glonass_t::string_3_t*>(str3_data.data());
REQUIRE(s3->p3() == data[ST3_P3_IDX].second);
int mul = s3->gamma_n_sign() ? (-1) : 1;
REQUIRE(s3->gamma_n() == (data[ST3_GAMMA_N_V_IDX].second * mul));
REQUIRE(s3->not_used() == data[ST3_NU_1_IDX].second);
REQUIRE(s3->p() == data[ST3_P_IDX].second);
REQUIRE(s3->l_n() == data[ST3_L_N_IDX].second);
mul = s3->z_vel_sign() ? (-1) : 1;
REQUIRE(s3->z_vel() == (data[ST3_Z_VEL_V_IDX].second * mul));
mul = s3->z_accel_sign() ? (-1) : 1;
REQUIRE(s3->z_accel() == (data[ST3_Z_ACCEL_V_IDX].second * mul));
mul = s3->z_sign() ? (-1) : 1;
REQUIRE(s3->z() == (data[ST3_Z_V_IDX].second * mul));
}
TEST_CASE("parse_string_number_4"){
string_data data = generate_string_data(4);
std::string inp_data = generate_inp_data(data);
kaitai::kstream stream(inp_data);
glonass_t gl_string(&stream);
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
REQUIRE(gl_string.hamming_code() == data[ST4_HC_OFF + HC_IDX].second);
REQUIRE(gl_string.pad_1() == data[ST4_HC_OFF + PAD1_IDX].second);
REQUIRE(gl_string.superframe_number() == data[ST4_HC_OFF + SUPERFRAME_IDX].second);
REQUIRE(gl_string.pad_2() == data[ST4_HC_OFF + PAD2_IDX].second);
REQUIRE(gl_string.frame_number() == data[ST4_HC_OFF + FRAME_IDX].second);
kaitai::kstream str4(inp_data);
glonass_t str4_data(&str4);
glonass_t::string_4_t* s4 = static_cast<glonass_t::string_4_t*>(str4_data.data());
int mul = s4->tau_n_sign() ? (-1) : 1;
REQUIRE(s4->tau_n() == (data[ST4_TAU_N_V_IDX].second * mul));
mul = s4->delta_tau_n_sign() ? (-1) : 1;
REQUIRE(s4->delta_tau_n() == (data[ST4_DELTA_TAU_N_V_IDX].second * mul));
REQUIRE(s4->e_n() == data[ST4_E_N_IDX].second);
REQUIRE(s4->not_used_1() == data[ST4_NU_1_IDX].second);
REQUIRE(s4->p4() == data[ST4_P4_IDX].second);
REQUIRE(s4->f_t() == data[ST4_F_T_IDX].second);
REQUIRE(s4->not_used_2() == data[ST4_NU_2_IDX].second);
REQUIRE(s4->n_t() == data[ST4_N_T_IDX].second);
REQUIRE(s4->n() == data[ST4_N_IDX].second);
REQUIRE(s4->m() == data[ST4_M_IDX].second);
}
TEST_CASE("parse_string_number_5"){
string_data data = generate_string_data(5);
std::string inp_data = generate_inp_data(data);
kaitai::kstream stream(inp_data);
glonass_t gl_string(&stream);
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
REQUIRE(gl_string.hamming_code() == data[ST5_HC_OFF + HC_IDX].second);
REQUIRE(gl_string.pad_1() == data[ST5_HC_OFF + PAD1_IDX].second);
REQUIRE(gl_string.superframe_number() == data[ST5_HC_OFF + SUPERFRAME_IDX].second);
REQUIRE(gl_string.pad_2() == data[ST5_HC_OFF + PAD2_IDX].second);
REQUIRE(gl_string.frame_number() == data[ST5_HC_OFF + FRAME_IDX].second);
kaitai::kstream str5(inp_data);
glonass_t str5_data(&str5);
glonass_t::string_5_t* s5 = static_cast<glonass_t::string_5_t*>(str5_data.data());
REQUIRE(s5->n_a() == data[ST5_N_A_IDX].second);
REQUIRE(s5->tau_c() == data[ST5_TAU_C_IDX].second);
REQUIRE(s5->not_used() == data[ST5_NU_IDX].second);
REQUIRE(s5->n_4() == data[ST5_N_4_IDX].second);
REQUIRE(s5->tau_gps() == data[ST5_TAU_GPS_IDX].second);
REQUIRE(s5->l_n() == data[ST5_L_N_IDX].second);
}
TEST_CASE("parse_string_number_NI"){
string_data data = generate_string_data((rand() % 10) + 6);
std::string inp_data = generate_inp_data(data);
kaitai::kstream stream(inp_data);
glonass_t gl_string(&stream);
REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
REQUIRE(gl_string.hamming_code() == data[ST6_HC_OFF + HC_IDX].second);
REQUIRE(gl_string.pad_1() == data[ST6_HC_OFF + PAD1_IDX].second);
REQUIRE(gl_string.superframe_number() == data[ST6_HC_OFF + SUPERFRAME_IDX].second);
REQUIRE(gl_string.pad_2() == data[ST6_HC_OFF + PAD2_IDX].second);
REQUIRE(gl_string.frame_number() == data[ST6_HC_OFF + FRAME_IDX].second);
kaitai::kstream strni(inp_data);
glonass_t strni_data(&strni);
glonass_t::string_non_immediate_t* sni = static_cast<glonass_t::string_non_immediate_t*>(strni_data.data());
REQUIRE(sni->data_1() == data[ST6_DATA_1_IDX].second);
REQUIRE(sni->data_2() == data[ST6_DATA_2_IDX].second);
}
system/ubloxd/tests/test_glonass_runner.cc
-2
View File
@@ -1,2 +0,0 @@
#define CATCH_CONFIG_MAIN
#include "catch2/catch.hpp"
system/ubloxd/tests/ubloxd.py
-89
View File
@@ -1,89 +0,0 @@
#!/usr/bin/env python3
# type: ignore
from openpilot.selfdrive.locationd.test import ublox
import struct
baudrate = 460800
rate = 100 # send new data every 100ms
def configure_ublox(dev):
# configure ports and solution parameters and rate
dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1) # enable only UBX on USB
dev.configure_port(port=0, inMask=0, outMask=0) # disable DDC
payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 1, 1, 0, 0, 0)
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # enable UART
dev.configure_port(port=4, inMask=0, outMask=0) # disable SPI
dev.configure_poll_port()
dev.configure_poll_port(ublox.PORT_SERIAL1)
dev.configure_poll_port(ublox.PORT_USB)
dev.configure_solution_rate(rate_ms=rate)
# Configure solution
payload = struct.pack('<HBBIIBB4H6BH6B', 5, 4, 3, 0, 0,
0, 0, 0, 0, 0,
0, 0, 0, 0, 0,
0, 0, 0, 0, 0,
0, 0, 0, 0)
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5, payload)
payload = struct.pack('<B3BBB6BBB2BBB2B', 0, 0, 0, 0, 1,
3, 0, 0, 0, 0,
0, 0, 0, 0, 0,
0, 0, 0, 0, 0)
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO, payload)
#bits_ITMF_config1 = '10101101011000101010110111111111'
#bits_ITMF_config2 = '00000000000000000110001100011110'
ITMF_config1 = 2908925439
ITMF_config2 = 25374
payload = struct.pack('<II', ITMF_config1, ITMF_config2)
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ITMF, payload)
payload = struct.pack('<HHIBBBBBBBBBBH6BBB2BH4B3BB', 0, (1 << 10), 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0)
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5, payload)
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5)
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5)
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO)
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ITMF)
# Configure RAW, PVT and HW messages to be sent every solution cycle
dev.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_PVT, 1)
dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_RAW, 1)
dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_SFRBX, 1)
dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW, 1)
dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW2, 1)
dev.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_SAT, 1)
# Query the backup restore status
print("backup restore polling message (implement custom response handler!):")
dev.configure_poll(0x09, 0x14)
print("if successful, send this to clear the flash:")
dev.send_message(0x09, 0x14, b"\x01\x00\x00\x00")
print("send on stop:")
# Save on shutdown
# Controlled GNSS stop and hot start
payload = struct.pack('<HBB', 0x0000, 0x08, 0x00)
dev.send_message(ublox.CLASS_CFG, ublox.MSG_CFG_RST, payload)
# UBX-UPD-SOS backup
dev.send_message(0x09, 0x14, b"\x00\x00\x00\x00")
if __name__ == "__main__":
class Device:
def write(self, s):
d = '"{}"s'.format(''.join(f'\\x{b:02X}' for b in s))
print(f" if (!send_with_ack({d})) continue;")
dev = ublox.UBlox(Device(), baudrate=baudrate)
configure_ublox(dev)
system/ubloxd/ublox_msg.cc
-530
View File
@@ -1,530 +0,0 @@
#include "system/ubloxd/ublox_msg.h"
#include <unistd.h>
#include <algorithm>
#include <cassert>
#include <chrono>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <unordered_map>
#include <utility>
#include "common/swaglog.h"
const double gpsPi = 3.1415926535898;
#define UBLOX_MSG_SIZE(hdr) (*(uint16_t *)&hdr[4])
inline static bool bit_to_bool(uint8_t val, int shifts) {
return (bool)(val & (1 << shifts));
}
inline int UbloxMsgParser::needed_bytes() {
// Msg header incomplete?
if (bytes_in_parse_buf < ublox::UBLOX_HEADER_SIZE)
return ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE - bytes_in_parse_buf;
uint16_t needed = UBLOX_MSG_SIZE(msg_parse_buf) + ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE;
// too much data
if (needed < (uint16_t)bytes_in_parse_buf)
return -1;
return needed - (uint16_t)bytes_in_parse_buf;
}
inline bool UbloxMsgParser::valid_cheksum() {
uint8_t ck_a = 0, ck_b = 0;
for (int i = 2; i < bytes_in_parse_buf - ublox::UBLOX_CHECKSUM_SIZE; i++) {
ck_a = (ck_a + msg_parse_buf[i]) & 0xFF;
ck_b = (ck_b + ck_a) & 0xFF;
}
if (ck_a != msg_parse_buf[bytes_in_parse_buf - 2]) {
LOGD("Checksum a mismatch: %02X, %02X", ck_a, msg_parse_buf[6]);
return false;
}
if (ck_b != msg_parse_buf[bytes_in_parse_buf - 1]) {
LOGD("Checksum b mismatch: %02X, %02X", ck_b, msg_parse_buf[7]);
return false;
}
return true;
}
inline bool UbloxMsgParser::valid() {
return bytes_in_parse_buf >= ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE &&
needed_bytes() == 0 && valid_cheksum();
}
inline bool UbloxMsgParser::valid_so_far() {
if (bytes_in_parse_buf > 0 && msg_parse_buf[0] != ublox::PREAMBLE1) {
return false;
}
if (bytes_in_parse_buf > 1 && msg_parse_buf[1] != ublox::PREAMBLE2) {
return false;
}
if (needed_bytes() == 0 && !valid()) {
return false;
}
return true;
}
bool UbloxMsgParser::add_data(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
last_log_time = log_time;
int needed = needed_bytes();
if (needed > 0) {
bytes_consumed = std::min((uint32_t)needed, incoming_data_len);
// Add data to buffer
memcpy(msg_parse_buf + bytes_in_parse_buf, incoming_data, bytes_consumed);
bytes_in_parse_buf += bytes_consumed;
} else {
bytes_consumed = incoming_data_len;
}
// Validate msg format, detect invalid header and invalid checksum.
while (!valid_so_far() && bytes_in_parse_buf != 0) {
// Corrupted msg, drop a byte.
bytes_in_parse_buf -= 1;
if (bytes_in_parse_buf > 0)
memmove(&msg_parse_buf[0], &msg_parse_buf[1], bytes_in_parse_buf);
}
// There is redundant data at the end of buffer, reset the buffer.
if (needed_bytes() == -1) {
bytes_in_parse_buf = 0;
}
return valid();
}
std::pair<std::string, kj::Array<capnp::word>> UbloxMsgParser::gen_msg() {
std::string dat = data();
kaitai::kstream stream(dat);
ubx_t ubx_message(&stream);
auto body = ubx_message.body();
switch (ubx_message.msg_type()) {
case 0x0107:
return {"gpsLocationExternal", gen_nav_pvt(static_cast<ubx_t::nav_pvt_t*>(body))};
case 0x0213: // UBX-RXM-SFRB (Broadcast Navigation Data Subframe)
return {"ubloxGnss", gen_rxm_sfrbx(static_cast<ubx_t::rxm_sfrbx_t*>(body))};
case 0x0215: // UBX-RXM-RAW (Multi-GNSS Raw Measurement Data)
return {"ubloxGnss", gen_rxm_rawx(static_cast<ubx_t::rxm_rawx_t*>(body))};
case 0x0a09:
return {"ubloxGnss", gen_mon_hw(static_cast<ubx_t::mon_hw_t*>(body))};
case 0x0a0b:
return {"ubloxGnss", gen_mon_hw2(static_cast<ubx_t::mon_hw2_t*>(body))};
case 0x0135:
return {"ubloxGnss", gen_nav_sat(static_cast<ubx_t::nav_sat_t*>(body))};
default:
LOGE("Unknown message type %x", ubx_message.msg_type());
return {"ubloxGnss", kj::Array<capnp::word>()};
}
}
kj::Array<capnp::word> UbloxMsgParser::gen_nav_pvt(ubx_t::nav_pvt_t *msg) {
MessageBuilder msg_builder;
auto gpsLoc = msg_builder.initEvent().initGpsLocationExternal();
gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX);
gpsLoc.setFlags(msg->flags());
gpsLoc.setHasFix((msg->flags() % 2) == 1);
gpsLoc.setLatitude(msg->lat() * 1e-07);
gpsLoc.setLongitude(msg->lon() * 1e-07);
gpsLoc.setAltitude(msg->height() * 1e-03);
gpsLoc.setSpeed(msg->g_speed() * 1e-03);
gpsLoc.setBearingDeg(msg->head_mot() * 1e-5);
gpsLoc.setHorizontalAccuracy(msg->h_acc() * 1e-03);
gpsLoc.setSatelliteCount(msg->num_sv());
std::tm timeinfo = std::tm();
timeinfo.tm_year = msg->year() - 1900;
timeinfo.tm_mon = msg->month() - 1;
timeinfo.tm_mday = msg->day();
timeinfo.tm_hour = msg->hour();
timeinfo.tm_min = msg->min();
timeinfo.tm_sec = msg->sec();
std::time_t utc_tt = timegm(&timeinfo);
gpsLoc.setUnixTimestampMillis(utc_tt * 1e+03 + msg->nano() * 1e-06);
float f[] = { msg->vel_n() * 1e-03f, msg->vel_e() * 1e-03f, msg->vel_d() * 1e-03f };
gpsLoc.setVNED(f);
gpsLoc.setVerticalAccuracy(msg->v_acc() * 1e-03);
gpsLoc.setSpeedAccuracy(msg->s_acc() * 1e-03);
gpsLoc.setBearingAccuracyDeg(msg->head_acc() * 1e-05);
return capnp::messageToFlatArray(msg_builder);
}
kj::Array<capnp::word> UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg) {
// GPS subframes are packed into 10x 4 bytes, each containing 3 actual bytes
// We will first need to separate the data from the padding and parity
auto body = *msg->body();
assert(body.size() == 10);
std::string subframe_data;
subframe_data.reserve(30);
for (uint32_t word : body) {
word = word >> 6; // TODO: Verify parity
subframe_data.push_back(word >> 16);
subframe_data.push_back(word >> 8);
subframe_data.push_back(word >> 0);
}
// Collect subframes in map and parse when we have all the parts
{
kaitai::kstream stream(subframe_data);
gps_t subframe(&stream);
int subframe_id = subframe.how()->subframe_id();
if (subframe_id > 3 || subframe_id < 1) {
// don't parse almanac subframes
return kj::Array<capnp::word>();
}
gps_subframes[msg->sv_id()][subframe_id] = subframe_data;
}
// publish if subframes 1-3 have been collected
if (gps_subframes[msg->sv_id()].size() == 3) {
MessageBuilder msg_builder;
auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris();
eph.setSvId(msg->sv_id());
int iode_s2 = 0;
int iode_s3 = 0;
int iodc_lsb = 0;
int week;
// Subframe 1
{
kaitai::kstream stream(gps_subframes[msg->sv_id()][1]);
gps_t subframe(&stream);
gps_t::subframe_1_t* subframe_1 = static_cast<gps_t::subframe_1_t*>(subframe.body());
// Each message is incremented to be greater or equal than week 1877 (2015-12-27).
// To skip this use the current_time argument
week = subframe_1->week_no();
week += 1024;
if (week < 1877) {
week += 1024;
}
//eph.setGpsWeek(subframe_1->week_no());
eph.setTgd(subframe_1->t_gd() * pow(2, -31));
eph.setToc(subframe_1->t_oc() * pow(2, 4));
eph.setAf2(subframe_1->af_2() * pow(2, -55));
eph.setAf1(subframe_1->af_1() * pow(2, -43));
eph.setAf0(subframe_1->af_0() * pow(2, -31));
eph.setSvHealth(subframe_1->sv_health());
eph.setTowCount(subframe.how()->tow_count());
iodc_lsb = subframe_1->iodc_lsb();
}
// Subframe 2
{
kaitai::kstream stream(gps_subframes[msg->sv_id()][2]);
gps_t subframe(&stream);
gps_t::subframe_2_t* subframe_2 = static_cast<gps_t::subframe_2_t*>(subframe.body());
// GPS week refers to current week, the ephemeris can be valid for the next
// if toe equals 0, this can be verified by the TOW count if it is within the
// last 2 hours of the week (gps ephemeris valid for 4hours)
if (subframe_2->t_oe() == 0 and subframe.how()->tow_count()*6 >= (SECS_IN_WEEK - 2*SECS_IN_HR)){
week += 1;
}
eph.setCrs(subframe_2->c_rs() * pow(2, -5));
eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi);
eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi);
eph.setCuc(subframe_2->c_uc() * pow(2, -29));
eph.setEcc(subframe_2->e() * pow(2, -33));
eph.setCus(subframe_2->c_us() * pow(2, -29));
eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0));
eph.setToe(subframe_2->t_oe() * pow(2, 4));
iode_s2 = subframe_2->iode();
}
// Subframe 3
{
kaitai::kstream stream(gps_subframes[msg->sv_id()][3]);
gps_t subframe(&stream);
gps_t::subframe_3_t* subframe_3 = static_cast<gps_t::subframe_3_t*>(subframe.body());
eph.setCic(subframe_3->c_ic() * pow(2, -29));
eph.setOmega0(subframe_3->omega_0() * pow(2, -31) * gpsPi);
eph.setCis(subframe_3->c_is() * pow(2, -29));
eph.setI0(subframe_3->i_0() * pow(2, -31) * gpsPi);
eph.setCrc(subframe_3->c_rc() * pow(2, -5));
eph.setOmega(subframe_3->omega() * pow(2, -31) * gpsPi);
eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi);
eph.setIode(subframe_3->iode());
eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi);
iode_s3 = subframe_3->iode();
}
eph.setToeWeek(week);
eph.setTocWeek(week);
gps_subframes[msg->sv_id()].clear();
if (iodc_lsb != iode_s2 || iodc_lsb != iode_s3) {
// data set cutover, reject ephemeris
return kj::Array<capnp::word>();
}
return capnp::messageToFlatArray(msg_builder);
}
return kj::Array<capnp::word>();
}
kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg) {
// This parser assumes that no 2 satellites of the same frequency
// can be in view at the same time
auto body = *msg->body();
assert(body.size() == 4);
{
std::string string_data;
string_data.reserve(16);
for (uint32_t word : body) {
for (int i = 3; i >= 0; i--)
string_data.push_back(word >> 8*i);
}
kaitai::kstream stream(string_data);
glonass_t gl_string(&stream);
int string_number = gl_string.string_number();
if (string_number < 1 || string_number > 5 || gl_string.idle_chip()) {
// don't parse non immediate data, idle_chip == 0
return kj::Array<capnp::word>();
}
// Check if new string either has same superframe_id or log transmission times make sense
bool superframe_unknown = false;
bool needs_clear = false;
for (int i = 1; i <= 5; i++) {
if (glonass_strings[msg->freq_id()].find(i) == glonass_strings[msg->freq_id()].end())
continue;
if (glonass_string_superframes[msg->freq_id()][i] == 0 || gl_string.superframe_number() == 0) {
superframe_unknown = true;
} else if (glonass_string_superframes[msg->freq_id()][i] != gl_string.superframe_number()) {
needs_clear = true;
}
// Check if string times add up to being from the same frame
// If superframe is known this is redundant
// Strings are sent 2s apart and frames are 30s apart
if (superframe_unknown &&
std::abs((glonass_string_times[msg->freq_id()][i] - 2.0 * i) - (last_log_time - 2.0 * string_number)) > 10)
needs_clear = true;
}
if (needs_clear) {
glonass_strings[msg->freq_id()].clear();
glonass_string_superframes[msg->freq_id()].clear();
glonass_string_times[msg->freq_id()].clear();
}
glonass_strings[msg->freq_id()][string_number] = string_data;
glonass_string_superframes[msg->freq_id()][string_number] = gl_string.superframe_number();
glonass_string_times[msg->freq_id()][string_number] = last_log_time;
}
if (msg->sv_id() == 255) {
// data can be decoded before identifying the SV number, in this case 255
// is returned, which means "unknown" (ublox p32)
return kj::Array<capnp::word>();
}
// publish if strings 1-5 have been collected
if (glonass_strings[msg->freq_id()].size() != 5) {
return kj::Array<capnp::word>();
}
MessageBuilder msg_builder;
auto eph = msg_builder.initEvent().initUbloxGnss().initGlonassEphemeris();
eph.setSvId(msg->sv_id());
eph.setFreqNum(msg->freq_id() - 7);
uint16_t current_day = 0;
uint16_t tk = 0;
// string number 1
{
kaitai::kstream stream(glonass_strings[msg->freq_id()][1]);
glonass_t gl_stream(&stream);
glonass_t::string_1_t* data = static_cast<glonass_t::string_1_t*>(gl_stream.data());
eph.setP1(data->p1());
tk = data->t_k();
eph.setTkDEPRECATED(tk);
eph.setXVel(data->x_vel() * pow(2, -20));
eph.setXAccel(data->x_accel() * pow(2, -30));
eph.setX(data->x() * pow(2, -11));
}
// string number 2
{
kaitai::kstream stream(glonass_strings[msg->freq_id()][2]);
glonass_t gl_stream(&stream);
glonass_t::string_2_t* data = static_cast<glonass_t::string_2_t*>(gl_stream.data());
eph.setSvHealth(data->b_n()>>2); // MSB indicates health
eph.setP2(data->p2());
eph.setTb(data->t_b());
eph.setYVel(data->y_vel() * pow(2, -20));
eph.setYAccel(data->y_accel() * pow(2, -30));
eph.setY(data->y() * pow(2, -11));
}
// string number 3
{
kaitai::kstream stream(glonass_strings[msg->freq_id()][3]);
glonass_t gl_stream(&stream);
glonass_t::string_3_t* data = static_cast<glonass_t::string_3_t*>(gl_stream.data());
eph.setP3(data->p3());
eph.setGammaN(data->gamma_n() * pow(2, -40));
eph.setSvHealth(eph.getSvHealth() | data->l_n());
eph.setZVel(data->z_vel() * pow(2, -20));
eph.setZAccel(data->z_accel() * pow(2, -30));
eph.setZ(data->z() * pow(2, -11));
}
// string number 4
{
kaitai::kstream stream(glonass_strings[msg->freq_id()][4]);
glonass_t gl_stream(&stream);
glonass_t::string_4_t* data = static_cast<glonass_t::string_4_t*>(gl_stream.data());
current_day = data->n_t();
eph.setNt(current_day);
eph.setTauN(data->tau_n() * pow(2, -30));
eph.setDeltaTauN(data->delta_tau_n() * pow(2, -30));
eph.setAge(data->e_n());
eph.setP4(data->p4());
eph.setSvURA(glonass_URA_lookup.at(data->f_t()));
if (msg->sv_id() != data->n()) {
LOGE("SV_ID != SLOT_NUMBER: %d %" PRIu64, msg->sv_id(), data->n());
}
eph.setSvType(data->m());
}
// string number 5
{
kaitai::kstream stream(glonass_strings[msg->freq_id()][5]);
glonass_t gl_stream(&stream);
glonass_t::string_5_t* data = static_cast<glonass_t::string_5_t*>(gl_stream.data());
// string5 parsing is only needed to get the year, this can be removed and
// the year can be fetched later in laika (note rollovers and leap year)
eph.setN4(data->n_4());
int tk_seconds = SECS_IN_HR * ((tk>>7) & 0x1F) + SECS_IN_MIN * ((tk>>1) & 0x3F) + (tk & 0x1) * 30;
eph.setTkSeconds(tk_seconds);
}
glonass_strings[msg->freq_id()].clear();
return capnp::messageToFlatArray(msg_builder);
}
kj::Array<capnp::word> UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) {
switch (msg->gnss_id()) {
case ubx_t::gnss_type_t::GNSS_TYPE_GPS:
return parse_gps_ephemeris(msg);
case ubx_t::gnss_type_t::GNSS_TYPE_GLONASS:
return parse_glonass_ephemeris(msg);
default:
return kj::Array<capnp::word>();
}
}
kj::Array<capnp::word> UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) {
MessageBuilder msg_builder;
auto mr = msg_builder.initEvent().initUbloxGnss().initMeasurementReport();
mr.setRcvTow(msg->rcv_tow());
mr.setGpsWeek(msg->week());
mr.setLeapSeconds(msg->leap_s());
mr.setGpsWeek(msg->week());
auto mb = mr.initMeasurements(msg->num_meas());
auto measurements = *msg->meas();
for (int8_t i = 0; i < msg->num_meas(); i++) {
mb[i].setSvId(measurements[i]->sv_id());
mb[i].setPseudorange(measurements[i]->pr_mes());
mb[i].setCarrierCycles(measurements[i]->cp_mes());
mb[i].setDoppler(measurements[i]->do_mes());
mb[i].setGnssId(measurements[i]->gnss_id());
mb[i].setGlonassFrequencyIndex(measurements[i]->freq_id());
mb[i].setLocktime(measurements[i]->lock_time());
mb[i].setCno(measurements[i]->cno());
mb[i].setPseudorangeStdev(0.01 * (pow(2, (measurements[i]->pr_stdev() & 15)))); // weird scaling, might be wrong
mb[i].setCarrierPhaseStdev(0.004 * (measurements[i]->cp_stdev() & 15));
mb[i].setDopplerStdev(0.002 * (pow(2, (measurements[i]->do_stdev() & 15)))); // weird scaling, might be wrong
auto ts = mb[i].initTrackingStatus();
auto trk_stat = measurements[i]->trk_stat();
ts.setPseudorangeValid(bit_to_bool(trk_stat, 0));
ts.setCarrierPhaseValid(bit_to_bool(trk_stat, 1));
ts.setHalfCycleValid(bit_to_bool(trk_stat, 2));
ts.setHalfCycleSubtracted(bit_to_bool(trk_stat, 3));
}
mr.setNumMeas(msg->num_meas());
auto rs = mr.initReceiverStatus();
rs.setLeapSecValid(bit_to_bool(msg->rec_stat(), 0));
rs.setClkReset(bit_to_bool(msg->rec_stat(), 2));
return capnp::messageToFlatArray(msg_builder);
}
kj::Array<capnp::word> UbloxMsgParser::gen_nav_sat(ubx_t::nav_sat_t *msg) {
MessageBuilder msg_builder;
auto sr = msg_builder.initEvent().initUbloxGnss().initSatReport();
sr.setITow(msg->itow());
auto svs = sr.initSvs(msg->num_svs());
auto svs_data = *msg->svs();
for (int8_t i = 0; i < msg->num_svs(); i++) {
svs[i].setSvId(svs_data[i]->sv_id());
svs[i].setGnssId(svs_data[i]->gnss_id());
svs[i].setFlagsBitfield(svs_data[i]->flags());
svs[i].setCno(svs_data[i]->cno());
svs[i].setElevationDeg(svs_data[i]->elev());
svs[i].setAzimuthDeg(svs_data[i]->azim());
svs[i].setPseudorangeResidual(svs_data[i]->pr_res() * 0.1);
}
return capnp::messageToFlatArray(msg_builder);
}
kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw(ubx_t::mon_hw_t *msg) {
MessageBuilder msg_builder;
auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus();
hwStatus.setNoisePerMS(msg->noise_per_ms());
hwStatus.setFlags(msg->flags());
hwStatus.setAgcCnt(msg->agc_cnt());
hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->a_status());
hwStatus.setAPower((cereal::UbloxGnss::HwStatus::AntennaPowerStatus) msg->a_power());
hwStatus.setJamInd(msg->jam_ind());
return capnp::messageToFlatArray(msg_builder);
}
kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw2(ubx_t::mon_hw2_t *msg) {
MessageBuilder msg_builder;
auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus2();
hwStatus.setOfsI(msg->ofs_i());
hwStatus.setMagI(msg->mag_i());
hwStatus.setOfsQ(msg->ofs_q());
hwStatus.setMagQ(msg->mag_q());
switch (msg->cfg_source()) {
case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_ROM:
hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::ROM);
break;
case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_OTP:
hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::OTP);
break;
case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_CONFIG_PINS:
hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::CONFIGPINS);
break;
case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_FLASH:
hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::FLASH);
break;
default:
hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::UNDEFINED);
break;
}
hwStatus.setLowLevCfg(msg->low_lev_cfg());
hwStatus.setPostStatus(msg->post_status());
return capnp::messageToFlatArray(msg_builder);
}
system/ubloxd/ublox_msg.h
-131
View File
@@ -1,131 +0,0 @@
#pragma once
#include <cassert>
#include <cstdint>
#include <ctime>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include "cereal/messaging/messaging.h"
#include "common/util.h"
#include "system/ubloxd/generated/gps.h"
#include "system/ubloxd/generated/glonass.h"
#include "system/ubloxd/generated/ubx.h"
using namespace std::string_literals;
const int SECS_IN_MIN = 60;
const int SECS_IN_HR = 60 * SECS_IN_MIN;
const int SECS_IN_DAY = 24 * SECS_IN_HR;
const int SECS_IN_WEEK = 7 * SECS_IN_DAY;
// protocol constants
namespace ublox {
const uint8_t PREAMBLE1 = 0xb5;
const uint8_t PREAMBLE2 = 0x62;
const int UBLOX_HEADER_SIZE = 6;
const int UBLOX_CHECKSUM_SIZE = 2;
const int UBLOX_MAX_MSG_SIZE = 65536;
struct ubx_mga_ini_time_utc_t {
uint8_t type;
uint8_t version;
uint8_t ref;
int8_t leapSecs;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t minute;
uint8_t second;
uint8_t reserved1;
uint32_t ns;
uint16_t tAccS;
uint16_t reserved2;
uint32_t tAccNs;
} __attribute__((packed));
inline std::string ubx_add_checksum(const std::string &msg) {
assert(msg.size() > 2);
uint8_t ck_a = 0, ck_b = 0;
for (int i = 2; i < msg.size(); i++) {
ck_a = (ck_a + msg[i]) & 0xFF;
ck_b = (ck_b + ck_a) & 0xFF;
}
std::string r = msg;
r.push_back(ck_a);
r.push_back(ck_b);
return r;
}
inline std::string build_ubx_mga_ini_time_utc(struct tm time) {
ublox::ubx_mga_ini_time_utc_t payload = {
.type = 0x10,
.version = 0x0,
.ref = 0x0,
.leapSecs = -128, // Unknown
.year = (uint16_t)(1900 + time.tm_year),
.month = (uint8_t)(1 + time.tm_mon),
.day = (uint8_t)time.tm_mday,
.hour = (uint8_t)time.tm_hour,
.minute = (uint8_t)time.tm_min,
.second = (uint8_t)time.tm_sec,
.reserved1 = 0x0,
.ns = 0,
.tAccS = 30,
.reserved2 = 0x0,
.tAccNs = 0,
};
assert(sizeof(payload) == 24);
std::string msg = "\xb5\x62\x13\x40\x18\x00"s;
msg += std::string((char*)&payload, sizeof(payload));
return ubx_add_checksum(msg);
}
}
class UbloxMsgParser {
public:
bool add_data(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed);
inline void reset() {bytes_in_parse_buf = 0;}
inline int needed_bytes();
inline std::string data() {return std::string((const char*)msg_parse_buf, bytes_in_parse_buf);}
std::pair<std::string, kj::Array<capnp::word>> gen_msg();
kj::Array<capnp::word> gen_nav_pvt(ubx_t::nav_pvt_t *msg);
kj::Array<capnp::word> gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg);
kj::Array<capnp::word> gen_rxm_rawx(ubx_t::rxm_rawx_t *msg);
kj::Array<capnp::word> gen_mon_hw(ubx_t::mon_hw_t *msg);
kj::Array<capnp::word> gen_mon_hw2(ubx_t::mon_hw2_t *msg);
kj::Array<capnp::word> gen_nav_sat(ubx_t::nav_sat_t *msg);
private:
inline bool valid_cheksum();
inline bool valid();
inline bool valid_so_far();
kj::Array<capnp::word> parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg);
kj::Array<capnp::word> parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg);
std::unordered_map<int, std::unordered_map<int, std::string>> gps_subframes;
float last_log_time = 0.0;
size_t bytes_in_parse_buf = 0;
uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE];
// user range accuracy in meters
const std::unordered_map<uint8_t, float> glonass_URA_lookup =
{{ 0, 1}, { 1, 2}, { 2, 2.5}, { 3, 4}, { 4, 5}, {5, 7},
{ 6, 10}, { 7, 12}, { 8, 14}, { 9, 16}, {10, 32},
{11, 64}, {12, 128}, {13, 256}, {14, 512}, {15, 1024}};
std::unordered_map<int, std::unordered_map<int, std::string>> glonass_strings;
std::unordered_map<int, std::unordered_map<int, long>> glonass_string_times;
std::unordered_map<int, std::unordered_map<int, int>> glonass_string_superframes;
};
system/ubloxd/ubloxd.cc
-62
View File
@@ -1,62 +0,0 @@
#include <cassert>
#include <kaitai/kaitaistream.h>
#include "cereal/messaging/messaging.h"
#include "common/swaglog.h"
#include "common/util.h"
#include "system/ubloxd/ublox_msg.h"
ExitHandler do_exit;
using namespace ublox;
int main() {
LOGW("starting ubloxd");
AlignedBuffer aligned_buf;
UbloxMsgParser parser;
PubMaster pm({"ubloxGnss", "gpsLocationExternal"});
std::unique_ptr<Context> context(Context::create());
std::unique_ptr<SubSocket> subscriber(SubSocket::create(context.get(), "ubloxRaw"));
assert(subscriber != NULL);
subscriber->setTimeout(100);
while (!do_exit) {
std::unique_ptr<Message> msg(subscriber->receive());
if (!msg) {
continue;
}
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto ubloxRaw = event.getUbloxRaw();
float log_time = 1e-9 * event.getLogMonoTime();
const uint8_t *data = ubloxRaw.begin();
size_t len = ubloxRaw.size();
size_t bytes_consumed = 0;
while (bytes_consumed < len && !do_exit) {
size_t bytes_consumed_this_time = 0U;
if (parser.add_data(log_time, data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) {
try {
auto ublox_msg = parser.gen_msg();
if (ublox_msg.second.size() > 0) {
auto bytes = ublox_msg.second.asBytes();
pm.send(ublox_msg.first.c_str(), bytes.begin(), bytes.size());
}
} catch (const std::exception& e) {
LOGE("Error parsing ublox message %s", e.what());
}
parser.reset();
}
bytes_consumed += bytes_consumed_this_time;
}
}
return 0;
}
system/ubloxd/ubloxd.py
+519
View File
@@ -0,0 +1,519 @@
#!/usr/bin/env python3
import math
import capnp
import calendar
import numpy as np
from collections import defaultdict
from dataclasses import dataclass
from cereal import log
from cereal import messaging
from openpilot.system.ubloxd.generated.ubx import Ubx
from openpilot.system.ubloxd.generated.gps import Gps
from openpilot.system.ubloxd.generated.glonass import Glonass
SECS_IN_MIN = 60
SECS_IN_HR = 60 * SECS_IN_MIN
SECS_IN_DAY = 24 * SECS_IN_HR
SECS_IN_WEEK = 7 * SECS_IN_DAY
class UbxFramer:
PREAMBLE1 = 0xB5
PREAMBLE2 = 0x62
HEADER_SIZE = 6
CHECKSUM_SIZE = 2
def __init__(self) -> None:
self.buf = bytearray()
self.last_log_time = 0.0
def reset(self) -> None:
self.buf.clear()
@staticmethod
def _checksum_ok(frame: bytes) -> bool:
ck_a = 0
ck_b = 0
for b in frame[2:-2]:
ck_a = (ck_a + b) & 0xFF
ck_b = (ck_b + ck_a) & 0xFF
return ck_a == frame[-2] and ck_b == frame[-1]
def add_data(self, log_time: float, incoming: bytes) -> list[bytes]:
self.last_log_time = log_time
out: list[bytes] = []
if not incoming:
return out
self.buf += incoming
while True:
# find preamble
if len(self.buf) < 2:
break
start = self.buf.find(b"\xB5\x62")
if start < 0:
# no preamble in buffer
self.buf.clear()
break
if start > 0:
# drop garbage before preamble
self.buf = self.buf[start:]
if len(self.buf) < self.HEADER_SIZE:
break
length_le = int.from_bytes(self.buf[4:6], 'little', signed=False)
total_len = self.HEADER_SIZE + length_le + self.CHECKSUM_SIZE
if len(self.buf) < total_len:
break
candidate = bytes(self.buf[:total_len])
if self._checksum_ok(candidate):
out.append(candidate)
# consume this frame
self.buf = self.buf[total_len:]
else:
# drop first byte and retry
self.buf = self.buf[1:]
return out
def _bit(b: int, shift: int) -> bool:
return (b & (1 << shift)) != 0
@dataclass
class EphemerisCaches:
gps_subframes: defaultdict[int, dict[int, bytes]]
glonass_strings: defaultdict[int, dict[int, bytes]]
glonass_string_times: defaultdict[int, dict[int, float]]
glonass_string_superframes: defaultdict[int, dict[int, int]]
class UbloxMsgParser:
gpsPi = 3.1415926535898
# user range accuracy in meters
glonass_URA_lookup: dict[int, float] = {
0: 1, 1: 2, 2: 2.5, 3: 4, 4: 5, 5: 7,
6: 10, 7: 12, 8: 14, 9: 16, 10: 32,
11: 64, 12: 128, 13: 256, 14: 512, 15: 1024,
}
def __init__(self) -> None:
self.framer = UbxFramer()
self.caches = EphemerisCaches(
gps_subframes=defaultdict(dict),
glonass_strings=defaultdict(dict),
glonass_string_times=defaultdict(dict),
glonass_string_superframes=defaultdict(dict),
)
# Message generation entry point
def parse_frame(self, frame: bytes) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder] | None:
# Quick header parse
msg_type = int.from_bytes(frame[2:4], 'big')
payload = frame[6:-2]
if msg_type == 0x0107:
body = Ubx.NavPvt.from_bytes(payload)
return self._gen_nav_pvt(body)
if msg_type == 0x0213:
# Manually parse RXM-SFRBX to avoid Kaitai EOF on some frames
if len(payload) < 8:
return None
gnss_id = payload[0]
sv_id = payload[1]
freq_id = payload[3]
num_words = payload[4]
exp = 8 + 4 * num_words
if exp != len(payload):
return None
words: list[int] = []
off = 8
for _ in range(num_words):
words.append(int.from_bytes(payload[off:off+4], 'little'))
off += 4
class _SfrbxView:
def __init__(self, gid: int, sid: int, fid: int, body: list[int]):
self.gnss_id = Ubx.GnssType(gid)
self.sv_id = sid
self.freq_id = fid
self.body = body
view = _SfrbxView(gnss_id, sv_id, freq_id, words)
return self._gen_rxm_sfrbx(view)
if msg_type == 0x0215:
body = Ubx.RxmRawx.from_bytes(payload)
return self._gen_rxm_rawx(body)
if msg_type == 0x0A09:
body = Ubx.MonHw.from_bytes(payload)
return self._gen_mon_hw(body)
if msg_type == 0x0A0B:
body = Ubx.MonHw2.from_bytes(payload)
return self._gen_mon_hw2(body)
if msg_type == 0x0135:
body = Ubx.NavSat.from_bytes(payload)
return self._gen_nav_sat(body)
return None
# NAV-PVT -> gpsLocationExternal
def _gen_nav_pvt(self, msg: Ubx.NavPvt) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]:
dat = messaging.new_message('gpsLocationExternal', valid=True)
gps = dat.gpsLocationExternal
gps.source = log.GpsLocationData.SensorSource.ublox
gps.flags = msg.flags
gps.hasFix = (msg.flags % 2) == 1
gps.latitude = msg.lat * 1e-07
gps.longitude = msg.lon * 1e-07
gps.altitude = msg.height * 1e-03
gps.speed = msg.g_speed * 1e-03
gps.bearingDeg = msg.head_mot * 1e-5
gps.horizontalAccuracy = msg.h_acc * 1e-03
gps.satelliteCount = msg.num_sv
# build UTC timestamp millis (NAV-PVT is in UTC)
# tolerate invalid or unset date values like C++ timegm
try:
utc_tt = calendar.timegm((msg.year, msg.month, msg.day, msg.hour, msg.min, msg.sec, 0, 0, 0))
except Exception:
utc_tt = 0
gps.unixTimestampMillis = int(utc_tt * 1e3 + (msg.nano * 1e-6))
# match C++ float32 rounding semantics exactly
gps.vNED = [
float(np.float32(msg.vel_n) * np.float32(1e-03)),
float(np.float32(msg.vel_e) * np.float32(1e-03)),
float(np.float32(msg.vel_d) * np.float32(1e-03)),
]
gps.verticalAccuracy = msg.v_acc * 1e-03
gps.speedAccuracy = msg.s_acc * 1e-03
gps.bearingAccuracyDeg = msg.head_acc * 1e-05
return ('gpsLocationExternal', dat)
# RXM-SFRBX dispatch to GPS or GLONASS ephemeris
def _gen_rxm_sfrbx(self, msg) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder] | None:
if msg.gnss_id == Ubx.GnssType.gps:
return self._parse_gps_ephemeris(msg)
if msg.gnss_id == Ubx.GnssType.glonass:
return self._parse_glonass_ephemeris(msg)
return None
def _parse_gps_ephemeris(self, msg: Ubx.RxmSfrbx) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder] | None:
# body is list of 10 words; convert to 30-byte subframe (strip parity/padding)
body = msg.body
if len(body) != 10:
return None
subframe_data = bytearray()
for word in body:
word >>= 6
subframe_data.append((word >> 16) & 0xFF)
subframe_data.append((word >> 8) & 0xFF)
subframe_data.append(word & 0xFF)
sf = Gps.from_bytes(bytes(subframe_data))
subframe_id = sf.how.subframe_id
if subframe_id < 1 or subframe_id > 3:
return None
self.caches.gps_subframes[msg.sv_id][subframe_id] = bytes(subframe_data)
if len(self.caches.gps_subframes[msg.sv_id]) != 3:
return None
dat = messaging.new_message('ubloxGnss', valid=True)
eph = dat.ubloxGnss.init('ephemeris')
eph.svId = msg.sv_id
iode_s2 = 0
iode_s3 = 0
iodc_lsb = 0
week = 0
# Subframe 1
sf1 = Gps.from_bytes(self.caches.gps_subframes[msg.sv_id][1])
s1 = sf1.body
assert isinstance(s1, Gps.Subframe1)
week = s1.week_no
week += 1024
if week < 1877:
week += 1024
eph.tgd = s1.t_gd * math.pow(2, -31)
eph.toc = s1.t_oc * math.pow(2, 4)
eph.af2 = s1.af_2 * math.pow(2, -55)
eph.af1 = s1.af_1 * math.pow(2, -43)
eph.af0 = s1.af_0 * math.pow(2, -31)
eph.svHealth = s1.sv_health
eph.towCount = sf1.how.tow_count
iodc_lsb = s1.iodc_lsb
# Subframe 2
sf2 = Gps.from_bytes(self.caches.gps_subframes[msg.sv_id][2])
s2 = sf2.body
assert isinstance(s2, Gps.Subframe2)
if s2.t_oe == 0 and sf2.how.tow_count * 6 >= (SECS_IN_WEEK - 2 * SECS_IN_HR):
week += 1
eph.crs = s2.c_rs * math.pow(2, -5)
eph.deltaN = s2.delta_n * math.pow(2, -43) * self.gpsPi
eph.m0 = s2.m_0 * math.pow(2, -31) * self.gpsPi
eph.cuc = s2.c_uc * math.pow(2, -29)
eph.ecc = s2.e * math.pow(2, -33)
eph.cus = s2.c_us * math.pow(2, -29)
eph.a = math.pow(s2.sqrt_a * math.pow(2, -19), 2.0)
eph.toe = s2.t_oe * math.pow(2, 4)
iode_s2 = s2.iode
# Subframe 3
sf3 = Gps.from_bytes(self.caches.gps_subframes[msg.sv_id][3])
s3 = sf3.body
assert isinstance(s3, Gps.Subframe3)
eph.cic = s3.c_ic * math.pow(2, -29)
eph.omega0 = s3.omega_0 * math.pow(2, -31) * self.gpsPi
eph.cis = s3.c_is * math.pow(2, -29)
eph.i0 = s3.i_0 * math.pow(2, -31) * self.gpsPi
eph.crc = s3.c_rc * math.pow(2, -5)
eph.omega = s3.omega * math.pow(2, -31) * self.gpsPi
eph.omegaDot = s3.omega_dot * math.pow(2, -43) * self.gpsPi
eph.iode = s3.iode
eph.iDot = s3.idot * math.pow(2, -43) * self.gpsPi
iode_s3 = s3.iode
eph.toeWeek = week
eph.tocWeek = week
# clear cache for this SV
self.caches.gps_subframes[msg.sv_id].clear()
if not (iodc_lsb == iode_s2 == iode_s3):
return None
return ('ubloxGnss', dat)
def _parse_glonass_ephemeris(self, msg: Ubx.RxmSfrbx) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder] | None:
# words are 4 bytes each; Glonass parser expects 16 bytes (string)
body = msg.body
if len(body) != 4:
return None
string_bytes = bytearray()
for word in body:
for i in (3, 2, 1, 0):
string_bytes.append((word >> (8 * i)) & 0xFF)
gl = Glonass.from_bytes(bytes(string_bytes))
string_number = gl.string_number
if string_number < 1 or string_number > 5 or gl.idle_chip:
return None
# correlate by superframe and timing, similar to C++ logic
freq_id = msg.freq_id
superframe_unknown = False
needs_clear = False
for i in range(1, 6):
if i not in self.caches.glonass_strings[freq_id]:
continue
sf_prev = self.caches.glonass_string_superframes[freq_id].get(i, 0)
if sf_prev == 0 or gl.superframe_number == 0:
superframe_unknown = True
elif sf_prev != gl.superframe_number:
needs_clear = True
if superframe_unknown:
prev_time = self.caches.glonass_string_times[freq_id].get(i, 0.0)
if abs((prev_time - 2.0 * i) - (self.framer.last_log_time - 2.0 * string_number)) > 10:
needs_clear = True
if needs_clear:
self.caches.glonass_strings[freq_id].clear()
self.caches.glonass_string_superframes[freq_id].clear()
self.caches.glonass_string_times[freq_id].clear()
self.caches.glonass_strings[freq_id][string_number] = bytes(string_bytes)
self.caches.glonass_string_superframes[freq_id][string_number] = gl.superframe_number
self.caches.glonass_string_times[freq_id][string_number] = self.framer.last_log_time
if msg.sv_id == 255:
# unknown SV id
return None
if len(self.caches.glonass_strings[freq_id]) != 5:
return None
dat = messaging.new_message('ubloxGnss', valid=True)
eph = dat.ubloxGnss.init('glonassEphemeris')
eph.svId = msg.sv_id
eph.freqNum = msg.freq_id - 7
current_day = 0
tk = 0
# string 1
try:
s1 = Glonass.from_bytes(self.caches.glonass_strings[freq_id][1]).data
except Exception:
return None
assert isinstance(s1, Glonass.String1)
eph.p1 = int(s1.p1)
tk = int(s1.t_k)
eph.tkDEPRECATED = tk
eph.xVel = float(s1.x_vel) * math.pow(2, -20)
eph.xAccel = float(s1.x_accel) * math.pow(2, -30)
eph.x = float(s1.x) * math.pow(2, -11)
# string 2
try:
s2 = Glonass.from_bytes(self.caches.glonass_strings[freq_id][2]).data
except Exception:
return None
assert isinstance(s2, Glonass.String2)
eph.svHealth = int(s2.b_n >> 2)
eph.p2 = int(s2.p2)
eph.tb = int(s2.t_b)
eph.yVel = float(s2.y_vel) * math.pow(2, -20)
eph.yAccel = float(s2.y_accel) * math.pow(2, -30)
eph.y = float(s2.y) * math.pow(2, -11)
# string 3
try:
s3 = Glonass.from_bytes(self.caches.glonass_strings[freq_id][3]).data
except Exception:
return None
assert isinstance(s3, Glonass.String3)
eph.p3 = int(s3.p3)
eph.gammaN = float(s3.gamma_n) * math.pow(2, -40)
eph.svHealth = int(eph.svHealth | (1 if s3.l_n else 0))
eph.zVel = float(s3.z_vel) * math.pow(2, -20)
eph.zAccel = float(s3.z_accel) * math.pow(2, -30)
eph.z = float(s3.z) * math.pow(2, -11)
# string 4
try:
s4 = Glonass.from_bytes(self.caches.glonass_strings[freq_id][4]).data
except Exception:
return None
assert isinstance(s4, Glonass.String4)
current_day = int(s4.n_t)
eph.nt = current_day
eph.tauN = float(s4.tau_n) * math.pow(2, -30)
eph.deltaTauN = float(s4.delta_tau_n) * math.pow(2, -30)
eph.age = int(s4.e_n)
eph.p4 = int(s4.p4)
eph.svURA = float(self.glonass_URA_lookup.get(int(s4.f_t), 0.0))
# consistency check: SV slot number
# if it doesn't match, keep going but note mismatch (no logging here)
eph.svType = int(s4.m)
# string 5
try:
s5 = Glonass.from_bytes(self.caches.glonass_strings[freq_id][5]).data
except Exception:
return None
assert isinstance(s5, Glonass.String5)
eph.n4 = int(s5.n_4)
tk_seconds = int(SECS_IN_HR * ((tk >> 7) & 0x1F) + SECS_IN_MIN * ((tk >> 1) & 0x3F) + (tk & 0x1) * 30)
eph.tkSeconds = tk_seconds
self.caches.glonass_strings[freq_id].clear()
return ('ubloxGnss', dat)
def _gen_rxm_rawx(self, msg: Ubx.RxmRawx) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]:
dat = messaging.new_message('ubloxGnss', valid=True)
mr = dat.ubloxGnss.init('measurementReport')
mr.rcvTow = msg.rcv_tow
mr.gpsWeek = msg.week
mr.leapSeconds = msg.leap_s
mb = mr.init('measurements', msg.num_meas)
for i, m in enumerate(msg.meas):
mb[i].svId = m.sv_id
mb[i].pseudorange = m.pr_mes
mb[i].carrierCycles = m.cp_mes
mb[i].doppler = m.do_mes
mb[i].gnssId = int(m.gnss_id.value)
mb[i].glonassFrequencyIndex = m.freq_id
mb[i].locktime = m.lock_time
mb[i].cno = m.cno
mb[i].pseudorangeStdev = 0.01 * (math.pow(2, (m.pr_stdev & 15)))
mb[i].carrierPhaseStdev = 0.004 * (m.cp_stdev & 15)
mb[i].dopplerStdev = 0.002 * (math.pow(2, (m.do_stdev & 15)))
ts = mb[i].init('trackingStatus')
trk = m.trk_stat
ts.pseudorangeValid = _bit(trk, 0)
ts.carrierPhaseValid = _bit(trk, 1)
ts.halfCycleValid = _bit(trk, 2)
ts.halfCycleSubtracted = _bit(trk, 3)
mr.numMeas = msg.num_meas
rs = mr.init('receiverStatus')
rs.leapSecValid = _bit(msg.rec_stat, 0)
rs.clkReset = _bit(msg.rec_stat, 2)
return ('ubloxGnss', dat)
def _gen_nav_sat(self, msg: Ubx.NavSat) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]:
dat = messaging.new_message('ubloxGnss', valid=True)
sr = dat.ubloxGnss.init('satReport')
sr.iTow = msg.itow
svs = sr.init('svs', msg.num_svs)
for i, s in enumerate(msg.svs):
svs[i].svId = s.sv_id
svs[i].gnssId = int(s.gnss_id.value)
svs[i].flagsBitfield = s.flags
svs[i].cno = s.cno
svs[i].elevationDeg = s.elev
svs[i].azimuthDeg = s.azim
svs[i].pseudorangeResidual = s.pr_res * 0.1
return ('ubloxGnss', dat)
def _gen_mon_hw(self, msg: Ubx.MonHw) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]:
dat = messaging.new_message('ubloxGnss', valid=True)
hw = dat.ubloxGnss.init('hwStatus')
hw.noisePerMS = msg.noise_per_ms
hw.flags = msg.flags
hw.agcCnt = msg.agc_cnt
hw.aStatus = int(msg.a_status.value)
hw.aPower = int(msg.a_power.value)
hw.jamInd = msg.jam_ind
return ('ubloxGnss', dat)
def _gen_mon_hw2(self, msg: Ubx.MonHw2) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]:
dat = messaging.new_message('ubloxGnss', valid=True)
hw = dat.ubloxGnss.init('hwStatus2')
hw.ofsI = msg.ofs_i
hw.magI = msg.mag_i
hw.ofsQ = msg.ofs_q
hw.magQ = msg.mag_q
# Map Ubx enum to cereal enum {undefined=0, rom=1, otp=2, configpins=3, flash=4}
cfg_map = {
Ubx.MonHw2.ConfigSource.rom: 1,
Ubx.MonHw2.ConfigSource.otp: 2,
Ubx.MonHw2.ConfigSource.config_pins: 3,
Ubx.MonHw2.ConfigSource.flash: 4,
}
hw.cfgSource = cfg_map.get(msg.cfg_source, 0)
hw.lowLevCfg = msg.low_lev_cfg
hw.postStatus = msg.post_status
return ('ubloxGnss', dat)
def main():
parser = UbloxMsgParser()
pm = messaging.PubMaster(['ubloxGnss', 'gpsLocationExternal'])
sock = messaging.sub_sock('ubloxRaw', timeout=100, conflate=False)
while True:
msg = messaging.recv_one(sock)
if msg is None:
continue
data = bytes(msg.ubloxRaw)
log_time = msg.logMonoTime * 1e-9
frames = parser.framer.add_data(log_time, data)
for frame in frames:
try:
res = parser.parse_frame(frame)
except Exception:
continue
if not res:
continue
service, dat = res
pm.send(service, dat)
if __name__ == '__main__':
main()
system/updated/updated.py
+2 -2
View File
@@ -31,8 +31,8 @@ FINALIZED = os.path.join(STAGING_ROOT, "finalized")
OVERLAY_INIT = Path(os.path.join(BASEDIR, ".overlay_init"))
DAYS_NO_CONNECTIVITY_MAX = 14 # do not allow to engage after this many days
DAYS_NO_CONNECTIVITY_PROMPT = 10 # send an offroad prompt after this many days
DAYS_NO_CONNECTIVITY_MAX = 1400 # do not allow to engage after this many days
DAYS_NO_CONNECTIVITY_PROMPT = 1000 # send an offroad prompt after this many days
class UserRequest:
NONE = 0