mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-07-25 11:22:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'upstream/openpilot/master' into sync-20250908

Browse Source 
# Conflicts:
#	.github/workflows/selfdrive_tests.yaml
#	README.md
#	docs/CARS.md
#	opendbc_repo
#	panda
#	selfdrive/car/tests/test_car_interfaces.py
#	selfdrive/modeld/modeld.py
#	selfdrive/selfdrived/selfdrived.py
#	selfdrive/ui/translations/main_ar.ts
#	selfdrive/ui/translations/main_de.ts
#	selfdrive/ui/translations/main_es.ts
#	selfdrive/ui/translations/main_fr.ts
#	selfdrive/ui/translations/main_ja.ts
#	selfdrive/ui/translations/main_ko.ts
#	selfdrive/ui/translations/main_pt-BR.ts
#	selfdrive/ui/translations/main_th.ts
#	selfdrive/ui/translations/main_tr.ts
#	selfdrive/ui/translations/main_zh-CHS.ts
#	selfdrive/ui/translations/main_zh-CHT.ts
#	system/hardware/hardwared.py
#	system/updated/updated.py
#	tinygrad_repo
#	uv.lock
This commit is contained in:
Jason Wen
2025-09-12 12:15:40 -04:00
parent 4f44d6e643 c9dbf97649
commit ec8f036850
136 changed files with 4744 additions and 6884 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.github/workflows/selfdrive_tests.yaml
+2 -3
View File
@@ -107,7 +107,6 @@ jobs:
build_mac:
name: build macOS
if: false # temp disable since homebrew install is getting stuck
runs-on: ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-profile-macos-8x14' || 'macos-latest' }}
steps:
- uses: actions/checkout@v4
@@ -125,8 +124,8 @@ jobs:
- name: Install dependencies
run: ./tools/mac_setup.sh
env:
# package install has DeprecationWarnings
PYTHONWARNINGS: default
PYTHONWARNINGS: default # package install has DeprecationWarnings
HOMEBREW_DISPLAY_INSTALL_TIMES: 1
- name: Save Homebrew cache
uses: actions/cache/save@v4
if: github.ref == 'refs/heads/master'
.gitignore
-1
View File
@@ -50,7 +50,6 @@ cereal/services.h
cereal/gen
cereal/messaging/bridge
selfdrive/mapd/default_speeds_by_region.json
system/proclogd/proclogd
selfdrive/ui/translations/tmp
selfdrive/test/longitudinal_maneuvers/out
selfdrive/car/tests/cars_dump
.vscode/launch.json
Vendored
+40 -1
View File
@@ -23,6 +23,11 @@
"id": "args",
"description": "Arguments to pass to the process",
"type": "promptString"
},
{
"id": "replayArg",
"type": "promptString",
"description": "Enter route or segment to replay."
}
],
"configurations": [
@@ -40,7 +45,41 @@
"type": "cppdbg",
"request": "launch",
"program": "${workspaceFolder}/${input:cpp_process}",
"cwd": "${workspaceFolder}",
"cwd": "${workspaceFolder}"
},
{
"name": "Attach LLDB to Replay drive",
"type": "lldb",
"request": "attach",
"pid": "${command:pickMyProcess}",
"initCommands": [
"script import time; time.sleep(3)"
]
},
{
"name": "Replay drive",
"type": "debugpy",
"request": "launch",
"program": "${workspaceFolder}/opendbc/safety/tests/safety_replay/replay_drive.py",
"args": [
"${input:replayArg}"
],
"console": "integratedTerminal",
"justMyCode": false,
"env": {
"PYTHONPATH": "${workspaceFolder}"
},
"subProcess": true,
"stopOnEntry": false
}
],
"compounds": [
{
"name": "Replay drive + Safety LLDB",
"configurations": [
"Replay drive",
"Attach LLDB to Replay drive"
]
}
]
}
Jenkinsfile
Vendored
+9 -23
View File
@@ -178,7 +178,7 @@ node {
try {
if (env.BRANCH_NAME == 'devel-staging') {
deviceStage("build release3-staging", "tici-needs-can", [], [
deviceStage("build release3-staging", "tizi-needs-can", [], [
step("build release3-staging", "RELEASE_BRANCH=release3-staging $SOURCE_DIR/release/build_release.sh"),
])
}
@@ -186,12 +186,12 @@ node {
if (env.BRANCH_NAME == '__nightly') {
parallel (
'nightly': {
deviceStage("build nightly", "tici-needs-can", [], [
deviceStage("build nightly", "tizi-needs-can", [], [
step("build nightly", "RELEASE_BRANCH=nightly $SOURCE_DIR/release/build_release.sh"),
])
},
'nightly-dev': {
deviceStage("build nightly-dev", "tici-needs-can", [], [
deviceStage("build nightly-dev", "tizi-needs-can", [], [
step("build nightly-dev", "PANDA_DEBUG_BUILD=1 RELEASE_BRANCH=nightly-dev $SOURCE_DIR/release/build_release.sh"),
])
},
@@ -200,39 +200,30 @@ node {
if (!env.BRANCH_NAME.matches(excludeRegex)) {
parallel (
// tici tests
'onroad tests': {
deviceStage("onroad", "tici-needs-can", ["UNSAFE=1"], [
deviceStage("onroad", "tizi-needs-can", ["UNSAFE=1"], [
step("build openpilot", "cd system/manager && ./build.py"),
step("check dirty", "release/check-dirty.sh"),
step("onroad tests", "pytest selfdrive/test/test_onroad.py -s", [timeout: 60]),
])
},
'HW + Unit Tests': {
deviceStage("tici-hardware", "tici-common", ["UNSAFE=1"], [
deviceStage("tizi-hardware", "tizi-common", ["UNSAFE=1"], [
step("build", "cd system/manager && ./build.py"),
step("test pandad", "pytest selfdrive/pandad/tests/test_pandad.py", [diffPaths: ["panda", "selfdrive/pandad/"]]),
step("test power draw", "pytest -s system/hardware/tici/tests/test_power_draw.py"),
step("test encoder", "LD_LIBRARY_PATH=/usr/local/lib pytest system/loggerd/tests/test_encoder.py", [diffPaths: ["system/loggerd/"]]),
step("test pigeond", "pytest system/ubloxd/tests/test_pigeond.py", [diffPaths: ["system/ubloxd/"]]),
step("test manager", "pytest system/manager/test/test_manager.py"),
])
},
'loopback': {
deviceStage("loopback", "tici-loopback", ["UNSAFE=1"], [
deviceStage("loopback", "tizi-loopback", ["UNSAFE=1"], [
step("build openpilot", "cd system/manager && ./build.py"),
step("test pandad loopback", "pytest selfdrive/pandad/tests/test_pandad_loopback.py"),
])
},
'camerad AR0231': {
deviceStage("AR0231", "tici-ar0231", ["UNSAFE=1"], [
step("build", "cd system/manager && ./build.py"),
step("test camerad", "pytest system/camerad/test/test_camerad.py", [timeout: 60]),
step("test exposure", "pytest system/camerad/test/test_exposure.py"),
])
},
'camerad OX03C10': {
deviceStage("OX03C10", "tici-ox03c10", ["UNSAFE=1"], [
deviceStage("OX03C10", "tizi-ox03c10", ["UNSAFE=1"], [
step("build", "cd system/manager && ./build.py"),
step("test camerad", "pytest system/camerad/test/test_camerad.py", [timeout: 60]),
step("test exposure", "pytest system/camerad/test/test_exposure.py"),
@@ -246,17 +237,13 @@ node {
])
},
'sensord': {
deviceStage("LSM + MMC", "tici-lsmc", ["UNSAFE=1"], [
step("build", "cd system/manager && ./build.py"),
step("test sensord", "pytest system/sensord/tests/test_sensord.py"),
])
deviceStage("BMX + LSM", "tici-bmx-lsm", ["UNSAFE=1"], [
deviceStage("LSM + MMC", "tizi-lsmc", ["UNSAFE=1"], [
step("build", "cd system/manager && ./build.py"),
step("test sensord", "pytest system/sensord/tests/test_sensord.py"),
])
},
'replay': {
deviceStage("model-replay", "tici-replay", ["UNSAFE=1"], [
deviceStage("model-replay", "tizi-replay", ["UNSAFE=1"], [
step("build", "cd system/manager && ./build.py", [diffPaths: ["selfdrive/modeld/", "tinygrad_repo", "selfdrive/test/process_replay/model_replay.py"]]),
step("model replay", "selfdrive/test/process_replay/model_replay.py", [diffPaths: ["selfdrive/modeld/", "tinygrad_repo", "selfdrive/test/process_replay/model_replay.py"]]),
])
@@ -266,7 +253,6 @@ node {
step("build openpilot", "cd system/manager && ./build.py"),
step("test pandad loopback", "SINGLE_PANDA=1 pytest selfdrive/pandad/tests/test_pandad_loopback.py"),
step("test pandad spi", "pytest selfdrive/pandad/tests/test_pandad_spi.py"),
step("test pandad", "pytest selfdrive/pandad/tests/test_pandad.py", [diffPaths: ["panda", "selfdrive/pandad/"]]),
step("test amp", "pytest system/hardware/tici/tests/test_amplifier.py"),
// TODO: enable once new AGNOS is available
// step("test esim", "pytest system/hardware/tici/tests/test_esim.py"),
RELEASES.md
+8 -2
View File
@@ -1,7 +1,13 @@
Version 0.10.1 (2025-09-08)
========================
* Record driving feedback using LKAS button
* Honda City 2023 support thanks to drFritz!
* New driving model
* World Model: removed global localization inputs
* World Model: 2x the number of parameters
* World Model: trained on 4x the number of segments
* Driving Vision Model: trained on 4x the number of segments
* Honda City 2023 support thanks to vanillagorillaa and drFritz!
* Honda N-Box 2018 support thanks to miettal!
* Honda Odyssey 2021-25 support thanks to csouers and MVL!
Version 0.10.0 (2025-08-05)
========================
SConstruct
-5
View File
@@ -359,11 +359,6 @@ SConscript([
'system/ubloxd/SConscript',
'system/loggerd/SConscript',
])
if arch != "Darwin":
SConscript([
'system/logcatd/SConscript',
'system/proclogd/SConscript',
])
if arch == "larch64":
SConscript(['system/camerad/SConscript'])
cereal/log.capnp
+1 -1
View File
@@ -585,7 +585,6 @@ struct PandaState @0xa7649e2575e4591e {
heartbeatLost @22 :Bool;
interruptLoad @25 :Float32;
fanPower @28 :UInt8;
fanStallCount @34 :UInt8;
spiErrorCount @33 :UInt16;
@@ -714,6 +713,7 @@ struct PandaState @0xa7649e2575e4591e {
usbPowerModeDEPRECATED @12 :PeripheralState.UsbPowerModeDEPRECATED;
safetyParamDEPRECATED @20 :Int16;
safetyParam2DEPRECATED @26 :UInt32;
fanStallCountDEPRECATED @34 :UInt8;
}
struct PeripheralState {
cereal/messaging/socketmaster.cc
+1 -1
View File
@@ -33,7 +33,7 @@ MessageContext message_context;
struct SubMaster::SubMessage {
std::string name;
SubSocket *socket = nullptr;
int freq = 0;
float freq = 0.0f;
bool updated = false, alive = false, valid = false, ignore_alive;
uint64_t rcv_time = 0, rcv_frame = 0;
void *allocated_msg_reader = nullptr;
cereal/services.py
+2 -2
View File
@@ -121,12 +121,12 @@ def build_header():
h += "#include <map>\n"
h += "#include <string>\n"
h += "struct service { std::string name; bool should_log; int frequency; int decimation; };\n"
h += "struct service { std::string name; bool should_log; float frequency; int decimation; };\n"
h += "static std::map<std::string, service> services = {\n"
for k, v in SERVICE_LIST.items():
should_log = "true" if v.should_log else "false"
decimation = -1 if v.decimation is None else v.decimation
h += ' { "%s", {"%s", %s, %d, %d}},\n' % \
h += ' { "%s", {"%s", %s, %f, %d}},\n' % \
(k, k, should_log, v.frequency, decimation)
h += "};\n"
common/model.h
+1 -1
View File
@@ -1 +1 @@
#define DEFAULT_MODEL "Steam Powered (Default)"
#define DEFAULT_MODEL "Firehose (Default)"
common/params_keys.h
-1
View File
@@ -94,7 +94,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"Offroad_NeosUpdate", {CLEAR_ON_MANAGER_START, JSON}},
{"Offroad_NoFirmware", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
{"Offroad_Recalibration", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}},
{"Offroad_StorageMissing", {CLEAR_ON_MANAGER_START, JSON}},
{"Offroad_TemperatureTooHigh", {CLEAR_ON_MANAGER_START, JSON}},
{"Offroad_UnregisteredHardware", {CLEAR_ON_MANAGER_START, JSON}},
{"Offroad_UpdateFailed", {CLEAR_ON_MANAGER_START, JSON}},
docs/CONTRIBUTING.md
+1 -1
View File
@@ -39,7 +39,7 @@ All of these are examples of good PRs:
### First contribution
[Projects / openpilot bounties](https://github.com/orgs/commaai/projects/26/views/1?pane=info) is the best place to get started and goes in-depth on what's expected when working on a bounty.
There's lot of bounties that don't require a comma 3/3X or a car.
There's lot of bounties that don't require a comma 3X or a car.
## Pull Requests
docs/DEBUGGING_SAFETY.md
+30
View File
@@ -0,0 +1,30 @@
# Debugging Panda Safety with Replay Drive + LLDB
## 1. Start the debugger in VS Code
* Select **Replay drive + Safety LLDB**.
* Enter the route or segment when prompted.
[<img src="https://github.com/user-attachments/assets/b0cc320a-083e-46a7-a9f8-ca775bbe5604">](https://github.com/user-attachments/assets/b0cc320a-083e-46a7-a9f8-ca775bbe5604)
## 2. Attach LLDB
* When prompted, pick the running **`replay_drive` process**.
* ⚠️ Attach quickly, or `replay_drive` will start consuming messages.
> [!TIP]
> Add a Python breakpoint at the start of `replay_drive.py` to pause execution and give yourself time to attach LLDB.
## 3. Set breakpoints in VS Code
Breakpoints can be set directly in `modes/xxx.h` (or any C file).
No extra LLDB commands are required — just place breakpoints in the editor.
## 4. Resume execution
Once attached, you can step through both Python (on the replay) and C safety code as CAN logs are replayed.
> [!NOTE]
> * Use short routes for quicker iteration.
> * Pause `replay_drive` early to avoid wasting log messages.
## Video
View a demo of this workflow on the PR that added it: https://github.com/commaai/openpilot/pull/36055#issue-3352911578
docs/SAFETY.md
+14 -4
View File
@@ -16,7 +16,7 @@ industry standards of safety for Level 2 Driver Assistance Systems. In particula
ISO26262 guidelines, including those from [pertinent documents](https://www.nhtsa.gov/sites/nhtsa.dot.gov/files/documents/13498a_812_573_alcsystemreport.pdf)
released by NHTSA. In addition, we impose strict coding guidelines (like [MISRA C : 2012](https://www.misra.org.uk/what-is-misra/))
on parts of openpilot that are safety relevant. We also perform software-in-the-loop,
hardware-in-the-loop and in-vehicle tests before each software release.
hardware-in-the-loop, and in-vehicle tests before each software release.
Following Hazard and Risk Analysis and FMEA, at a very high level, we have designed openpilot
ensuring two main safety requirements.
@@ -29,8 +29,18 @@ ensuring two main safety requirements.
For additional safety implementation details, refer to [panda safety model](https://github.com/commaai/panda#safety-model). For vehicle specific implementation of the safety concept, refer to [opendbc/safety/safety](https://github.com/commaai/opendbc/tree/master/opendbc/safety/safety).
**Extra note**: comma.ai strongly discourages the use of openpilot forks with safety code either missing or
not fully meeting the above requirements.
[^1]: For these actuator limits we observe ISO11270 and ISO15622. Lateral limits described there translate to 0.9 seconds of maximum actuation to achieve a 1m lateral deviation.
[^1]: For these actuator limits we observe ISO11270 and ISO15622. Lateral limits described there translate to 0.9 seconds of maximum actuation to achieve a 1m lateral deviation.
---
### Forks of openpilot
* Do not disable or nerf [driver monitoring](https://github.com/commaai/openpilot/tree/master/selfdrive/monitoring)
* Do not disable or nerf [excessive actuation checks](https://github.com/commaai/openpilot/tree/master/selfdrive/selfdrived/helpers.py)
* If your fork modifies any of the code in `opendbc/safety/`:
* your fork cannot use the openpilot trademark
* your fork must preserve the full [safety test suite](https://github.com/commaai/opendbc/tree/master/opendbc/safety/tests) and all tests must pass, including any new coverage required by the fork's changes
Failure to comply with these standards will get you and your users banned from comma.ai servers.
**comma.ai strongly discourages the use of openpilot forks with safety code either missing or not fully meeting the above requirements.**
docs/how-to/connect-to-comma.md
+4 -4
View File
@@ -1,11 +1,11 @@
# connect to a comma 3/3X
# connect to a comma 3X
A comma 3/3X is a normal [Linux](https://github.com/commaai/agnos-builder) computer that exposes [SSH](https://wiki.archlinux.org/title/Secure_Shell) and a [serial console](https://wiki.archlinux.org/title/Working_with_the_serial_console).
A comma 3X is a normal [Linux](https://github.com/commaai/agnos-builder) computer that exposes [SSH](https://wiki.archlinux.org/title/Secure_Shell) and a [serial console](https://wiki.archlinux.org/title/Working_with_the_serial_console).
## Serial Console
On both the comma three and 3X, the serial console is accessible from the main OBD-C port.
Connect the comma 3/3X to your computer with a normal USB C cable, or use a [comma serial](https://comma.ai/shop/comma-serial) for steady 12V power.
Connect the comma 3X to your computer with a normal USB C cable, or use a [comma serial](https://comma.ai/shop/comma-serial) for steady 12V power.
On the comma three, the serial console is exposed through a UART-to-USB chip, and `tools/scripts/serial.sh` can be used to connect.
@@ -45,7 +45,7 @@ In order to use ADB on your device, you'll need to perform the following steps u
* Here's an example command for connecting to your device using its tethered connection: `adb connect 192.168.43.1:5555`
> [!NOTE]
> The default port for ADB is 5555 on the comma 3/3X.
> The default port for ADB is 5555 on the comma 3X.
For more info on ADB, see the [Android Debug Bridge (ADB) documentation](https://developer.android.com/tools/adb).
docs/how-to/replay-a-drive.md
+1 -1
View File
@@ -8,7 +8,7 @@ Replaying is a critical tool for openpilot development and debugging.
Just run `tools/replay/replay --demo`.
## Replaying CAN data
*Hardware required: jungle and comma 3/3X*
*Hardware required: jungle and comma 3X*
1. Connect your PC to a jungle.
2.
docs/how-to/turn-the-speed-blue.md
+1 -1
View File
@@ -3,7 +3,7 @@
In 30 minutes, we'll get an openpilot development environment set up on your computer and make some changes to openpilot's UI.
And if you have a comma 3/3X, we'll deploy the change to your device for testing.
And if you have a comma 3X, we'll deploy the change to your device for testing.
## 1. Set up your development environment
launch_env.sh
+1 -1
View File
@@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1
export VECLIB_MAXIMUM_THREADS=1
if [ -z "$AGNOS_VERSION" ]; then
export AGNOS_VERSION="12.8"
export AGNOS_VERSION="13.1"
fi
export STAGING_ROOT="/data/safe_staging"
mkdocs.yml
+1 -1
View File
@@ -21,7 +21,7 @@ nav:
- What is openpilot?: getting-started/what-is-openpilot.md
- How-to:
- Turn the speed blue: how-to/turn-the-speed-blue.md
- Connect to a comma 3/3X: how-to/connect-to-comma.md
- Connect to a comma 3X: how-to/connect-to-comma.md
# - Make your first pull request: how-to/make-first-pr.md
#- Replay a drive: how-to/replay-a-drive.md
- Concepts:
opendbc_repo
+1 -1
Submodule opendbc_repo updated: 42bbd450b9...c705ebf987
panda
+1 -1
Submodule panda updated: 7eab6fd61b...7c393d1cd5
pyproject.toml
+8 -3
View File
@@ -36,6 +36,9 @@ dependencies = [
"pyopenssl < 24.3.0",
"pyaudio",
# ubloxd (TODO: just use struct)
"kaitaistruct",
# panda
"libusb1",
"spidev; platform_system == 'Linux'",
@@ -101,8 +104,9 @@ dev = [
"av",
"azure-identity",
"azure-storage-blob",
"dbus-next",
"dbus-next", # TODO: remove once we moved everything to jeepney
"dictdiffer",
"jeepney",
"matplotlib",
"opencv-python-headless",
"parameterized >=0.8, <0.9",
@@ -120,6 +124,7 @@ dev = [
tools = [
"metadrive-simulator @ https://github.com/commaai/metadrive/releases/download/MetaDrive-minimal-0.4.2.4/metadrive_simulator-0.4.2.4-py3-none-any.whl ; (platform_machine != 'aarch64')",
"dearpygui>=2.1.0",
]
[project.urls]
@@ -157,7 +162,6 @@ testpaths = [
"system/camerad",
"system/hardware",
"system/loggerd",
"system/proclogd",
"system/tests",
"system/ubloxd",
"system/webrtc",
@@ -173,7 +177,7 @@ quiet-level = 3
# if you've got a short variable name that's getting flagged, add it here
ignore-words-list = "bu,ro,te,ue,alo,hda,ois,nam,nams,ned,som,parm,setts,inout,warmup,bumb,nd,sie,preints,whit,indexIn,ws,uint,grey,deque,stdio,amin,BA,LITE,atEnd,UIs,errorString,arange,FocusIn,od,tim,relA,hist,copyable,jupyter,thead,TGE,abl,lite"
builtin = "clear,rare,informal,code,names,en-GB_to_en-US"
skip = "./third_party/*, ./tinygrad/*, ./tinygrad_repo/*, ./msgq/*, ./panda/*, ./opendbc/*, ./opendbc_repo/*, ./rednose/*, ./rednose_repo/*, ./teleoprtc/*, ./teleoprtc_repo/*, *.ts, uv.lock, *.onnx, ./cereal/gen/*, */c_generated_code/*, docs/assets/*"
skip = "./third_party/*, ./tinygrad/*, ./tinygrad_repo/*, ./msgq/*, ./panda/*, ./opendbc/*, ./opendbc_repo/*, ./rednose/*, ./rednose_repo/*, ./teleoprtc/*, ./teleoprtc_repo/*, *.ts, uv.lock, *.onnx, ./cereal/gen/*, */c_generated_code/*, docs/assets/*, tools/plotjuggler/layouts/*"
[tool.mypy]
python_version = "3.11"
@@ -247,6 +251,7 @@ exclude = [
"teleoprtc_repo",
"third_party",
"*.ipynb",
"generated",
]
lint.flake8-implicit-str-concat.allow-multiline = false
selfdrive/car/tests/test_car_interfaces.py
+4 -45
View File
@@ -1,15 +1,12 @@
import os
import math
import hypothesis.strategies as st
from hypothesis import Phase, given, settings
from parameterized import parameterized
from cereal import car, custom
from opendbc.car import DT_CTRL
from opendbc.car.car_helpers import interfaces
from opendbc.car.structs import CarParams
from opendbc.car.tests.test_car_interfaces import get_fuzzy_car_interface_args
from opendbc.car.fw_versions import FW_VERSIONS, FW_QUERY_CONFIGS
from opendbc.car.tests.test_car_interfaces import get_fuzzy_car_interface
from opendbc.car.mock.values import CAR as MOCK
from opendbc.car.values import PLATFORMS
from openpilot.selfdrive.car.helpers import convert_carControlSP
@@ -21,11 +18,6 @@ from openpilot.selfdrive.test.fuzzy_generation import FuzzyGenerator
from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
ALL_ECUS = {ecu for ecus in FW_VERSIONS.values() for ecu in ecus.keys()}
ALL_ECUS |= {ecu for config in FW_QUERY_CONFIGS.values() for ecu in config.extra_ecus}
ALL_REQUESTS = {tuple(r.request) for config in FW_QUERY_CONFIGS.values() for r in config.requests}
MAX_EXAMPLES = int(os.environ.get('MAX_EXAMPLES', '60'))
@@ -37,43 +29,10 @@ class TestCarInterfaces:
phases=(Phase.reuse, Phase.generate, Phase.shrink))
@given(data=st.data())
def test_car_interfaces(self, car_name, data):
CarInterface = interfaces[car_name]
args = get_fuzzy_car_interface_args(data.draw)
car_params = CarInterface.get_params(car_name, args['fingerprints'], args['car_fw'],
alpha_long=args['alpha_long'], is_release=False, docs=False)
car_params_sp = CarInterface.get_params_sp(car_params, car_name, args['fingerprints'], args['car_fw'],
alpha_long=args['alpha_long'], docs=False)
car_params = car_params.as_reader()
car_interface = CarInterface(car_params, car_params_sp)
car_interface = get_fuzzy_car_interface(car_name, data.draw)
car_params = car_interface.CP.as_reader()
car_params_sp = car_interface.CP_SP
sunnypilot_interfaces.setup_interfaces(car_interface)
assert car_params
assert car_params_sp
assert car_interface
assert car_params.mass > 1
assert car_params.wheelbase > 0
# centerToFront is center of gravity to front wheels, assert a reasonable range
assert car_params.wheelbase * 0.3 < car_params.centerToFront < car_params.wheelbase * 0.7
assert car_params.maxLateralAccel > 0
# Longitudinal sanity checks
assert len(car_params.longitudinalTuning.kpV) == len(car_params.longitudinalTuning.kpBP)
assert len(car_params.longitudinalTuning.kiV) == len(car_params.longitudinalTuning.kiBP)
# Lateral sanity checks
if car_params.steerControlType != CarParams.SteerControlType.angle:
tune = car_params.lateralTuning
if tune.which() == 'pid':
if car_name != MOCK.MOCK:
assert not math.isnan(tune.pid.kf) and tune.pid.kf > 0
assert len(tune.pid.kpV) > 0 and len(tune.pid.kpV) == len(tune.pid.kpBP)
assert len(tune.pid.kiV) > 0 and len(tune.pid.kiV) == len(tune.pid.kiBP)
elif tune.which() == 'torque':
assert not math.isnan(tune.torque.kf) and tune.torque.kf > 0
assert not math.isnan(tune.torque.friction) and tune.torque.friction > 0
cc_msg = FuzzyGenerator.get_random_msg(data.draw, car.CarControl, real_floats=True)
cc_sp_msg = FuzzyGenerator.get_random_msg(data.draw, custom.CarControlSP, real_floats=True)
selfdrive/controls/lib/desire_helper.py
+8 -3
View File
@@ -51,6 +51,10 @@ class DesireHelper:
self.lane_turn_controller = LaneTurnController(self)
self.lane_turn_direction = custom.TurnDirection.none
@staticmethod
def get_lane_change_direction(CS):
return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
def update(self, carstate, lateral_active, lane_change_prob):
self.alc.update_params()
self.lane_turn_controller.update_params()
@@ -71,12 +75,13 @@ class DesireHelper:
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
# Initialize lane change direction to prevent UI alert flicker
self.lane_change_direction = self.get_lane_change_direction(carstate)
# LaneChangeState.preLaneChange
elif self.lane_change_state == LaneChangeState.preLaneChange:
# Set lane change direction
self.lane_change_direction = LaneChangeDirection.left if \
carstate.leftBlinker else LaneChangeDirection.right
# Update lane change direction
self.lane_change_direction = self.get_lane_change_direction(carstate)
torque_applied = carstate.steeringPressed and \
((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
selfdrive/controls/lib/latcontrol_torque.py
+3 -3
View File
@@ -56,10 +56,8 @@ class LatControlTorque(LatControl):
actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
desired_lateral_accel = desired_curvature * CS.vEgo ** 2
# desired rate is the desired rate of change in the setpoint, not the absolute desired curvature
# desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2
desired_lateral_accel = desired_curvature * CS.vEgo ** 2
actual_lateral_accel = actual_curvature * CS.vEgo ** 2
lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
@@ -71,6 +69,8 @@ class LatControlTorque(LatControl):
# do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
pid_log.error = float(setpoint - measurement)
ff = gravity_adjusted_lateral_accel
# latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
ff -= self.torque_params.latAccelOffset
ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
selfdrive/controls/tests/test_torqued_lat_accel_offset.py
+70
View File
@@ -0,0 +1,70 @@
import numpy as np
from cereal import car, messaging
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY
from opendbc.car import structs
from opendbc.car.lateral import get_friction, FRICTION_THRESHOLD
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.locationd.torqued import TorqueEstimator, MIN_BUCKET_POINTS, POINTS_PER_BUCKET, STEER_BUCKET_BOUNDS
np.random.seed(0)
LA_ERR_STD = 1.0
INPUT_NOISE_STD = 0.08
V_EGO = 30.0
WARMUP_BUCKET_POINTS = (1.5*MIN_BUCKET_POINTS).astype(int)
STRAIGHT_ROAD_LA_BOUNDS = (0.02, 0.03)
ROLL_BIAS_DEG = 2.0
ROLL_COMPENSATION_BIAS = ACCELERATION_DUE_TO_GRAVITY*float(np.sin(np.deg2rad(ROLL_BIAS_DEG)))
TORQUE_TUNE = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=0.0, friction=0.2)
TORQUE_TUNE_BIASED = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=-ROLL_COMPENSATION_BIAS, friction=0.2)
def generate_inputs(torque_tune, la_err_std, input_noise_std=None):
rng = np.random.default_rng(0)
steer_torques = np.concat([rng.uniform(bnd[0], bnd[1], pts) for bnd, pts in zip(STEER_BUCKET_BOUNDS, WARMUP_BUCKET_POINTS, strict=True)])
la_errs = rng.normal(scale=la_err_std, size=steer_torques.size)
frictions = np.array([get_friction(la_err, 0.0, FRICTION_THRESHOLD, torque_tune) for la_err in la_errs])
lat_accels = torque_tune.latAccelFactor*steer_torques + torque_tune.latAccelOffset + frictions
if input_noise_std is not None:
steer_torques += rng.normal(scale=input_noise_std, size=steer_torques.size)
lat_accels += rng.normal(scale=input_noise_std, size=steer_torques.size)
return steer_torques, lat_accels
def get_warmed_up_estimator(steer_torques, lat_accels):
est = TorqueEstimator(car.CarParams())
for steer_torque, lat_accel in zip(steer_torques, lat_accels, strict=True):
est.filtered_points.add_point(steer_torque, lat_accel)
return est
def simulate_straight_road_msgs(est):
carControl = messaging.new_message('carControl').carControl
carOutput = messaging.new_message('carOutput').carOutput
carState = messaging.new_message('carState').carState
livePose = messaging.new_message('livePose').livePose
carControl.latActive = True
carState.vEgo = V_EGO
carState.steeringPressed = False
ts = DT_MDL*np.arange(2*POINTS_PER_BUCKET)
steer_torques = np.concat((np.linspace(-0.03, -0.02, POINTS_PER_BUCKET), np.linspace(0.02, 0.03, POINTS_PER_BUCKET)))
lat_accels = TORQUE_TUNE.latAccelFactor * steer_torques
for t, steer_torque, lat_accel in zip(ts, steer_torques, lat_accels, strict=True):
carOutput.actuatorsOutput.torque = float(-steer_torque)
livePose.orientationNED.x = float(np.deg2rad(ROLL_BIAS_DEG))
livePose.angularVelocityDevice.z = float(lat_accel / V_EGO)
for which, msg in (('carControl', carControl), ('carOutput', carOutput), ('carState', carState), ('livePose', livePose)):
est.handle_log(t, which, msg)
def test_estimated_offset():
steer_torques, lat_accels = generate_inputs(TORQUE_TUNE_BIASED, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD)
est = get_warmed_up_estimator(steer_torques, lat_accels)
msg = est.get_msg()
# TODO add lataccelfactor and friction check when we have more accurate estimates
assert abs(msg.liveTorqueParameters.latAccelOffsetRaw - TORQUE_TUNE_BIASED.latAccelOffset) < 0.1
def test_straight_road_roll_bias():
steer_torques, lat_accels = generate_inputs(TORQUE_TUNE, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD)
est = get_warmed_up_estimator(steer_torques, lat_accels)
simulate_straight_road_msgs(est)
msg = est.get_msg()
assert (msg.liveTorqueParameters.latAccelOffsetRaw < -0.05) and np.isfinite(msg.liveTorqueParameters.latAccelOffsetRaw)
selfdrive/modeld/constants.py
+3 -6
View File
@@ -13,12 +13,9 @@ class ModelConstants:
META_T_IDXS = [2., 4., 6., 8., 10.]
# model inputs constants
MODEL_FREQ = 20
HISTORY_FREQ = 5
HISTORY_LEN_SECONDS = 5
TEMPORAL_SKIP = MODEL_FREQ // HISTORY_FREQ
FULL_HISTORY_BUFFER_LEN = MODEL_FREQ * HISTORY_LEN_SECONDS
INPUT_HISTORY_BUFFER_LEN = HISTORY_FREQ * HISTORY_LEN_SECONDS
N_FRAMES = 2
MODEL_RUN_FREQ = 20
MODEL_CONTEXT_FREQ = 5 # "model_trained_fps"
FEATURE_LEN = 512
selfdrive/modeld/fill_model_msg.py
+1 -1
View File
@@ -149,7 +149,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
meta.hardBrakePredicted = hard_brake_predicted.item()
# confidence
if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:
if vipc_frame_id % (2*ModelConstants.MODEL_RUN_FREQ) == 0:
# any disengage prob
brake_disengage_probs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE]
gas_disengage_probs = net_output_data['meta'][0,Meta.GAS_DISENGAGE]
selfdrive/modeld/modeld.py
+73 -23
View File
@@ -80,6 +80,64 @@ class FrameMeta:
if vipc is not None:
self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof
class InputQueues:
def __init__ (self, model_fps, env_fps, n_frames_input):
assert env_fps % model_fps == 0
assert env_fps >= model_fps
self.model_fps = model_fps
self.env_fps = env_fps
self.n_frames_input = n_frames_input
self.dtypes = {}
self.shapes = {}
self.q = {}
def update_dtypes_and_shapes(self, input_dtypes, input_shapes) -> None:
self.dtypes.update(input_dtypes)
if self.env_fps == self.model_fps:
self.shapes.update(input_shapes)
else:
for k in input_shapes:
shape = list(input_shapes[k])
if 'img' in k:
n_channels = shape[1] // self.n_frames_input
shape[1] = (self.env_fps // self.model_fps + (self.n_frames_input - 1)) * n_channels
else:
shape[1] = (self.env_fps // self.model_fps) * shape[1]
self.shapes[k] = tuple(shape)
def reset(self) -> None:
self.q = {k: np.zeros(self.shapes[k], dtype=self.dtypes[k]) for k in self.dtypes.keys()}
def enqueue(self, inputs:dict[str, np.ndarray]) -> None:
for k in inputs.keys():
if inputs[k].dtype != self.dtypes[k]:
raise ValueError(f'supplied input <{k}({inputs[k].dtype})> has wrong dtype, expected {self.dtypes[k]}')
input_shape = list(self.shapes[k])
input_shape[1] = -1
single_input = inputs[k].reshape(tuple(input_shape))
sz = single_input.shape[1]
self.q[k][:,:-sz] = self.q[k][:,sz:]
self.q[k][:,-sz:] = single_input
def get(self, *names) -> dict[str, np.ndarray]:
if self.env_fps == self.model_fps:
return {k: self.q[k] for k in names}
else:
out = {}
for k in names:
shape = self.shapes[k]
if 'img' in k:
n_channels = shape[1] // (self.env_fps // self.model_fps + (self.n_frames_input - 1))
out[k] = np.concatenate([self.q[k][:, s:s+n_channels] for s in np.linspace(0, shape[1] - n_channels, self.n_frames_input, dtype=int)], axis=1)
elif 'pulse' in k:
# any pulse within interval counts
out[k] = self.q[k].reshape((shape[0], shape[1] * self.model_fps // self.env_fps, self.env_fps // self.model_fps, -1)).max(axis=2)
else:
idxs = np.arange(-1, -shape[1], -self.env_fps // self.model_fps)[::-1]
out[k] = self.q[k][:, idxs]
return out
class ModelState(ModelStateBase):
frames: dict[str, DrivingModelFrame]
inputs: dict[str, np.ndarray]
@@ -102,19 +160,15 @@ class ModelState(ModelStateBase):
self.policy_output_slices = policy_metadata['output_slices']
policy_output_size = policy_metadata['output_shapes']['outputs'][1]
self.frames = {name: DrivingModelFrame(context, ModelConstants.TEMPORAL_SKIP) for name in self.vision_input_names}
self.frames = {name: DrivingModelFrame(context, ModelConstants.MODEL_RUN_FREQ//ModelConstants.MODEL_CONTEXT_FREQ) for name in self.vision_input_names}
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
self.full_features_buffer = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32)
self.full_desire = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32)
self.temporal_idxs = slice(-1-(ModelConstants.TEMPORAL_SKIP*(ModelConstants.INPUT_HISTORY_BUFFER_LEN-1)), None, ModelConstants.TEMPORAL_SKIP)
# policy inputs
self.numpy_inputs = {
'desire': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32),
'traffic_convention': np.zeros((1, ModelConstants.TRAFFIC_CONVENTION_LEN), dtype=np.float32),
'features_buffer': np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32),
}
self.numpy_inputs = {k: np.zeros(self.policy_input_shapes[k], dtype=np.float32) for k in self.policy_input_shapes}
self.full_input_queues = InputQueues(ModelConstants.MODEL_CONTEXT_FREQ, ModelConstants.MODEL_RUN_FREQ, ModelConstants.N_FRAMES)
for k in ['desire_pulse', 'features_buffer']:
self.full_input_queues.update_dtypes_and_shapes({k: self.numpy_inputs[k].dtype}, {k: self.numpy_inputs[k].shape})
self.full_input_queues.reset()
# img buffers are managed in openCL transform code
self.vision_inputs: dict[str, Tensor] = {}
@@ -136,15 +190,10 @@ class ModelState(ModelStateBase):
def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
# Model decides when action is completed, so desire input is just a pulse triggered on rising edge
inputs['desire'][0] = 0
new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
self.prev_desire[:] = inputs['desire']
inputs['desire_pulse'][0] = 0
new_desire = np.where(inputs['desire_pulse'] - self.prev_desire > .99, inputs['desire_pulse'], 0)
self.prev_desire[:] = inputs['desire_pulse']
self.full_desire[0,:-1] = self.full_desire[0,1:]
self.full_desire[0,-1] = new_desire
self.numpy_inputs['desire'][:] = self.full_desire.reshape((1,ModelConstants.INPUT_HISTORY_BUFFER_LEN,ModelConstants.TEMPORAL_SKIP,-1)).max(axis=2)
self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
imgs_cl = {name: self.frames[name].prepare(bufs[name], transforms[name].flatten()) for name in self.vision_input_names}
if TICI and not USBGPU:
@@ -163,9 +212,10 @@ class ModelState(ModelStateBase):
self.vision_output = self.vision_run(**self.vision_inputs).contiguous().realize().uop.base.buffer.numpy()
vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(self.vision_output, self.vision_output_slices))
self.full_features_buffer[0,:-1] = self.full_features_buffer[0,1:]
self.full_features_buffer[0,-1] = vision_outputs_dict['hidden_state'][0, :]
self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs]
self.full_input_queues.enqueue({'features_buffer': vision_outputs_dict['hidden_state'], 'desire_pulse': new_desire})
for k in ['desire_pulse', 'features_buffer']:
self.numpy_inputs[k][:] = self.full_input_queues.get(k)[k]
self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
self.policy_output = self.policy_run(**self.policy_inputs).contiguous().realize().uop.base.buffer.numpy()
policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(self.policy_output, self.policy_output_slices))
@@ -223,7 +273,7 @@ def main(demo=False):
params = Params()
# setup filter to track dropped frames
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ)
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_RUN_FREQ)
frame_id = 0
last_vipc_frame_id = 0
run_count = 0
@@ -320,7 +370,7 @@ def main(demo=False):
bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names}
transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names}
inputs:dict[str, np.ndarray] = {
'desire': vec_desire,
'desire_pulse': vec_desire,
'traffic_convention': traffic_convention,
}
selfdrive/modeld/models/driving_policy.onnx
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:04b763fb71efe57a8a4c4168a8043ecd58939015026ded0dc755ded6905ac251
size 12343523
oid sha256:ebb38a934d6472c061cc6010f46d9720ca132d631a47e585a893bdd41ade2419
size 12343535
selfdrive/modeld/models/driving_vision.onnx
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:e66bb8d53eced3786ed71a59b55ffc6810944cb217f0518621cc76303260a1ef
oid sha256:befac016a247b7ad5dc5b55d339d127774ed7bd2b848f1583f72aa4caee37781
size 46271991
selfdrive/pandad/pandad.cc
+3 -6
View File
@@ -84,12 +84,10 @@ Panda *connect(std::string serial="", uint32_t index=0) {
panda->set_can_fd_auto(i, true);
}
bool is_deprecated_panda = std::find(DEPRECATED_PANDA_TYPES.begin(),
DEPRECATED_PANDA_TYPES.end(),
panda->hw_type) != DEPRECATED_PANDA_TYPES.end();
bool is_supported_panda = std::find(SUPPORTED_PANDA_TYPES.begin(), SUPPORTED_PANDA_TYPES.end(), panda->hw_type) != SUPPORTED_PANDA_TYPES.end();
if (is_deprecated_panda) {
LOGW("panda %s is deprecated (hw_type: %i), skipping firmware check...", panda->hw_serial().c_str(), static_cast<uint16_t>(panda->hw_type));
if (!is_supported_panda) {
LOGW("panda %s is not supported (hw_type: %i), skipping firmware check...", panda->hw_serial().c_str(), static_cast<uint16_t>(panda->hw_type));
return panda.release();
}
@@ -175,7 +173,6 @@ void fill_panda_state(cereal::PandaState::Builder &ps, cereal::PandaState::Panda
ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt));
ps.setInterruptLoad(health.interrupt_load_pkt);
ps.setFanPower(health.fan_power);
ps.setFanStallCount(health.fan_stall_count);
ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid_pkt));
ps.setSpiErrorCount(health.spi_error_count_pkt);
ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f);
selfdrive/pandad/pandad.h
+4 -7
View File
@@ -9,13 +9,10 @@
void pandad_main_thread(std::vector<std::string> serials);
// deprecated devices
static const std::vector<cereal::PandaState::PandaType> DEPRECATED_PANDA_TYPES = {
cereal::PandaState::PandaType::WHITE_PANDA,
cereal::PandaState::PandaType::GREY_PANDA,
cereal::PandaState::PandaType::BLACK_PANDA,
cereal::PandaState::PandaType::PEDAL,
cereal::PandaState::PandaType::UNO,
cereal::PandaState::PandaType::RED_PANDA_V2
static const std::vector<cereal::PandaState::PandaType> SUPPORTED_PANDA_TYPES = {
cereal::PandaState::PandaType::RED_PANDA,
cereal::PandaState::PandaType::TRES,
cereal::PandaState::PandaType::CUATRO,
};
selfdrive/pandad/pandad.py
+3 -8
View File
@@ -36,10 +36,10 @@ def flash_panda(panda_serial: str) -> Panda:
panda_signature = b"" if panda.bootstub else panda.get_signature()
cloudlog.warning(f"Panda {panda_serial} connected, version: {panda_version}, signature {panda_signature.hex()[:16]}, expected {fw_signature.hex()[:16]}")
# skip flashing if the detected device is deprecated from upstream
# skip flashing if the detected device is not supported from upstream
hw_type = panda.get_type()
if hw_type in Panda.DEPRECATED_DEVICES:
cloudlog.warning(f"Panda {panda_serial} is deprecated (hw_type: {hw_type}), skipping flash...")
if hw_type not in Panda.SUPPORTED_DEVICES:
cloudlog.warning(f"Panda {panda_serial} is not supported (hw_type: {hw_type}), skipping flash...")
return panda
if panda.bootstub or panda_signature != fw_signature:
@@ -91,11 +91,6 @@ def main() -> None:
cloudlog.event("pandad.flash_and_connect", count=count)
params.remove("PandaSignatures")
# TODO: remove this in the next AGNOS
# wait until USB is up before counting
if time.monotonic() < 60.:
no_internal_panda_count = 0
# Handle missing internal panda
if no_internal_panda_count > 0:
if no_internal_panda_count == 3:
selfdrive/pandad/tests/bootstub.panda.bin
BIN
View File
Binary file not shown.
selfdrive/pandad/tests/test_pandad.py
+1 -5
View File
@@ -22,8 +22,6 @@ class TestPandad:
if len(Panda.list()) == 0:
self._run_test(60)
self.spi = HARDWARE.get_device_type() != 'tici'
def teardown_method(self):
managed_processes['pandad'].stop()
@@ -106,11 +104,9 @@ class TestPandad:
# - 0.2s pandad -> pandad
# - plus some buffer
print("startup times", ts, sum(ts) / len(ts))
assert 0.1 < (sum(ts)/len(ts)) < (0.7 if self.spi else 5.0)
assert 0.1 < (sum(ts)/len(ts)) < 0.7
def test_protocol_version_check(self):
if not self.spi:
pytest.skip("SPI test")
# flash old fw
fn = os.path.join(HERE, "bootstub.panda_h7_spiv0.bin")
self._flash_bootstub_and_test(fn, expect_mismatch=True)
selfdrive/pandad/tests/test_pandad_spi.py
-3
View File
@@ -6,7 +6,6 @@ import random
import cereal.messaging as messaging
from cereal.services import SERVICE_LIST
from openpilot.system.hardware import HARDWARE
from openpilot.selfdrive.test.helpers import with_processes
from openpilot.selfdrive.pandad.tests.test_pandad_loopback import setup_pandad, send_random_can_messages
@@ -16,8 +15,6 @@ JUNGLE_SPAM = "JUNGLE_SPAM" in os.environ
class TestBoarddSpi:
@classmethod
def setup_class(cls):
if HARDWARE.get_device_type() == 'tici':
pytest.skip("only for spi pandas")
os.environ['STARTED'] = '1'
os.environ['SPI_ERR_PROB'] = '0.001'
if not JUNGLE_SPAM:
selfdrive/selfdrived/alerts_offroad.json
-4
View File
@@ -29,10 +29,6 @@
"text": "Device failed to register with the comma.ai backend. It will not connect or upload to comma.ai servers, and receives no support from comma.ai. If this is a device purchased at comma.ai/shop, open a ticket at https://comma.ai/support.",
"severity": 1
},
"Offroad_StorageMissing": {
"text": "NVMe drive not mounted.",
"severity": 1
},
"Offroad_CarUnrecognized": {
"text": "sunnypilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai.",
"severity": 0
selfdrive/selfdrived/events.py
+1 -1
View File
@@ -83,7 +83,7 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S
f"Steer Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}",
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4)
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.4)
def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
selfdrive/selfdrived/selfdrived.py
+1 -8
View File
@@ -21,7 +21,6 @@ from openpilot.selfdrive.selfdrived.helpers import ExcessiveActuationCheck
from openpilot.selfdrive.selfdrived.state import StateMachine
from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert
from openpilot.system.hardware import HARDWARE
from openpilot.system.version import get_build_metadata
from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem
@@ -135,13 +134,7 @@ class SelfdriveD(CruiseHelper):
self.state_machine = StateMachine()
self.rk = Ratekeeper(100, print_delay_threshold=None)
# some comma three with NVMe experience NVMe dropouts mid-drive that
# cause loggerd to crash on write, so ignore it only on that platform
self.ignored_processes = set()
nvme_expected = os.path.exists('/dev/nvme0n1') or (not os.path.isfile("/persist/comma/living-in-the-moment"))
if HARDWARE.get_device_type() == 'tici' and nvme_expected:
self.ignored_processes = {'loggerd', }
self.ignored_processes.update({'mapd'})
self.ignored_processes = {'mapd', }
# Determine startup event
is_remote = build_metadata.openpilot.comma_remote or build_metadata.openpilot.sunnypilot_remote
selfdrive/test/process_replay/process_replay.py
+1 -1
View File
@@ -595,7 +595,7 @@ def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") ->
if len(live_calibration) > 0:
custom_params["CalibrationParams"] = live_calibration[msg_index].as_builder().to_bytes()
if len(live_parameters) > 0:
custom_params["LiveParameters"] = live_parameters[msg_index].as_builder().to_bytes()
custom_params["LiveParametersV2"] = live_parameters[msg_index].as_builder().to_bytes()
if len(live_torque_parameters) > 0:
custom_params["LiveTorqueParameters"] = live_torque_parameters[msg_index].as_builder().to_bytes()
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
6d3219bca9f66a229b38a5382d301a92b0147edb
afcab1abb62b9d5678342956cced4712f44e909e
selfdrive/test/test_onroad.py
+5 -16
View File
@@ -18,7 +18,6 @@ from openpilot.common.timeout import Timeout
from openpilot.common.params import Params
from openpilot.selfdrive.selfdrived.events import EVENTS, ET
from openpilot.selfdrive.test.helpers import set_params_enabled, release_only
from openpilot.system.hardware import HARDWARE
from openpilot.system.hardware.hw import Paths
from openpilot.tools.lib.logreader import LogReader
from openpilot.tools.lib.log_time_series import msgs_to_time_series
@@ -33,7 +32,7 @@ CPU usage budget
TEST_DURATION = 25
LOG_OFFSET = 8
MAX_TOTAL_CPU = 280. # total for all 8 cores
MAX_TOTAL_CPU = 300. # total for all 8 cores
PROCS = {
# Baseline CPU usage by process
"selfdrive.controls.controlsd": 16.0,
@@ -57,30 +56,20 @@ PROCS = {
"selfdrive.ui.soundd": 3.0,
"selfdrive.ui.feedback.feedbackd": 1.0,
"selfdrive.monitoring.dmonitoringd": 4.0,
"./proclogd": 2.0,
"system.proclogd": 3.0,
"system.logmessaged": 1.0,
"system.tombstoned": 0,
"./logcatd": 1.0,
"system.journald": 1.0,
"system.micd": 5.0,
"system.timed": 0,
"selfdrive.pandad.pandad": 0,
"system.statsd": 1.0,
"system.loggerd.uploader": 15.0,
"system.loggerd.deleter": 1.0,
"./pandad": 19.0,
"system.qcomgpsd.qcomgpsd": 1.0,
}
PROCS.update({
"tici": {
"./pandad": 5.0,
"./ubloxd": 1.0,
"system.ubloxd.pigeond": 6.0,
},
"tizi": {
"./pandad": 19.0,
"system.qcomgpsd.qcomgpsd": 1.0,
}
}.get(HARDWARE.get_device_type(), {}))
TIMINGS = {
# rtols: max/min, rsd
"can": [2.5, 0.35],
selfdrive/ui/layouts/main.py
+9 -3
View File
@@ -63,14 +63,20 @@ class MainLayout(Widget):
# Don't hide sidebar from interactive timeout
if self._current_mode != MainState.ONROAD:
self._sidebar.set_visible(False)
self._current_mode = MainState.ONROAD
self._set_current_layout(MainState.ONROAD)
else:
self._current_mode = MainState.HOME
self._set_current_layout(MainState.HOME)
self._sidebar.set_visible(True)
def _set_current_layout(self, layout: MainState):
if layout != self._current_mode:
self._layouts[self._current_mode].hide_event()
self._current_mode = layout
self._layouts[self._current_mode].show_event()
def open_settings(self, panel_type: PanelType):
self._layouts[MainState.SETTINGS].set_current_panel(panel_type)
self._current_mode = MainState.SETTINGS
self._set_current_layout(MainState.SETTINGS)
self._sidebar.set_visible(False)
def _on_settings_clicked(self):
selfdrive/ui/layouts/network.py
-17
View File
@@ -1,17 +0,0 @@
import pyray as rl
from openpilot.system.ui.lib.wifi_manager import WifiManagerWrapper
from openpilot.system.ui.widgets import Widget
from openpilot.system.ui.widgets.network import WifiManagerUI
class NetworkLayout(Widget):
def __init__(self):
super().__init__()
self.wifi_manager = WifiManagerWrapper()
self.wifi_ui = WifiManagerUI(self.wifi_manager)
def _render(self, rect: rl.Rectangle):
self.wifi_ui.render(rect)
def shutdown(self):
self.wifi_manager.shutdown()
selfdrive/ui/layouts/settings/firehose.py
+3 -3
View File
@@ -2,7 +2,7 @@ import pyray as rl
import time
import threading
from openpilot.common.api import Api, api_get
from openpilot.common.api import api_get
from openpilot.common.params import Params
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.ui.ui_state import ui_state
@@ -11,7 +11,7 @@ from openpilot.system.ui.lib.application import gui_app, FontWeight
from openpilot.system.ui.lib.scroll_panel import GuiScrollPanel
from openpilot.system.ui.lib.wrap_text import wrap_text
from openpilot.system.ui.widgets import Widget
from openpilot.selfdrive.ui.lib.api_helpers import get_token
TITLE = "Firehose Mode"
DESCRIPTION = (
@@ -163,7 +163,7 @@ class FirehoseLayout(Widget):
dongle_id = self.params.get("DongleId")
if not dongle_id or dongle_id == UNREGISTERED_DONGLE_ID:
return
identity_token = Api(dongle_id).get_token()
identity_token = get_token(dongle_id)
response = api_get(f"v1/devices/{dongle_id}/firehose_stats", access_token=identity_token)
if response.status_code == 200:
data = response.json()
selfdrive/ui/layouts/settings/settings.py
+16 -6
View File
@@ -2,7 +2,6 @@ import pyray as rl
from dataclasses import dataclass
from enum import IntEnum
from collections.abc import Callable
from openpilot.selfdrive.ui.layouts.network import NetworkLayout
from openpilot.selfdrive.ui.layouts.settings.developer import DeveloperLayout
from openpilot.selfdrive.ui.layouts.settings.device import DeviceLayout
from openpilot.selfdrive.ui.layouts.settings.firehose import FirehoseLayout
@@ -10,7 +9,9 @@ from openpilot.selfdrive.ui.layouts.settings.software import SoftwareLayout
from openpilot.selfdrive.ui.layouts.settings.toggles import TogglesLayout
from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
from openpilot.system.ui.lib.text_measure import measure_text_cached
from openpilot.system.ui.lib.wifi_manager import WifiManager
from openpilot.system.ui.widgets import Widget
from openpilot.system.ui.widgets.network import WifiManagerUI
# Settings close button
SETTINGS_CLOSE_TEXT = "×"
@@ -43,7 +44,7 @@ class PanelType(IntEnum):
@dataclass
class PanelInfo:
name: str
instance: object
instance: Widget
button_rect: rl.Rectangle = rl.Rectangle(0, 0, 0, 0)
@@ -53,9 +54,12 @@ class SettingsLayout(Widget):
self._current_panel = PanelType.DEVICE
# Panel configuration
wifi_manager = WifiManager()
wifi_manager.set_active(False)
self._panels = {
PanelType.DEVICE: PanelInfo("Device", DeviceLayout()),
PanelType.NETWORK: PanelInfo("Network", NetworkLayout()),
PanelType.NETWORK: PanelInfo("Network", WifiManagerUI(wifi_manager)),
PanelType.TOGGLES: PanelInfo("Toggles", TogglesLayout()),
PanelType.SOFTWARE: PanelInfo("Software", SoftwareLayout()),
PanelType.FIREHOSE: PanelInfo("Firehose", FirehoseLayout()),
@@ -149,8 +153,14 @@ class SettingsLayout(Widget):
def set_current_panel(self, panel_type: PanelType):
if panel_type != self._current_panel:
self._panels[self._current_panel].instance.hide_event()
self._current_panel = panel_type
self._panels[self._current_panel].instance.show_event()
def close_settings(self):
if self._close_callback:
self._close_callback()
def show_event(self):
super().show_event()
self._panels[self._current_panel].instance.show_event()
def hide_event(self):
super().hide_event()
self._panels[self._current_panel].instance.hide_event()
selfdrive/ui/lib/api_helpers.py
+14
View File
@@ -0,0 +1,14 @@
import time
from functools import lru_cache
from openpilot.common.api import Api
TOKEN_EXPIRY_HOURS = 2
@lru_cache(maxsize=1)
def _get_token(dongle_id: str, t: int):
return Api(dongle_id).get_token(expiry_hours=TOKEN_EXPIRY_HOURS)
def get_token(dongle_id: str):
return _get_token(dongle_id, int(time.monotonic() / (TOKEN_EXPIRY_HOURS / 2 * 60 * 60)))
selfdrive/ui/lib/prime_state.py
+3 -13
View File
@@ -2,14 +2,12 @@ from enum import IntEnum
import os
import threading
import time
from functools import lru_cache
from openpilot.common.api import Api, api_get
from openpilot.common.api import api_get
from openpilot.common.params import Params
from openpilot.common.swaglog import cloudlog
from openpilot.system.athena.registration import UNREGISTERED_DONGLE_ID
TOKEN_EXPIRY_HOURS = 2
from openpilot.selfdrive.ui.lib.api_helpers import get_token
class PrimeType(IntEnum):
@@ -23,12 +21,6 @@ class PrimeType(IntEnum):
PURPLE = 5,
@lru_cache(maxsize=1)
def get_token(dongle_id: str, t: int):
print('getting token')
return Api(dongle_id).get_token(expiry_hours=TOKEN_EXPIRY_HOURS)
class PrimeState:
FETCH_INTERVAL = 5.0 # seconds between API calls
API_TIMEOUT = 10.0 # seconds for API requests
@@ -58,15 +50,13 @@ class PrimeState:
return
try:
identity_token = get_token(dongle_id, int(time.monotonic() / (TOKEN_EXPIRY_HOURS / 2 * 60 * 60)))
identity_token = get_token(dongle_id)
response = api_get(f"v1.1/devices/{dongle_id}", timeout=self.API_TIMEOUT, access_token=identity_token)
if response.status_code == 200:
data = response.json()
is_paired = data.get("is_paired", False)
prime_type = data.get("prime_type", 0)
self.set_type(PrimeType(prime_type) if is_paired else PrimeType.UNPAIRED)
elif response.status_code == 401:
get_token.cache_clear()
except Exception as e:
cloudlog.error(f"Failed to fetch prime status: {e}")
selfdrive/ui/tests/test_ui/run.py
+1 -1
View File
@@ -29,7 +29,7 @@ UI_DELAY = 0.5 # may be slower on CI?
TEST_ROUTE = "a2a0ccea32023010|2023-07-27--13-01-19"
STREAMS: list[tuple[VisionStreamType, CameraConfig, bytes]] = []
OFFROAD_ALERTS = ['Offroad_StorageMissing', 'Offroad_IsTakingSnapshot']
OFFROAD_ALERTS = ['Offroad_IsTakingSnapshot', ]
DATA: dict[str, capnp.lib.capnp._DynamicStructBuilder] = dict.fromkeys(
["carParams", "deviceState", "pandaStates", "controlsState", "selfdriveState",
"liveCalibration", "modelV2", "radarState", "driverMonitoringState", "carState",
selfdrive/ui/ui.cc
+1 -2
View File
@@ -55,8 +55,7 @@ void update_state(UIState *s) {
}
if (sm.updated("wideRoadCameraState")) {
auto cam_state = sm["wideRoadCameraState"].getWideRoadCameraState();
float scale = (cam_state.getSensor() == cereal::FrameData::ImageSensor::AR0231) ? 6.0f : 1.0f;
scene.light_sensor = std::max(100.0f - scale * cam_state.getExposureValPercent(), 0.0f);
scene.light_sensor = std::max(100.0f - cam_state.getExposureValPercent(), 0.0f);
} else if (!sm.allAliveAndValid({"wideRoadCameraState"})) {
scene.light_sensor = -1;
}
selfdrive/ui/ui_state.py
+1 -4
View File
@@ -105,10 +105,7 @@ class UIState:
# Handle wide road camera state updates
if self.sm.updated["wideRoadCameraState"]:
cam_state = self.sm["wideRoadCameraState"]
# Scale factor based on sensor type
scale = 6.0 if cam_state.sensor == 'ar0231' else 1.0
self.light_sensor = max(100.0 - scale * cam_state.exposureValPercent, 0.0)
self.light_sensor = max(100.0 - cam_state.exposureValPercent, 0.0)
elif not self.sm.alive["wideRoadCameraState"] or not self.sm.valid["wideRoadCameraState"]:
self.light_sensor = -1
sunnypilot/models/tests/model_hash
+1 -1
View File
@@ -1 +1 @@
2ff2f49176a13bc7f856645d785b3b838a5c7ecf7f6cb37699fa0459ebf12453
70406ab4dd66d0e384734a8a56632ae4a62bc9670c2e630a0f71588c4e212cd8
system/camerad/SConscript
+1 -1
View File
@@ -4,7 +4,7 @@ libs = [common, 'OpenCL', messaging, visionipc, gpucommon]
if arch != "Darwin":
camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/spectra.cc',
'cameras/cdm.cc', 'sensors/ar0231.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc'])
'cameras/cdm.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc'])
env.Program('camerad', ['main.cc', camera_obj], LIBS=libs)
if GetOption("extras") and arch == "x86_64":
system/camerad/cameras/hw.h
+1 -1
View File
@@ -13,7 +13,7 @@ typedef enum {
ISP_BPS_PROCESSED, // fully processed image through the BPS
} SpectraOutputType;
// For the comma 3/3X three camera platform
// For the comma 3X three camera platform
struct CameraConfig {
int camera_num;
system/camerad/cameras/spectra.cc
+1 -2
View File
@@ -1004,8 +1004,7 @@ bool SpectraCamera::openSensor() {
};
// Figure out which sensor we have
if (!init_sensor_lambda(new AR0231) &&
!init_sensor_lambda(new OX03C10) &&
if (!init_sensor_lambda(new OX03C10) &&
!init_sensor_lambda(new OS04C10)) {
LOGE("** sensor %d FAILED bringup, disabling", cc.camera_num);
enabled = false;
system/camerad/sensors/ar0231.cc
-136
View File
@@ -1,136 +0,0 @@
#include <cassert>
#include <cmath>
#include "system/camerad/sensors/sensor.h"
namespace {
const size_t AR0231_REGISTERS_HEIGHT = 2;
// TODO: this extra height is universal and doesn't apply per camera
const size_t AR0231_STATS_HEIGHT = 2 + 8;
const float sensor_analog_gains_AR0231[] = {
1.0 / 8.0, 2.0 / 8.0, 2.0 / 7.0, 3.0 / 7.0, // 0, 1, 2, 3
3.0 / 6.0, 4.0 / 6.0, 4.0 / 5.0, 5.0 / 5.0, // 4, 5, 6, 7
5.0 / 4.0, 6.0 / 4.0, 6.0 / 3.0, 7.0 / 3.0, // 8, 9, 10, 11
7.0 / 2.0, 8.0 / 2.0, 8.0 / 1.0}; // 12, 13, 14, 15 = bypass
} // namespace
AR0231::AR0231() {
image_sensor = cereal::FrameData::ImageSensor::AR0231;
bayer_pattern = CAM_ISP_PATTERN_BAYER_GRGRGR;
pixel_size_mm = 0.003;
data_word = true;
frame_width = 1928;
frame_height = 1208;
frame_stride = (frame_width * 12 / 8) + 4;
extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT;
registers_offset = 0;
frame_offset = AR0231_REGISTERS_HEIGHT;
stats_offset = AR0231_REGISTERS_HEIGHT + frame_height;
start_reg_array.assign(std::begin(start_reg_array_ar0231), std::end(start_reg_array_ar0231));
init_reg_array.assign(std::begin(init_array_ar0231), std::end(init_array_ar0231));
probe_reg_addr = 0x3000;
probe_expected_data = 0x354;
bits_per_pixel = 12;
mipi_format = CAM_FORMAT_MIPI_RAW_12;
frame_data_type = 0x12; // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead
mclk_frequency = 19200000; //Hz
readout_time_ns = 22850000;
dc_gain_factor = 2.5;
dc_gain_min_weight = 0;
dc_gain_max_weight = 1;
dc_gain_on_grey = 0.2;
dc_gain_off_grey = 0.3;
exposure_time_min = 2; // with HDR, fastest ss
exposure_time_max = 0x0855; // with HDR, slowest ss, 40ms
analog_gain_min_idx = 0x1; // 0.25x
analog_gain_rec_idx = 0x6; // 0.8x
analog_gain_max_idx = 0xD; // 4.0x
analog_gain_cost_delta = 0;
analog_gain_cost_low = 0.1;
analog_gain_cost_high = 5.0;
for (int i = 0; i <= analog_gain_max_idx; i++) {
sensor_analog_gains[i] = sensor_analog_gains_AR0231[i];
}
min_ev = exposure_time_min * sensor_analog_gains[analog_gain_min_idx];
max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx];
target_grey_factor = 1.0;
black_level = 168;
color_correct_matrix = {
0x000000af, 0x00000ff9, 0x00000fd8,
0x00000fbc, 0x000000bb, 0x00000009,
0x00000fb6, 0x00000fe0, 0x000000ea,
};
for (int i = 0; i < 65; i++) {
float fx = i / 64.0;
const float gamma_k = 0.75;
const float gamma_b = 0.125;
const float mp = 0.01; // ideally midpoint should be adaptive
const float rk = 9 - 100*mp;
// poly approximation for s curve
fx = (fx > mp) ?
((rk * (fx-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(fx-mp))) + gamma_k*mp + gamma_b) :
((rk * (fx-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(fx-mp))) + gamma_k*mp + gamma_b);
gamma_lut_rgb.push_back((uint32_t)(fx*1023.0 + 0.5));
}
prepare_gamma_lut();
linearization_lut = {
0x02000000, 0x02000000, 0x02000000, 0x02000000,
0x020007ff, 0x020007ff, 0x020007ff, 0x020007ff,
0x02000bff, 0x02000bff, 0x02000bff, 0x02000bff,
0x020017ff, 0x020017ff, 0x020017ff, 0x020017ff,
0x02001bff, 0x02001bff, 0x02001bff, 0x02001bff,
0x020023ff, 0x020023ff, 0x020023ff, 0x020023ff,
0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff,
0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff,
0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff,
};
linearization_pts = {0x07ff0bff, 0x17ff1bff, 0x23ff3fff, 0x3fff3fff};
vignetting_lut = {
0x00eaa755, 0x00cf2679, 0x00bc05e0, 0x00acc566, 0x00a1450a, 0x009984cc, 0x0095a4ad, 0x009584ac, 0x009944ca, 0x00a0c506, 0x00ac0560, 0x00bb25d9, 0x00ce2671, 0x00e90748, 0x01112889, 0x014a2a51, 0x01984cc2,
0x00db06d8, 0x00c30618, 0x00afe57f, 0x00a0a505, 0x009524a9, 0x008d646b, 0x0089844c, 0x0089644b, 0x008d2469, 0x0094a4a5, 0x009fe4ff, 0x00af0578, 0x00c20610, 0x00d986cc, 0x00fda7ed, 0x01320990, 0x017aebd7,
0x00d1868c, 0x00baa5d5, 0x00a7853c, 0x009844c2, 0x008cc466, 0x0085a42d, 0x0083641b, 0x0083641b, 0x0085842c, 0x008c4462, 0x0097a4bd, 0x00a6c536, 0x00b9a5cd, 0x00d06683, 0x00f1678b, 0x01226913, 0x0167ab3d,
0x00cd0668, 0x00b625b1, 0x00a30518, 0x0093c49e, 0x00884442, 0x00830418, 0x0080e407, 0x0080c406, 0x0082e417, 0x0087c43e, 0x00932499, 0x00a22511, 0x00b525a9, 0x00cbe65f, 0x00eb0758, 0x011a68d3, 0x015daaed,
0x00cc4662, 0x00b565ab, 0x00a24512, 0x00930498, 0x0087843c, 0x0082a415, 0x00806403, 0x00806403, 0x00828414, 0x00870438, 0x00926493, 0x00a1850c, 0x00b465a3, 0x00cb2659, 0x00ea2751, 0x011928c9, 0x015c2ae1,
0x00cf667b, 0x00b885c4, 0x00a5652b, 0x009624b1, 0x008aa455, 0x00846423, 0x00822411, 0x00822411, 0x00844422, 0x008a2451, 0x009564ab, 0x00a48524, 0x00b785bc, 0x00ce4672, 0x00ee6773, 0x011e88f4, 0x0162eb17,
0x00d6c6b6, 0x00bf65fb, 0x00ac4562, 0x009d04e8, 0x0091848c, 0x0089c44e, 0x00862431, 0x00860430, 0x0089844c, 0x00910488, 0x009c64e3, 0x00ab655b, 0x00be65f3, 0x00d566ab, 0x00f847c2, 0x012b2959, 0x01726b93,
0x00e3e71f, 0x00ca0650, 0x00b705b8, 0x00a7a53d, 0x009c24e1, 0x009484a4, 0x00908484, 0x00908484, 0x009424a1, 0x009bc4de, 0x00a70538, 0x00b625b1, 0x00c90648, 0x00e26713, 0x0108e847, 0x013fe9ff, 0x018bcc5e,
0x00f807c0, 0x00d966cb, 0x00c5862c, 0x00b625b1, 0x00aaa555, 0x00a30518, 0x009f04f8, 0x009f04f8, 0x00a2a515, 0x00aa2551, 0x00b585ac, 0x00c4a625, 0x00d846c2, 0x00f647b2, 0x0121a90d, 0x015e4af2, 0x01b8cdc6,
0x011548aa, 0x00f1678b, 0x00d886c4, 0x00c86643, 0x00bce5e7, 0x00b545aa, 0x00b1658b, 0x00b1458a, 0x00b505a8, 0x00bc85e4, 0x00c7c63e, 0x00d786bc, 0x00efe77f, 0x0113489a, 0x0144ea27, 0x01888c44, 0x01fdcfee,
0x013e49f2, 0x0113e89f, 0x00f5a7ad, 0x00e0c706, 0x00d30698, 0x00cb665b, 0x00c7663b, 0x00c7663b, 0x00cb0658, 0x00d2a695, 0x00dfe6ff, 0x00f467a3, 0x01122891, 0x013be9df, 0x01750ba8, 0x01cfae7d, 0x025912c8,
0x01766bb3, 0x01446a23, 0x011fc8fe, 0x0105e82f, 0x00f467a3, 0x00e9874c, 0x00e46723, 0x00e44722, 0x00e92749, 0x00f3a79d, 0x0104c826, 0x011e48f2, 0x01424a12, 0x01738b9c, 0x01bf6dfb, 0x023611b0, 0x02ced676,
0x01cf8e7c, 0x01866c33, 0x015aaad5, 0x013ae9d7, 0x01250928, 0x011768bb, 0x0110a885, 0x01108884, 0x0116e8b7, 0x01242921, 0x0139a9cd, 0x0158eac7, 0x01840c20, 0x01cb0e58, 0x0233719b, 0x02b9d5ce, 0x03645b22,
};
}
std::vector<i2c_random_wr_payload> AR0231::getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const {
uint16_t analog_gain_reg = 0xFF00 | (new_exp_g << 4) | new_exp_g;
return {
{0x3366, analog_gain_reg},
{0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)},
{0x3012, (uint16_t)exposure_time},
};
}
int AR0231::getSlaveAddress(int port) const {
assert(port >= 0 && port <= 2);
return (int[]){0x20, 0x30, 0x20}[port];
}
float AR0231::getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const {
// Cost of ev diff
float score = std::abs(desired_ev - (exp_t * exp_gain)) * 10;
// Cost of absolute gain
float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low;
score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m;
// Cost of changing gain
score += std::abs(exp_g_idx - gain_idx) * (score + 1.0) / 10.0;
return score;
}
system/camerad/sensors/ar0231_registers.h
-121
View File
@@ -1,121 +0,0 @@
#pragma once
const struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}};
const struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}};
const struct i2c_random_wr_payload init_array_ar0231[] = {
{0x301A, 0x0018}, // RESET_REGISTER
// **NOTE**: if this is changed, readout_time_ns must be updated in the Sensor config
// CLOCK Settings
// input clock is 19.2 / 2 * 0x37 = 528 MHz
// pixclk is 528 / 6 = 88 MHz
// full roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*FRAME_LENGTH_LINES)) = 39.99 ms
// img roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*Y_OUTPUT_CONTROL)) = 22.85 ms
{0x302A, 0x0006}, // VT_PIX_CLK_DIV
{0x302C, 0x0001}, // VT_SYS_CLK_DIV
{0x302E, 0x0002}, // PRE_PLL_CLK_DIV
{0x3030, 0x0037}, // PLL_MULTIPLIER
{0x3036, 0x000C}, // OP_PIX_CLK_DIV
{0x3038, 0x0001}, // OP_SYS_CLK_DIV
// FORMAT
{0x3040, 0xC000}, // READ_MODE
{0x3004, 0x0000}, // X_ADDR_START_
{0x3008, 0x0787}, // X_ADDR_END_
{0x3002, 0x0000}, // Y_ADDR_START_
{0x3006, 0x04B7}, // Y_ADDR_END_
{0x3032, 0x0000}, // SCALING_MODE
{0x30A2, 0x0001}, // X_ODD_INC_
{0x30A6, 0x0001}, // Y_ODD_INC_
{0x3402, 0x0788}, // X_OUTPUT_CONTROL
{0x3404, 0x04B8}, // Y_OUTPUT_CONTROL
{0x3064, 0x1982}, // SMIA_TEST
{0x30BA, 0x11F2}, // DIGITAL_CTRL
// Enable external trigger and disable GPIO outputs
{0x30CE, 0x0120}, // SLAVE_SH_SYNC_MODE | FRAME_START_MODE
{0x340A, 0xE0}, // GPIO3_INPUT_DISABLE | GPIO2_INPUT_DISABLE | GPIO1_INPUT_DISABLE
{0x340C, 0x802}, // GPIO_HIDRV_EN | GPIO0_ISEL=2
// Readout timing
{0x300C, 0x0672}, // LINE_LENGTH_PCK (valid for 3-exposure HDR)
{0x300A, 0x0855}, // FRAME_LENGTH_LINES
{0x3042, 0x0000}, // EXTRA_DELAY
// Readout Settings
{0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI
{0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12
{0x3342, 0x1212}, // MIPI_F1_PDT_EDT
{0x3346, 0x1212}, // MIPI_F2_PDT_EDT
{0x334A, 0x1212}, // MIPI_F3_PDT_EDT
{0x334E, 0x1212}, // MIPI_F4_PDT_EDT
{0x3344, 0x0011}, // MIPI_F1_VDT_VC
{0x3348, 0x0111}, // MIPI_F2_VDT_VC
{0x334C, 0x0211}, // MIPI_F3_VDT_VC
{0x3350, 0x0311}, // MIPI_F4_VDT_VC
{0x31B0, 0x0053}, // FRAME_PREAMBLE
{0x31B2, 0x003B}, // LINE_PREAMBLE
{0x301A, 0x001C}, // RESET_REGISTER
// Noise Corrections
{0x3092, 0x0C24}, // ROW_NOISE_CONTROL
{0x337A, 0x0C80}, // DBLC_SCALE0
{0x3370, 0x03B1}, // DBLC
{0x3044, 0x0400}, // DARK_CONTROL
// Enable temperature sensor
{0x30B4, 0x0007}, // TEMPSENS0_CTRL_REG
{0x30B8, 0x0007}, // TEMPSENS1_CTRL_REG
// Enable dead pixel correction using
// the 1D line correction scheme
{0x31E0, 0x0003},
// HDR Settings
{0x3082, 0x0004}, // OPERATION_MODE_CTRL
{0x3238, 0x0444}, // EXPOSURE_RATIO
{0x1008, 0x0361}, // FINE_INTEGRATION_TIME_MIN
{0x100C, 0x0589}, // FINE_INTEGRATION_TIME2_MIN
{0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN
{0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN
// TODO: do these have to be lower than LINE_LENGTH_PCK?
{0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_
{0x321E, 0x0894}, // FINE_INTEGRATION_TIME2
{0x31D0, 0x0000}, // COMPANDING, no good in 10 bit?
{0x33DA, 0x0000}, // COMPANDING
{0x318E, 0x0200}, // PRE_HDR_GAIN_EN
// DLO Settings
{0x3100, 0x4000}, // DLO_CONTROL0
{0x3280, 0x0CCC}, // T1 G1
{0x3282, 0x0CCC}, // T1 R
{0x3284, 0x0CCC}, // T1 B
{0x3286, 0x0CCC}, // T1 G2
{0x3288, 0x0FA0}, // T2 G1
{0x328A, 0x0FA0}, // T2 R
{0x328C, 0x0FA0}, // T2 B
{0x328E, 0x0FA0}, // T2 G2
// Initial Gains
{0x3022, 0x0001}, // GROUPED_PARAMETER_HOLD_
{0x3366, 0xFF77}, // ANALOG_GAIN (1x)
{0x3060, 0x3333}, // ANALOG_COLOR_GAIN
{0x3362, 0x0000}, // DC GAIN
{0x305A, 0x00F8}, // red gain
{0x3058, 0x0122}, // blue gain
{0x3056, 0x009A}, // g1 gain
{0x305C, 0x009A}, // g2 gain
{0x3022, 0x0000}, // GROUPED_PARAMETER_HOLD_
// Initial Integration Time
{0x3012, 0x0005},
};
system/camerad/sensors/sensor.h
-12
View File
@@ -10,7 +10,6 @@
#include "media/cam_sensor.h"
#include "cereal/gen/cpp/log.capnp.h"
#include "system/camerad/sensors/ar0231_registers.h"
#include "system/camerad/sensors/ox03c10_registers.h"
#include "system/camerad/sensors/os04c10_registers.h"
@@ -88,17 +87,6 @@ public:
};
};
class AR0231 : public SensorInfo {
public:
AR0231();
std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override;
float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override;
int getSlaveAddress(int port) const override;
private:
mutable std::map<uint16_t, std::pair<int, int>> ar0231_register_lut;
};
class OX03C10 : public SensorInfo {
public:
OX03C10();
system/hardware/base.py
+7
View File
@@ -65,6 +65,10 @@ class ThermalConfig:
return ret
class LPABase(ABC):
@abstractmethod
def bootstrap(self) -> None:
pass
@abstractmethod
def list_profiles(self) -> list[Profile]:
pass
@@ -89,6 +93,9 @@ class LPABase(ABC):
def switch_profile(self, iccid: str) -> None:
pass
def is_comma_profile(self, iccid: str) -> bool:
return any(iccid.startswith(prefix) for prefix in ('8985235',))
class HardwareBase(ABC):
@staticmethod
def get_cmdline() -> dict[str, str]:
system/hardware/esim.py
+26 -1
View File
@@ -3,10 +3,32 @@
import argparse
import time
from openpilot.system.hardware import HARDWARE
from openpilot.system.hardware.base import LPABase
def bootstrap(lpa: LPABase) -> None:
print('┌──────────────────────────────────────────────────────────────────────────────┐')
print('│ WARNING, PLEASE READ BEFORE PROCEEDING │')
print('│ │')
print('│ this is an irreversible operation that will remove the comma-provisioned │')
print('│ profile. │')
print('│ │')
print('│ after this operation, you must purchase a new eSIM from comma in order to │')
print('│ use the comma prime subscription again. │')
print('└──────────────────────────────────────────────────────────────────────────────┘')
print()
for severity in ('sure', '100% sure'):
print(f'are you {severity} you want to proceed? (y/N) ', end='')
confirm = input()
if confirm != 'y':
print('aborting')
exit(0)
lpa.bootstrap()
if __name__ == '__main__':
parser = argparse.ArgumentParser(prog='esim.py', description='manage eSIM profiles on your comma device', epilog='comma.ai')
parser.add_argument('--bootstrap', action='store_true', help='bootstrap the eUICC (required before downloading profiles)')
parser.add_argument('--backend', choices=['qmi', 'at'], default='qmi', help='use the specified backend, defaults to qmi')
parser.add_argument('--switch', metavar='iccid', help='switch to profile')
parser.add_argument('--delete', metavar='iccid', help='delete profile (warning: this cannot be undone)')
@@ -16,7 +38,10 @@ if __name__ == '__main__':
mutated = False
lpa = HARDWARE.get_sim_lpa()
if args.switch:
if args.bootstrap:
bootstrap(lpa)
mutated = True
elif args.switch:
lpa.switch_profile(args.switch)
mutated = True
elif args.delete:
system/hardware/hardwared.py
-7
View File
@@ -6,7 +6,6 @@ import struct
import threading
import time
from collections import OrderedDict, namedtuple
from pathlib import Path
import psutil
@@ -342,12 +341,6 @@ def hardware_thread(end_event, hw_queue) -> None:
show_alert = (not onroad_conditions["device_temp_good"] or not startup_conditions["device_temp_engageable"]) and onroad_conditions["ignition"]
set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", show_alert, extra_text=extra_text)
# TODO: this should move to TICI.initialize_hardware, but we currently can't import params there
if TICI and HARDWARE.get_device_type() == "tici":
if not os.path.isfile("/persist/comma/living-in-the-moment"):
if not Path("/data/media").is_mount():
set_offroad_alert_if_changed("Offroad_StorageMissing", True)
# Handle offroad/onroad transition
should_start = all(onroad_conditions.values())
if started_ts is None:
system/hardware/tici/agnos.json
+17 -17
View File
@@ -23,14 +23,14 @@
},
{
"name": "abl",
"url": "https://commadist.azureedge.net/agnosupdate/abl-32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6.img.xz",
"hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
"hash_raw": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
"url": "https://commadist.azureedge.net/agnosupdate/abl-556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee.img.xz",
"hash": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee",
"hash_raw": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee",
"size": 274432,
"sparse": false,
"full_check": true,
"has_ab": true,
"ondevice_hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6"
"ondevice_hash": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee"
},
{
"name": "aop",
@@ -56,29 +56,29 @@
},
{
"name": "boot",
"url": "https://commadist.azureedge.net/agnosupdate/boot-0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4.img.xz",
"hash": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4",
"hash_raw": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4",
"size": 18515968,
"url": "https://commadist.azureedge.net/agnosupdate/boot-b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb.img.xz",
"hash": "b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb",
"hash_raw": "b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb",
"size": 17442816,
"sparse": false,
"full_check": true,
"has_ab": true,
"ondevice_hash": "492ae27f569e8db457c79d0e358a7a6297d1a1c685c2b1ae6deba7315d3a6cb0"
"ondevice_hash": "8ed6c2796be5c5b29d64e6413b8e878d5bd1a3981d15216d2b5e84140cc4ea2a"
},
{
"name": "system",
"url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img.xz",
"hash": "1468d50b7ad0fda0f04074755d21e786e3b1b6ca5dd5b17eb2608202025e6126",
"hash_raw": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087",
"size": 5368709120,
"url": "https://commadist.azureedge.net/agnosupdate/system-2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc.img.xz",
"hash": "325414e5c9f7516b2bf0fedb6abe6682f717897a6d84ab70d5afe91a59f244e9",
"hash_raw": "2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc",
"size": 4718592000,
"sparse": true,
"full_check": false,
"has_ab": true,
"ondevice_hash": "242aa5adad1c04e1398e00e2440d1babf962022eb12b89adf2e60ee3068946e7",
"ondevice_hash": "79f4f6d0b5b4a416f0f31261b430943a78e37c26d0e226e0ef412fe0eae3c727",
"alt": {
"hash": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087",
"url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img",
"size": 5368709120
"hash": "2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc",
"url": "https://commadist.azureedge.net/agnosupdate/system-2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc.img",
"size": 4718592000
}
}
]
system/hardware/tici/all-partitions.json
+29 -29
View File
@@ -152,14 +152,14 @@
},
{
"name": "abl",
"url": "https://commadist.azureedge.net/agnosupdate/abl-32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6.img.xz",
"hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
"hash_raw": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
"url": "https://commadist.azureedge.net/agnosupdate/abl-556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee.img.xz",
"hash": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee",
"hash_raw": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee",
"size": 274432,
"sparse": false,
"full_check": true,
"has_ab": true,
"ondevice_hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6"
"ondevice_hash": "556bbb4ed1c671402b217bd2f3c07edce4f88b0bbd64e92241b82e396aa9ebee"
},
{
"name": "aop",
@@ -339,62 +339,62 @@
},
{
"name": "boot",
"url": "https://commadist.azureedge.net/agnosupdate/boot-0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4.img.xz",
"hash": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4",
"hash_raw": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4",
"size": 18515968,
"url": "https://commadist.azureedge.net/agnosupdate/boot-b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb.img.xz",
"hash": "b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb",
"hash_raw": "b96882012ab6cddda04f440009c798a6cff65977f984b12072e89afa592d86cb",
"size": 17442816,
"sparse": false,
"full_check": true,
"has_ab": true,
"ondevice_hash": "492ae27f569e8db457c79d0e358a7a6297d1a1c685c2b1ae6deba7315d3a6cb0"
"ondevice_hash": "8ed6c2796be5c5b29d64e6413b8e878d5bd1a3981d15216d2b5e84140cc4ea2a"
},
{
"name": "system",
"url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img.xz",
"hash": "1468d50b7ad0fda0f04074755d21e786e3b1b6ca5dd5b17eb2608202025e6126",
"hash_raw": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087",
"size": 5368709120,
"url": "https://commadist.azureedge.net/agnosupdate/system-2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc.img.xz",
"hash": "325414e5c9f7516b2bf0fedb6abe6682f717897a6d84ab70d5afe91a59f244e9",
"hash_raw": "2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc",
"size": 4718592000,
"sparse": true,
"full_check": false,
"has_ab": true,
"ondevice_hash": "242aa5adad1c04e1398e00e2440d1babf962022eb12b89adf2e60ee3068946e7",
"ondevice_hash": "79f4f6d0b5b4a416f0f31261b430943a78e37c26d0e226e0ef412fe0eae3c727",
"alt": {
"hash": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087",
"url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img",
"size": 5368709120
"hash": "2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc",
"url": "https://commadist.azureedge.net/agnosupdate/system-2b1bb223bf2100376ad5d543bfa4a483f33327b3478ec20ab36048388472c4bc.img",
"size": 4718592000
}
},
{
"name": "userdata_90",
"url": "https://commadist.azureedge.net/agnosupdate/userdata_90-602d5103cba97e1b07f76508d5febb47cfc4463a7e31bd20e461b55c801feb0a.img.xz",
"hash": "6a11d448bac50467791809339051eed2894aae971c37bf6284b3b972a99ba3ac",
"hash_raw": "602d5103cba97e1b07f76508d5febb47cfc4463a7e31bd20e461b55c801feb0a",
"url": "https://commadist.azureedge.net/agnosupdate/userdata_90-b3112984d2a8534a83d2ce43d35efdd10c7d163d9699f611f0f72ad9e9cb5af9.img.xz",
"hash": "bea163e6fb6ac6224c7f32619affb5afb834cd859971b0cab6d8297dd0098f0a",
"hash_raw": "b3112984d2a8534a83d2ce43d35efdd10c7d163d9699f611f0f72ad9e9cb5af9",
"size": 96636764160,
"sparse": true,
"full_check": true,
"has_ab": false,
"ondevice_hash": "e014d92940a696bf8582807259820ab73948b950656ed83a45da738f26083705"
"ondevice_hash": "f4841c6ae3207197886e5efbd50f44cc24822680d7b785fa2d2743c657f23287"
},
{
"name": "userdata_89",
"url": "https://commadist.azureedge.net/agnosupdate/userdata_89-4d7f6d12a5557eb6e3cbff9a4cd595677456fdfddcc879eddcea96a43a9d8b48.img.xz",
"hash": "748e31a5fc01fc256c012e359c3382d1f98cce98feafe8ecc0fca3e47caef116",
"hash_raw": "4d7f6d12a5557eb6e3cbff9a4cd595677456fdfddcc879eddcea96a43a9d8b48",
"url": "https://commadist.azureedge.net/agnosupdate/userdata_89-3e63f670e4270474cec96f4da9250ee4e87e3106b0b043b7e82371e1c761e167.img.xz",
"hash": "b5458a29dd7d4a4c9b7ad77b8baa5f804142ac78d97c6668839bf2a650e32518",
"hash_raw": "3e63f670e4270474cec96f4da9250ee4e87e3106b0b043b7e82371e1c761e167",
"size": 95563022336,
"sparse": true,
"full_check": true,
"has_ab": false,
"ondevice_hash": "c181b93050787adcfef730c086bcb780f28508d84e6376d9b80d37e5dc02b55e"
"ondevice_hash": "1dc10c542d3b019258fc08dc7dfdb49d9abad065e46d030b89bc1a2e0197f526"
},
{
"name": "userdata_30",
"url": "https://commadist.azureedge.net/agnosupdate/userdata_30-80a76c8e56bbd7536fd5e87e8daa12984e2960db4edeb1f83229b2baeecc4668.img.xz",
"hash": "09ff390e639e4373d772e1688d05a5ac77a573463ed1deeff86390686fa686f9",
"hash_raw": "80a76c8e56bbd7536fd5e87e8daa12984e2960db4edeb1f83229b2baeecc4668",
"url": "https://commadist.azureedge.net/agnosupdate/userdata_30-1d3885d4370974e55f0c6f567fd0344fc5ee10db067aa5810fbaf402eadb032c.img.xz",
"hash": "687d178cfc91be5d7e8aa1333405b610fdce01775b8333bd0985b81642b94eea",
"hash_raw": "1d3885d4370974e55f0c6f567fd0344fc5ee10db067aa5810fbaf402eadb032c",
"size": 32212254720,
"sparse": true,
"full_check": true,
"has_ab": false,
"ondevice_hash": "2c01ab470c02121c721ff6afc25582437e821686207f3afef659387afb69c507"
"ondevice_hash": "9ddbd1dae6ee7dc919f018364cf2f29dad138c9203c5a49aea0cbb9bf2e137e5"
}
]
system/hardware/tici/amplifier.py
-12
View File
@@ -61,18 +61,6 @@ BASE_CONFIG = [
]
CONFIGS = {
"tici": [
AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111),
AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100),
AmpConfig("Right speaker output volume", 0x1c, 0x3E, 0, 0b00011111),
AmpConfig("DAI2 EQ enable", 0b1, 0x49, 1, 0b00000010),
*configs_from_eq_params(0x84, EQParams(0x274F, 0xC0FF, 0x3BF9, 0x0B3C, 0x1656)),
*configs_from_eq_params(0x8E, EQParams(0x1009, 0xC6BF, 0x2952, 0x1C97, 0x30DF)),
*configs_from_eq_params(0x98, EQParams(0x0F75, 0xCBE5, 0x0ED2, 0x2528, 0x3E42)),
*configs_from_eq_params(0xA2, EQParams(0x091F, 0x3D4C, 0xCE11, 0x1266, 0x2807)),
*configs_from_eq_params(0xAC, EQParams(0x0A9E, 0x3F20, 0xE573, 0x0A8B, 0x3A3B)),
],
"tizi": [
AmpConfig("Left speaker output from left DAC", 0b1, 0x2B, 0, 0b11111111),
AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111),
system/hardware/tici/esim.py
+29
View File
@@ -40,6 +40,7 @@ class TiciLPA(LPABase):
self._process_notifications()
def download_profile(self, qr: str, nickname: str | None = None) -> None:
self._check_bootstrapped()
msgs = self._invoke('profile', 'download', '-a', qr)
self._validate_successful(msgs)
new_profile = next((m for m in msgs if m['payload']['message'] == 'es8p_meatadata_parse'), None)
@@ -54,6 +55,7 @@ class TiciLPA(LPABase):
self._validate_successful(self._invoke('profile', 'nickname', iccid, nickname))
def switch_profile(self, iccid: str) -> None:
self._check_bootstrapped()
self._validate_profile_exists(iccid)
latest = self.get_active_profile()
if latest and latest.iccid == iccid:
@@ -61,6 +63,33 @@ class TiciLPA(LPABase):
self._validate_successful(self._invoke('profile', 'enable', iccid))
self._process_notifications()
def bootstrap(self) -> None:
"""
find all comma-provisioned profiles and delete them. they conflict with user-provisioned profiles
and must be deleted.
**note**: this is a **very** destructive operation. you **must** purchase a new comma SIM in order
to use comma prime again.
"""
if self._is_bootstrapped():
return
for p in self.list_profiles():
if self.is_comma_profile(p.iccid):
self._disable_profile(p.iccid)
self.delete_profile(p.iccid)
def _disable_profile(self, iccid: str) -> None:
self._validate_successful(self._invoke('profile', 'disable', iccid))
self._process_notifications()
def _check_bootstrapped(self) -> None:
assert self._is_bootstrapped(), 'eUICC is not bootstrapped, please bootstrap before performing this operation'
def _is_bootstrapped(self) -> bool:
""" check if any comma provisioned profiles are on the eUICC """
return not any(self.is_comma_profile(iccid) for iccid in (p.iccid for p in self.list_profiles()))
def _invoke(self, *cmd: str):
proc = subprocess.Popen(['sudo', '-E', 'lpac'] + list(cmd), stdout=subprocess.PIPE, stderr=subprocess.PIPE, env=self.env)
try:
system/hardware/tici/hardware.py
+14 -11
View File
@@ -425,9 +425,6 @@ class Tici(HardwareBase):
# pandad core
affine_irq(3, "spi_geni") # SPI
if "tici" in self.get_device_type():
affine_irq(3, "xhci-hcd:usb3") # aux panda USB (or potentially anything else on USB)
affine_irq(3, "xhci-hcd:usb1") # internal panda USB (also modem)
try:
pid = subprocess.check_output(["pgrep", "-f", "spi0"], encoding='utf8').strip()
subprocess.call(["sudo", "chrt", "-f", "-p", "1", pid])
@@ -446,22 +443,20 @@ class Tici(HardwareBase):
cmds = []
if self.get_device_type() in ("tici", "tizi"):
if self.get_device_type() in ("tizi", ):
# clear out old blue prime initial APN
os.system('mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn="')
cmds += [
# SIM hot swap
'AT+QSIMDET=1,0',
'AT+QSIMSTAT=1',
# configure modem as data-centric
'AT+QNVW=5280,0,"0102000000000000"',
'AT+QNVFW="/nv/item_files/ims/IMS_enable",00',
'AT+QNVFW="/nv/item_files/modem/mmode/ue_usage_setting",01',
]
if self.get_device_type() == "tizi":
# SIM hot swap, not routed on tici
cmds += [
'AT+QSIMDET=1,0',
'AT+QSIMSTAT=1',
]
elif manufacturer == 'Cavli Inc.':
cmds += [
'AT^SIMSWAP=1', # use SIM slot, instead of internal eSIM
@@ -492,7 +487,7 @@ class Tici(HardwareBase):
# eSIM prime
dest = "/etc/NetworkManager/system-connections/esim.nmconnection"
if sim_id.startswith('8985235') and not os.path.exists(dest):
if self.get_sim_lpa().is_comma_profile(sim_id) and not os.path.exists(dest):
with open(Path(__file__).parent/'esim.nmconnection') as f, tempfile.NamedTemporaryFile(mode='w') as tf:
dat = f.read()
dat = dat.replace("sim-id=", f"sim-id={sim_id}")
@@ -503,6 +498,14 @@ class Tici(HardwareBase):
os.system(f"sudo cp {tf.name} {dest}")
os.system(f"sudo nmcli con load {dest}")
def reboot_modem(self):
modem = self.get_modem()
for state in (0, 1):
try:
modem.Command(f'AT+CFUN={state}', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
except Exception:
pass
def get_networks(self):
r = {}
system/hardware/tici/pins.py
-5
View File
@@ -1,5 +1,3 @@
# TODO: these are also defined in a header
# GPIO pin definitions
class GPIO:
# both GPIO_STM_RST_N and GPIO_LTE_RST_N are misnamed, they are high to reset
@@ -26,7 +24,4 @@ class GPIO:
CAM2_RSTN = 12
# Sensor interrupts
BMX055_ACCEL_INT = 21
BMX055_GYRO_INT = 23
BMX055_MAGN_INT = 87
LSM_INT = 84
system/hardware/tici/tests/test_power_draw.py
+3 -3
View File
@@ -31,9 +31,9 @@ class Proc:
PROCS = [
Proc(['camerad'], 1.75, msgs=['roadCameraState', 'wideRoadCameraState', 'driverCameraState']),
Proc(['modeld'], 1.12, atol=0.2, msgs=['modelV2']),
Proc(['dmonitoringmodeld'], 0.6, msgs=['driverStateV2']),
Proc(['camerad'], 1.65, atol=0.4, msgs=['roadCameraState', 'wideRoadCameraState', 'driverCameraState']),
Proc(['modeld'], 1.24, atol=0.2, msgs=['modelV2']),
Proc(['dmonitoringmodeld'], 0.65, atol=0.35, msgs=['driverStateV2']),
Proc(['encoderd'], 0.23, msgs=[]),
]
system/journald.py
Executable
+43
View File
@@ -0,0 +1,43 @@
#!/usr/bin/env python3
import json
import subprocess
import cereal.messaging as messaging
from openpilot.common.swaglog import cloudlog
def main():
pm = messaging.PubMaster(['androidLog'])
cmd = ['journalctl', '-f', '-o', 'json']
proc = subprocess.Popen(cmd, stdout=subprocess.PIPE, text=True)
assert proc.stdout is not None
try:
for line in proc.stdout:
line = line.strip()
if not line:
continue
try:
kv = json.loads(line)
except json.JSONDecodeError:
cloudlog.exception("failed to parse journalctl output")
continue
msg = messaging.new_message('androidLog')
entry = msg.androidLog
entry.ts = int(kv.get('__REALTIME_TIMESTAMP', 0))
entry.message = json.dumps(kv)
if '_PID' in kv:
entry.pid = int(kv['_PID'])
if 'PRIORITY' in kv:
entry.priority = int(kv['PRIORITY'])
if 'SYSLOG_IDENTIFIER' in kv:
entry.tag = kv['SYSLOG_IDENTIFIER']
pm.send('androidLog', msg)
finally:
proc.terminate()
proc.wait()
if __name__ == '__main__':
main()
system/logcatd/.gitignore
-1
View File
@@ -1 +0,0 @@
logcatd
system/logcatd/SConscript
-3
View File
@@ -1,3 +0,0 @@
Import('env', 'messaging', 'common')
env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[messaging, common, 'systemd'])
system/logcatd/logcatd_systemd.cc
-75
View File
@@ -1,75 +0,0 @@
#include <systemd/sd-journal.h>
#include <cassert>
#include <csignal>
#include <map>
#include <string>
#include "third_party/json11/json11.hpp"
#include "cereal/messaging/messaging.h"
#include "common/timing.h"
#include "common/util.h"
ExitHandler do_exit;
int main(int argc, char *argv[]) {
PubMaster pm({"androidLog"});
sd_journal *journal;
int err = sd_journal_open(&journal, 0);
assert(err >= 0);
err = sd_journal_get_fd(journal); // needed so sd_journal_wait() works properly if files rotate
assert(err >= 0);
err = sd_journal_seek_tail(journal);
assert(err >= 0);
// workaround for bug https://github.com/systemd/systemd/issues/9934
// call sd_journal_previous_skip after sd_journal_seek_tail (like journalctl -f does) to makes things work.
sd_journal_previous_skip(journal, 1);
while (!do_exit) {
err = sd_journal_next(journal);
assert(err >= 0);
// Wait for new message if we didn't receive anything
if (err == 0) {
err = sd_journal_wait(journal, 1000 * 1000);
assert(err >= 0);
continue; // Try again
}
uint64_t timestamp = 0;
err = sd_journal_get_realtime_usec(journal, &timestamp);
assert(err >= 0);
const void *data;
size_t length;
std::map<std::string, std::string> kv;
SD_JOURNAL_FOREACH_DATA(journal, data, length) {
std::string str((char*)data, length);
// Split "KEY=VALUE"" on "=" and put in map
std::size_t found = str.find("=");
if (found != std::string::npos) {
kv[str.substr(0, found)] = str.substr(found + 1, std::string::npos);
}
}
MessageBuilder msg;
// Build message
auto androidEntry = msg.initEvent().initAndroidLog();
androidEntry.setTs(timestamp);
androidEntry.setMessage(json11::Json(kv).dump());
if (kv.count("_PID")) androidEntry.setPid(std::atoi(kv["_PID"].c_str()));
if (kv.count("PRIORITY")) androidEntry.setPriority(std::atoi(kv["PRIORITY"].c_str()));
if (kv.count("SYSLOG_IDENTIFIER")) androidEntry.setTag(kv["SYSLOG_IDENTIFIER"]);
pm.send("androidLog", msg);
}
sd_journal_close(journal);
return 0;
}
system/loggerd/tests/vidc_debug.sh
Executable
+13
View File
@@ -0,0 +1,13 @@
#!/usr/bin/env bash
set -e
cd /sys/kernel/debug/tracing
echo "" > trace
echo 1 > tracing_on
echo 1 > /sys/kernel/debug/tracing/events/msm_vidc/enable
echo 0xff > /sys/module/videobuf2_core/parameters/debug
echo 0x7fffffff > /sys/kernel/debug/msm_vidc/debug_level
echo 0xff > /sys/devices/platform/soc/aa00000.qcom,vidc/video4linux/video33/dev_debug
cat /sys/kernel/debug/tracing/trace_pipe
system/manager/process_config.py
+3 -3
View File
@@ -111,8 +111,8 @@ procs = [
NativeProcess("camerad", "system/camerad", ["./camerad"], driverview, enabled=not WEBCAM),
PythonProcess("webcamerad", "tools.webcam.camerad", driverview, enabled=WEBCAM),
NativeProcess("logcatd", "system/logcatd", ["./logcatd"], only_onroad, platform.system() != "Darwin"),
NativeProcess("proclogd", "system/proclogd", ["./proclogd"], only_onroad, platform.system() != "Darwin"),
PythonProcess("proclogd", "system.proclogd", only_onroad, enabled=platform.system() != "Darwin"),
PythonProcess("journald", "system.journald", only_onroad, platform.system() != "Darwin"),
PythonProcess("micd", "system.micd", iscar),
PythonProcess("timed", "system.timed", always_run, enabled=not PC),
@@ -137,7 +137,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/proclogd.py
Executable
+227
View File
@@ -0,0 +1,227 @@
#!/usr/bin/env python3
import os
from typing import NoReturn, TypedDict
from cereal import messaging
from openpilot.common.realtime import Ratekeeper
from openpilot.common.swaglog import cloudlog
JIFFY = os.sysconf(os.sysconf_names['SC_CLK_TCK'])
PAGE_SIZE = os.sysconf(os.sysconf_names['SC_PAGE_SIZE'])
def _cpu_times() -> list[dict[str, float]]:
cpu_times: list[dict[str, float]] = []
try:
with open('/proc/stat') as f:
lines = f.readlines()[1:]
for line in lines:
if not line.startswith('cpu') or len(line) < 4 or not line[3].isdigit():
break
parts = line.split()
cpu_times.append({
'cpuNum': int(parts[0][3:]),
'user': float(parts[1]) / JIFFY,
'nice': float(parts[2]) / JIFFY,
'system': float(parts[3]) / JIFFY,
'idle': float(parts[4]) / JIFFY,
'iowait': float(parts[5]) / JIFFY,
'irq': float(parts[6]) / JIFFY,
'softirq': float(parts[7]) / JIFFY,
})
except Exception:
cloudlog.exception("failed to read /proc/stat")
return cpu_times
def _mem_info() -> dict[str, int]:
keys = ["MemTotal:", "MemFree:", "MemAvailable:", "Buffers:", "Cached:", "Active:", "Inactive:", "Shmem:"]
info: dict[str, int] = dict.fromkeys(keys, 0)
try:
with open('/proc/meminfo') as f:
for line in f:
parts = line.split()
if parts and parts[0] in info:
info[parts[0]] = int(parts[1]) * 1024
except Exception:
cloudlog.exception("failed to read /proc/meminfo")
return info
_STAT_POS = {
'pid': 1,
'state': 3,
'ppid': 4,
'utime': 14,
'stime': 15,
'cutime': 16,
'cstime': 17,
'priority': 18,
'nice': 19,
'num_threads': 20,
'starttime': 22,
'vsize': 23,
'rss': 24,
'processor': 39,
}
class ProcStat(TypedDict):
name: str
pid: int
state: str
ppid: int
utime: int
stime: int
cutime: int
cstime: int
priority: int
nice: int
num_threads: int
starttime: int
vms: int
rss: int
processor: int
def _parse_proc_stat(stat: str) -> ProcStat | None:
open_paren = stat.find('(')
close_paren = stat.rfind(')')
if open_paren == -1 or close_paren == -1 or open_paren > close_paren:
return None
name = stat[open_paren + 1:close_paren]
stat = stat[:open_paren] + stat[open_paren:close_paren].replace(' ', '_') + stat[close_paren:]
parts = stat.split()
if len(parts) < 52:
return None
try:
return {
'name': name,
'pid': int(parts[_STAT_POS['pid'] - 1]),
'state': parts[_STAT_POS['state'] - 1][0],
'ppid': int(parts[_STAT_POS['ppid'] - 1]),
'utime': int(parts[_STAT_POS['utime'] - 1]),
'stime': int(parts[_STAT_POS['stime'] - 1]),
'cutime': int(parts[_STAT_POS['cutime'] - 1]),
'cstime': int(parts[_STAT_POS['cstime'] - 1]),
'priority': int(parts[_STAT_POS['priority'] - 1]),
'nice': int(parts[_STAT_POS['nice'] - 1]),
'num_threads': int(parts[_STAT_POS['num_threads'] - 1]),
'starttime': int(parts[_STAT_POS['starttime'] - 1]),
'vms': int(parts[_STAT_POS['vsize'] - 1]),
'rss': int(parts[_STAT_POS['rss'] - 1]),
'processor': int(parts[_STAT_POS['processor'] - 1]),
}
except Exception:
cloudlog.exception("failed to parse /proc/<pid>/stat")
return None
class ProcExtra(TypedDict):
pid: int
name: str
exe: str
cmdline: list[str]
_proc_cache: dict[int, ProcExtra] = {}
def _get_proc_extra(pid: int, name: str) -> ProcExtra:
cache: ProcExtra | None = _proc_cache.get(pid)
if cache is None or cache.get('name') != name:
exe = ''
cmdline: list[str] = []
try:
exe = os.readlink(f'/proc/{pid}/exe')
except OSError:
pass
try:
with open(f'/proc/{pid}/cmdline', 'rb') as f:
cmdline = [c.decode('utf-8', errors='replace') for c in f.read().split(b'\0') if c]
except OSError:
pass
cache = {'pid': pid, 'name': name, 'exe': exe, 'cmdline': cmdline}
_proc_cache[pid] = cache
return cache
def _procs() -> list[ProcStat]:
stats: list[ProcStat] = []
for pid_str in os.listdir('/proc'):
if not pid_str.isdigit():
continue
try:
with open(f'/proc/{pid_str}/stat') as f:
stat = f.read()
parsed = _parse_proc_stat(stat)
if parsed is not None:
stats.append(parsed)
except OSError:
continue
return stats
def build_proc_log_message(msg) -> None:
pl = msg.procLog
procs = _procs()
l = pl.init('procs', len(procs))
for i, r in enumerate(procs):
proc = l[i]
proc.pid = r['pid']
proc.state = ord(r['state'][0])
proc.ppid = r['ppid']
proc.cpuUser = r['utime'] / JIFFY
proc.cpuSystem = r['stime'] / JIFFY
proc.cpuChildrenUser = r['cutime'] / JIFFY
proc.cpuChildrenSystem = r['cstime'] / JIFFY
proc.priority = r['priority']
proc.nice = r['nice']
proc.numThreads = r['num_threads']
proc.startTime = r['starttime'] / JIFFY
proc.memVms = r['vms']
proc.memRss = r['rss'] * PAGE_SIZE
proc.processor = r['processor']
proc.name = r['name']
extra = _get_proc_extra(r['pid'], r['name'])
proc.exe = extra['exe']
cmdline = proc.init('cmdline', len(extra['cmdline']))
for j, arg in enumerate(extra['cmdline']):
cmdline[j] = arg
cpu_times = _cpu_times()
cpu_list = pl.init('cpuTimes', len(cpu_times))
for i, ct in enumerate(cpu_times):
cpu = cpu_list[i]
cpu.cpuNum = ct['cpuNum']
cpu.user = ct['user']
cpu.nice = ct['nice']
cpu.system = ct['system']
cpu.idle = ct['idle']
cpu.iowait = ct['iowait']
cpu.irq = ct['irq']
cpu.softirq = ct['softirq']
mem_info = _mem_info()
pl.mem.total = mem_info["MemTotal:"]
pl.mem.free = mem_info["MemFree:"]
pl.mem.available = mem_info["MemAvailable:"]
pl.mem.buffers = mem_info["Buffers:"]
pl.mem.cached = mem_info["Cached:"]
pl.mem.active = mem_info["Active:"]
pl.mem.inactive = mem_info["Inactive:"]
pl.mem.shared = mem_info["Shmem:"]
def main() -> NoReturn:
pm = messaging.PubMaster(['procLog'])
rk = Ratekeeper(0.5)
while True:
msg = messaging.new_message('procLog', valid=True)
build_proc_log_message(msg)
pm.send('procLog', msg)
rk.keep_time()
if __name__ == '__main__':
main()
system/proclogd/SConscript
-6
View File
@@ -1,6 +0,0 @@
Import('env', 'messaging', 'common')
libs = [messaging, 'pthread', common]
env.Program('proclogd', ['main.cc', 'proclog.cc'], LIBS=libs)
if GetOption('extras'):
env.Program('tests/test_proclog', ['tests/test_proclog.cc', 'proclog.cc'], LIBS=libs)
system/proclogd/main.cc
-25
View File
@@ -1,25 +0,0 @@
#include <sys/resource.h>
#include "common/ratekeeper.h"
#include "common/util.h"
#include "system/proclogd/proclog.h"
ExitHandler do_exit;
int main(int argc, char **argv) {
setpriority(PRIO_PROCESS, 0, -15);
RateKeeper rk("proclogd", 0.5);
PubMaster publisher({"procLog"});
while (!do_exit) {
MessageBuilder msg;
buildProcLogMessage(msg);
publisher.send("procLog", msg);
rk.keepTime();
}
return 0;
}
system/proclogd/proclog.cc
-239
View File
@@ -1,239 +0,0 @@
#include "system/proclogd/proclog.h"
#include <dirent.h>
#include <cassert>
#include <fstream>
#include <iterator>
#include <sstream>
#include "common/swaglog.h"
#include "common/util.h"
namespace Parser {
// parse /proc/stat
std::vector<CPUTime> cpuTimes(std::istream &stream) {
std::vector<CPUTime> cpu_times;
std::string line;
// skip the first line for cpu total
std::getline(stream, line);
while (std::getline(stream, line)) {
if (line.compare(0, 3, "cpu") != 0) break;
CPUTime t = {};
std::istringstream iss(line);
if (iss.ignore(3) >> t.id >> t.utime >> t.ntime >> t.stime >> t.itime >> t.iowtime >> t.irqtime >> t.sirqtime)
cpu_times.push_back(t);
}
return cpu_times;
}
// parse /proc/meminfo
std::unordered_map<std::string, uint64_t> memInfo(std::istream &stream) {
std::unordered_map<std::string, uint64_t> mem_info;
std::string line, key;
while (std::getline(stream, line)) {
uint64_t val = 0;
std::istringstream iss(line);
if (iss >> key >> val) {
mem_info[key] = val * 1024;
}
}
return mem_info;
}
// field position (https://man7.org/linux/man-pages/man5/proc.5.html)
enum StatPos {
pid = 1,
state = 3,
ppid = 4,
utime = 14,
stime = 15,
cutime = 16,
cstime = 17,
priority = 18,
nice = 19,
num_threads = 20,
starttime = 22,
vsize = 23,
rss = 24,
processor = 39,
MAX_FIELD = 52,
};
// parse /proc/pid/stat
std::optional<ProcStat> procStat(std::string stat) {
// To avoid being fooled by names containing a closing paren, scan backwards.
auto open_paren = stat.find('(');
auto close_paren = stat.rfind(')');
if (open_paren == std::string::npos || close_paren == std::string::npos || open_paren > close_paren) {
return std::nullopt;
}
std::string name = stat.substr(open_paren + 1, close_paren - open_paren - 1);
// replace space in name with _
std::replace(&stat[open_paren], &stat[close_paren], ' ', '_');
std::istringstream iss(stat);
std::vector<std::string> v{std::istream_iterator<std::string>(iss),
std::istream_iterator<std::string>()};
try {
if (v.size() != StatPos::MAX_FIELD) {
throw std::invalid_argument("stat");
}
ProcStat p = {
.name = name,
.pid = stoi(v[StatPos::pid - 1]),
.state = v[StatPos::state - 1][0],
.ppid = stoi(v[StatPos::ppid - 1]),
.utime = stoul(v[StatPos::utime - 1]),
.stime = stoul(v[StatPos::stime - 1]),
.cutime = stol(v[StatPos::cutime - 1]),
.cstime = stol(v[StatPos::cstime - 1]),
.priority = stol(v[StatPos::priority - 1]),
.nice = stol(v[StatPos::nice - 1]),
.num_threads = stol(v[StatPos::num_threads - 1]),
.starttime = stoull(v[StatPos::starttime - 1]),
.vms = stoul(v[StatPos::vsize - 1]),
.rss = stol(v[StatPos::rss - 1]),
.processor = stoi(v[StatPos::processor - 1]),
};
return p;
} catch (const std::invalid_argument &e) {
LOGE("failed to parse procStat (%s) :%s", e.what(), stat.c_str());
} catch (const std::out_of_range &e) {
LOGE("failed to parse procStat (%s) :%s", e.what(), stat.c_str());
}
return std::nullopt;
}
// return list of PIDs from /proc
std::vector<int> pids() {
std::vector<int> ids;
DIR *d = opendir("/proc");
assert(d);
char *p_end;
struct dirent *de = NULL;
while ((de = readdir(d))) {
if (de->d_type == DT_DIR) {
int pid = strtol(de->d_name, &p_end, 10);
if (p_end == (de->d_name + strlen(de->d_name))) {
ids.push_back(pid);
}
}
}
closedir(d);
return ids;
}
// null-delimited cmdline arguments to vector
std::vector<std::string> cmdline(std::istream &stream) {
std::vector<std::string> ret;
std::string line;
while (std::getline(stream, line, '\0')) {
if (!line.empty()) {
ret.push_back(line);
}
}
return ret;
}
const ProcCache &getProcExtraInfo(int pid, const std::string &name) {
static std::unordered_map<pid_t, ProcCache> proc_cache;
ProcCache &cache = proc_cache[pid];
if (cache.pid != pid || cache.name != name) {
cache.pid = pid;
cache.name = name;
std::string proc_path = "/proc/" + std::to_string(pid);
cache.exe = util::readlink(proc_path + "/exe");
std::ifstream stream(proc_path + "/cmdline");
cache.cmdline = cmdline(stream);
}
return cache;
}
} // namespace Parser
const double jiffy = sysconf(_SC_CLK_TCK);
const size_t page_size = sysconf(_SC_PAGE_SIZE);
void buildCPUTimes(cereal::ProcLog::Builder &builder) {
std::ifstream stream("/proc/stat");
std::vector<CPUTime> stats = Parser::cpuTimes(stream);
auto log_cpu_times = builder.initCpuTimes(stats.size());
for (int i = 0; i < stats.size(); ++i) {
auto l = log_cpu_times[i];
const CPUTime &r = stats[i];
l.setCpuNum(r.id);
l.setUser(r.utime / jiffy);
l.setNice(r.ntime / jiffy);
l.setSystem(r.stime / jiffy);
l.setIdle(r.itime / jiffy);
l.setIowait(r.iowtime / jiffy);
l.setIrq(r.irqtime / jiffy);
l.setSoftirq(r.sirqtime / jiffy);
}
}
void buildMemInfo(cereal::ProcLog::Builder &builder) {
std::ifstream stream("/proc/meminfo");
auto mem_info = Parser::memInfo(stream);
auto mem = builder.initMem();
mem.setTotal(mem_info["MemTotal:"]);
mem.setFree(mem_info["MemFree:"]);
mem.setAvailable(mem_info["MemAvailable:"]);
mem.setBuffers(mem_info["Buffers:"]);
mem.setCached(mem_info["Cached:"]);
mem.setActive(mem_info["Active:"]);
mem.setInactive(mem_info["Inactive:"]);
mem.setShared(mem_info["Shmem:"]);
}
void buildProcs(cereal::ProcLog::Builder &builder) {
auto pids = Parser::pids();
std::vector<ProcStat> proc_stats;
proc_stats.reserve(pids.size());
for (int pid : pids) {
std::string path = "/proc/" + std::to_string(pid) + "/stat";
if (auto stat = Parser::procStat(util::read_file(path))) {
proc_stats.push_back(*stat);
}
}
auto procs = builder.initProcs(proc_stats.size());
for (size_t i = 0; i < proc_stats.size(); i++) {
auto l = procs[i];
const ProcStat &r = proc_stats[i];
l.setPid(r.pid);
l.setState(r.state);
l.setPpid(r.ppid);
l.setCpuUser(r.utime / jiffy);
l.setCpuSystem(r.stime / jiffy);
l.setCpuChildrenUser(r.cutime / jiffy);
l.setCpuChildrenSystem(r.cstime / jiffy);
l.setPriority(r.priority);
l.setNice(r.nice);
l.setNumThreads(r.num_threads);
l.setStartTime(r.starttime / jiffy);
l.setMemVms(r.vms);
l.setMemRss((uint64_t)r.rss * page_size);
l.setProcessor(r.processor);
l.setName(r.name);
const ProcCache &extra_info = Parser::getProcExtraInfo(r.pid, r.name);
l.setExe(extra_info.exe);
auto lcmdline = l.initCmdline(extra_info.cmdline.size());
for (size_t j = 0; j < lcmdline.size(); j++) {
lcmdline.set(j, extra_info.cmdline[j]);
}
}
}
void buildProcLogMessage(MessageBuilder &msg) {
auto procLog = msg.initEvent().initProcLog();
buildProcs(procLog);
buildCPUTimes(procLog);
buildMemInfo(procLog);
}
system/proclogd/proclog.h
-40
View File
@@ -1,40 +0,0 @@
#include <optional>
#include <string>
#include <unordered_map>
#include <vector>
#include "cereal/messaging/messaging.h"
struct CPUTime {
int id;
unsigned long utime, ntime, stime, itime;
unsigned long iowtime, irqtime, sirqtime;
};
struct ProcCache {
int pid;
std::string name, exe;
std::vector<std::string> cmdline;
};
struct ProcStat {
int pid, ppid, processor;
char state;
long cutime, cstime, priority, nice, num_threads, rss;
unsigned long utime, stime, vms;
unsigned long long starttime;
std::string name;
};
namespace Parser {
std::vector<int> pids();
std::optional<ProcStat> procStat(std::string stat);
std::vector<std::string> cmdline(std::istream &stream);
std::vector<CPUTime> cpuTimes(std::istream &stream);
std::unordered_map<std::string, uint64_t> memInfo(std::istream &stream);
const ProcCache &getProcExtraInfo(int pid, const std::string &name);
}; // namespace Parser
void buildProcLogMessage(MessageBuilder &msg);
system/proclogd/tests/.gitignore
-1
View File
@@ -1 +0,0 @@
test_proclog
system/proclogd/tests/test_proclog.cc
-142
View File
@@ -1,142 +0,0 @@
#define CATCH_CONFIG_MAIN
#include "catch2/catch.hpp"
#include "common/util.h"
#include "system/proclogd/proclog.h"
const std::string allowed_states = "RSDTZtWXxKWPI";
TEST_CASE("Parser::procStat") {
SECTION("from string") {
const std::string stat_str =
"33012 (code )) S 32978 6620 6620 0 -1 4194368 2042377 0 144 0 24510 11627 0 "
"0 20 0 39 0 53077 830029824 62214 18446744073709551615 94257242783744 94257366235808 "
"140735738643248 0 0 0 0 4098 1073808632 0 0 0 17 2 0 0 2 0 0 94257370858656 94257371248232 "
"94257404952576 140735738648768 140735738648823 140735738648823 140735738650595 0";
auto stat = Parser::procStat(stat_str);
REQUIRE(stat);
REQUIRE(stat->pid == 33012);
REQUIRE(stat->name == "code )");
REQUIRE(stat->state == 'S');
REQUIRE(stat->ppid == 32978);
REQUIRE(stat->utime == 24510);
REQUIRE(stat->stime == 11627);
REQUIRE(stat->cutime == 0);
REQUIRE(stat->cstime == 0);
REQUIRE(stat->priority == 20);
REQUIRE(stat->nice == 0);
REQUIRE(stat->num_threads == 39);
REQUIRE(stat->starttime == 53077);
REQUIRE(stat->vms == 830029824);
REQUIRE(stat->rss == 62214);
REQUIRE(stat->processor == 2);
}
SECTION("all processes") {
std::vector<int> pids = Parser::pids();
REQUIRE(pids.size() > 1);
for (int pid : pids) {
std::string stat_path = "/proc/" + std::to_string(pid) + "/stat";
INFO(stat_path);
if (auto stat = Parser::procStat(util::read_file(stat_path))) {
REQUIRE(stat->pid == pid);
REQUIRE(allowed_states.find(stat->state) != std::string::npos);
} else {
REQUIRE(util::file_exists(stat_path) == false);
}
}
}
}
TEST_CASE("Parser::cpuTimes") {
SECTION("from string") {
std::string stat =
"cpu 0 0 0 0 0 0 0 0 0 0\n"
"cpu0 1 2 3 4 5 6 7 8 9 10\n"
"cpu1 1 2 3 4 5 6 7 8 9 10\n";
std::istringstream stream(stat);
auto stats = Parser::cpuTimes(stream);
REQUIRE(stats.size() == 2);
for (int i = 0; i < stats.size(); ++i) {
REQUIRE(stats[i].id == i);
REQUIRE(stats[i].utime == 1);
REQUIRE(stats[i].ntime ==2);
REQUIRE(stats[i].stime == 3);
REQUIRE(stats[i].itime == 4);
REQUIRE(stats[i].iowtime == 5);
REQUIRE(stats[i].irqtime == 6);
REQUIRE(stats[i].sirqtime == 7);
}
}
SECTION("all cpus") {
std::istringstream stream(util::read_file("/proc/stat"));
auto stats = Parser::cpuTimes(stream);
REQUIRE(stats.size() == sysconf(_SC_NPROCESSORS_ONLN));
for (int i = 0; i < stats.size(); ++i) {
REQUIRE(stats[i].id == i);
}
}
}
TEST_CASE("Parser::memInfo") {
SECTION("from string") {
std::istringstream stream("MemTotal: 1024 kb\nMemFree: 2048 kb\n");
auto meminfo = Parser::memInfo(stream);
REQUIRE(meminfo["MemTotal:"] == 1024 * 1024);
REQUIRE(meminfo["MemFree:"] == 2048 * 1024);
}
SECTION("from /proc/meminfo") {
std::string require_keys[] = {"MemTotal:", "MemFree:", "MemAvailable:", "Buffers:", "Cached:", "Active:", "Inactive:", "Shmem:"};
std::istringstream stream(util::read_file("/proc/meminfo"));
auto meminfo = Parser::memInfo(stream);
for (auto &key : require_keys) {
REQUIRE(meminfo.find(key) != meminfo.end());
REQUIRE(meminfo[key] > 0);
}
}
}
void test_cmdline(std::string cmdline, const std::vector<std::string> requires) {
std::stringstream ss;
ss.write(&cmdline[0], cmdline.size());
auto cmds = Parser::cmdline(ss);
REQUIRE(cmds.size() == requires.size());
for (int i = 0; i < requires.size(); ++i) {
REQUIRE(cmds[i] == requires[i]);
}
}
TEST_CASE("Parser::cmdline") {
test_cmdline(std::string("a\0b\0c\0", 7), {"a", "b", "c"});
test_cmdline(std::string("a\0\0c\0", 6), {"a", "c"});
test_cmdline(std::string("a\0b\0c\0\0\0", 9), {"a", "b", "c"});
}
TEST_CASE("buildProcLoggerMessage") {
MessageBuilder msg;
buildProcLogMessage(msg);
kj::Array<capnp::word> buf = capnp::messageToFlatArray(msg);
capnp::FlatArrayMessageReader reader(buf);
auto log = reader.getRoot<cereal::Event>().getProcLog();
REQUIRE(log.totalSize().wordCount > 0);
// test cereal::ProcLog::CPUTimes
auto cpu_times = log.getCpuTimes();
REQUIRE(cpu_times.size() == sysconf(_SC_NPROCESSORS_ONLN));
REQUIRE(cpu_times[cpu_times.size() - 1].getCpuNum() == cpu_times.size() - 1);
// test cereal::ProcLog::Mem
auto mem = log.getMem();
REQUIRE(mem.getTotal() > 0);
REQUIRE(mem.getShared() > 0);
// test cereal::ProcLog::Process
auto procs = log.getProcs();
for (auto p : procs) {
REQUIRE(allowed_states.find(p.getState()) != std::string::npos);
if (p.getPid() == ::getpid()) {
REQUIRE(p.getName() == "test_proclog");
REQUIRE(p.getState() == 'R');
REQUIRE_THAT(p.getExe().cStr(), Catch::Matchers::Contains("test_proclog"));
REQUIRE_THAT(p.getCmdline()[0], Catch::Matchers::Contains("test_proclog"));
}
}
}
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/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;
};

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