Merge remote-tracking branch 'upstream/master' into elantra-2019-fw

pull/25951/head
Shane Smiskol 3 years ago
commit 2b3d1f2cfc
  1. 1
      .github/ISSUE_TEMPLATE/bug_report.yml
  2. 5
      .github/ISSUE_TEMPLATE/config.yml
  3. 2
      .github/workflows/selfdrive_tests.yaml
  4. 2
      .gitmodules
  5. 7
      .pre-commit-config.yaml
  6. 2
      Jenkinsfile
  7. 4
      SConstruct
  8. 2
      cereal
  9. 12
      common/params.cc
  10. 4
      common/params.h
  11. 6
      common/params_pyx.pyx
  12. 17
      common/tests/test_util.cc
  13. 16
      common/util.cc
  14. 1
      common/util.h
  15. 8
      docs/CARS.md
  16. 2
      laika_repo
  17. 2
      launch_env.sh
  18. 2
      opendbc
  19. 2
      panda
  20. 3
      release/files_common
  21. 1
      scripts/launch_corolla.sh
  22. 1
      selfdrive/boardd/boardd.cc
  23. 7
      selfdrive/car/car_helpers.py
  24. 13
      selfdrive/car/gm/carstate.py
  25. 16
      selfdrive/car/gm/values.py
  26. 19
      selfdrive/car/hyundai/carstate.py
  27. 12
      selfdrive/car/hyundai/hyundaican.py
  28. 4
      selfdrive/car/hyundai/interface.py
  29. 20
      selfdrive/car/hyundai/values.py
  30. 5
      selfdrive/car/isotp_parallel_query.py
  31. 16
      selfdrive/car/mock/interface.py
  32. 5
      selfdrive/car/subaru/carstate.py
  33. 4
      selfdrive/car/tests/routes.py
  34. 10
      selfdrive/car/tests/test_docs.py
  35. 4
      selfdrive/car/torque_data/substitute.yaml
  36. 10
      selfdrive/car/toyota/values.py
  37. 4
      selfdrive/car/vin.py
  38. 2
      selfdrive/car/volkswagen/carstate.py
  39. 8
      selfdrive/controls/controlsd.py
  40. 6
      selfdrive/controls/lib/drive_helpers.py
  41. 65
      selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
  42. 55
      selfdrive/controls/lib/lateral_planner.py
  43. 3
      selfdrive/controls/lib/longcontrol.py
  44. 27
      selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
  45. 10
      selfdrive/controls/lib/longitudinal_planner.py
  46. 5
      selfdrive/controls/tests/test_lateral_mpc.py
  47. 6
      selfdrive/debug/vw_mqb_config.py
  48. 54
      selfdrive/locationd/laikad.py
  49. 6
      selfdrive/locationd/locationd.cc
  50. 1
      selfdrive/locationd/test/test_locationd.py
  51. 5
      selfdrive/locationd/torqued.py
  52. 2
      selfdrive/loggerd/tests/test_loggerd.py
  53. 5
      selfdrive/manager/process_config.py
  54. 4
      selfdrive/modeld/SConscript
  55. 16
      selfdrive/modeld/models/driving.cc
  56. 15
      selfdrive/modeld/models/driving.h
  57. 4
      selfdrive/modeld/models/supercombo.onnx
  58. 13
      selfdrive/modeld/runners/onnx_runner.py
  59. 2
      selfdrive/modeld/thneed/thneed_common.cc
  60. 12
      selfdrive/sensord/pigeond.py
  61. 15
      selfdrive/sensord/rawgps/modemdiag.py
  62. 189
      selfdrive/sensord/rawgps/rawgpsd.py
  63. 41
      selfdrive/sensord/rawgps/structs.py
  64. 49
      selfdrive/sensord/rawgps/test_rawgps.py
  65. 2
      selfdrive/test/process_replay/compare_logs.py
  66. 4
      selfdrive/test/process_replay/model_replay.py
  67. 2
      selfdrive/test/process_replay/model_replay_ref_commit
  68. 1
      selfdrive/test/process_replay/process_replay.py
  69. 2
      selfdrive/test/process_replay/ref_commit
  70. 4
      selfdrive/test/process_replay/regen.py
  71. 1
      selfdrive/test/profiling/profiler.py
  72. 30
      selfdrive/thermald/thermald.py
  73. 43
      selfdrive/tombstoned.py
  74. 1
      selfdrive/ui/SConscript
  75. 5
      selfdrive/ui/qt/maps/map_settings.cc
  76. 15
      selfdrive/ui/qt/offroad/networking.cc
  77. 3
      selfdrive/ui/qt/offroad/networking.h
  78. 20
      selfdrive/ui/qt/offroad/wifiManager.cc
  79. 6
      selfdrive/ui/qt/widgets/prime.cc
  80. 7
      selfdrive/ui/ui.cc
  81. 7
      selfdrive/ui/ui.h
  82. 3
      selfdrive/updated.py
  83. 6
      system/hardware/tici/agnos.json
  84. 2
      system/hardware/tici/hardware.py
  85. 2
      system/hardware/tici/test_power_draw.py
  86. 2
      system/proclogd/proclog.cc
  87. 4
      system/proclogd/proclog.h
  88. 2
      tinygrad_repo
  89. 4
      tools/cabana/.gitignore
  90. 9
      tools/cabana/README.md
  91. 20
      tools/cabana/SConscript
  92. 4
      tools/cabana/cabana
  93. 32
      tools/cabana/cabana.cc
  94. 271
      tools/cabana/chartswidget.cc
  95. 77
      tools/cabana/chartswidget.h
  96. 264
      tools/cabana/detailwidget.cc
  97. 70
      tools/cabana/detailwidget.h
  98. 58
      tools/cabana/mainwin.cc
  99. 25
      tools/cabana/mainwin.h
  100. 93
      tools/cabana/messageswidget.cc
  101. Some files were not shown because too many files have changed in this diff Show More

@ -8,6 +8,7 @@ body:
value: > value: >
Before creating a **bug report**, please check the following: Before creating a **bug report**, please check the following:
* If the issue likely only affects your car model or make, go back and open a **car bug report** instead. * If the issue likely only affects your car model or make, go back and open a **car bug report** instead.
* If the issue is related to the driving or driver monitoring models, you should open a [discussion](https://github.com/commaai/openpilot/discussions/categories/model-feedback) instead.
* Ensure you're running the latest openpilot release. * Ensure you're running the latest openpilot release.
* Ensure you're using officially supported hardware. Issues running on PCs have a different issue template. * Ensure you're using officially supported hardware. Issues running on PCs have a different issue template.
* Ensure there isn't an existing issue for your bug. If there is, leave a comment on the existing issue. * Ensure there isn't an existing issue for your bug. If there is, leave a comment on the existing issue.

@ -1,8 +1,11 @@
blank_issues_enabled: false blank_issues_enabled: false
contact_links: contact_links:
- name: Report model bugs
url: https://github.com/commaai/openpilot/discussions/categories/model-feedback
about: Provide feedback for the driving or driver monitoring models
- name: Discussions - name: Discussions
url: https://github.com/commaai/openpilot/discussions url: https://github.com/commaai/openpilot/discussions
about: For questions and discussion about openpilot about: For questions and general discussion about openpilot
- name: Community Wiki - name: Community Wiki
url: https://github.com/commaai/openpilot/wiki url: https://github.com/commaai/openpilot/wiki
about: Check out our community wiki about: Check out our community wiki

@ -62,7 +62,7 @@ jobs:
cp .pylintrc $STRIPPED_DIR cp .pylintrc $STRIPPED_DIR
cp mypy.ini $STRIPPED_DIR cp mypy.ini $STRIPPED_DIR
cd $STRIPPED_DIR cd $STRIPPED_DIR
${{ env.RUN }} "pre-commit run --all" ${{ env.RUN }} "SKIP=test_translations pre-commit run --all"
build_all: build_all:
name: build all name: build all

2
.gitmodules vendored

@ -18,4 +18,4 @@
url = ../../commaai/body.git url = ../../commaai/body.git
[submodule "tinygrad"] [submodule "tinygrad"]
path = tinygrad_repo path = tinygrad_repo
url = https://github.com/geohot/tinygrad.git url = ../../commaai/tinygrad.git

@ -70,3 +70,10 @@ repos:
- --quiet - --quiet
- --force - --force
- -j8 - -j8
- repo: local
hooks:
- id: test_translations
name: test translations
entry: selfdrive/ui/tests/test_translations.py
language: script
pass_filenames: false

2
Jenkinsfile vendored

@ -127,7 +127,7 @@ pipeline {
steps { steps {
phone_steps("tici2", [ phone_steps("tici2", [
["build", "cd selfdrive/manager && ./build.py"], ["build", "cd selfdrive/manager && ./build.py"],
//["test power draw", "python system/hardware/tici/test_power_draw.py"], ["test power draw", "python system/hardware/tici/test_power_draw.py"],
["test boardd loopback", "python selfdrive/boardd/tests/test_boardd_loopback.py"], ["test boardd loopback", "python selfdrive/boardd/tests/test_boardd_loopback.py"],
["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"], ["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"],
["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python selfdrive/loggerd/tests/test_encoder.py"], ["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python selfdrive/loggerd/tests/test_encoder.py"],

@ -433,6 +433,10 @@ SConscript(['selfdrive/navd/SConscript'])
SConscript(['tools/replay/SConscript']) SConscript(['tools/replay/SConscript'])
opendbc = abspath([File('opendbc/can/libdbc.so')])
Export('opendbc')
SConscript(['tools/cabana/SConscript'])
if GetOption('test'): if GetOption('test'):
SConscript('panda/tests/safety/SConscript') SConscript('panda/tests/safety/SConscript')

@ -1 +1 @@
Subproject commit d4cf8728e2fa2d87d90098efa7ddeaf8f98a03db Subproject commit b29717c4c328d5cf34d46f682f25267150f82637

@ -166,6 +166,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"TermsVersion", PERSISTENT}, {"TermsVersion", PERSISTENT},
{"Timezone", PERSISTENT}, {"Timezone", PERSISTENT},
{"TrainingVersion", PERSISTENT}, {"TrainingVersion", PERSISTENT},
{"UbloxAvailable", PERSISTENT},
{"UpdateAvailable", CLEAR_ON_MANAGER_START}, {"UpdateAvailable", CLEAR_ON_MANAGER_START},
{"UpdateFailedCount", CLEAR_ON_MANAGER_START}, {"UpdateFailedCount", CLEAR_ON_MANAGER_START},
{"UpdaterState", CLEAR_ON_MANAGER_START}, {"UpdaterState", CLEAR_ON_MANAGER_START},
@ -298,10 +299,13 @@ std::map<std::string, std::string> Params::readAll() {
void Params::clearAll(ParamKeyType key_type) { void Params::clearAll(ParamKeyType key_type) {
FileLock file_lock(params_path + "/.lock"); FileLock file_lock(params_path + "/.lock");
std::string path; if (key_type == ALL) {
for (auto &[key, type] : keys) { util::remove_files_in_dir(getParamPath());
if (type & key_type) { } else {
unlink(getParamPath(key).c_str()); for (auto &[key, type] : keys) {
if (type & key_type) {
unlink(getParamPath(key).c_str());
}
} }
} }

@ -29,8 +29,8 @@ public:
// helpers for reading values // helpers for reading values
std::string get(const std::string &key, bool block = false); std::string get(const std::string &key, bool block = false);
inline bool getBool(const std::string &key) { inline bool getBool(const std::string &key, bool block = false) {
return get(key) == "1"; return get(key, block) == "1";
} }
std::map<std::string, std::string> readAll(); std::map<std::string, std::string> readAll();

@ -16,7 +16,7 @@ cdef extern from "common/params.h":
cdef cppclass c_Params "Params": cdef cppclass c_Params "Params":
c_Params(string) nogil c_Params(string) nogil
string get(string, bool) nogil string get(string, bool) nogil
bool getBool(string) nogil bool getBool(string, bool) nogil
int remove(string) nogil int remove(string) nogil
int put(string, string) nogil int put(string, string) nogil
int putBool(string, bool) nogil int putBool(string, bool) nogil
@ -68,11 +68,11 @@ cdef class Params:
return val if encoding is None else val.decode(encoding) return val if encoding is None else val.decode(encoding)
def get_bool(self, key): def get_bool(self, key, bool block=False):
cdef string k = self.check_key(key) cdef string k = self.check_key(key)
cdef bool r cdef bool r
with nogil: with nogil:
r = self.p.getBool(k) r = self.p.getBool(k, block)
return r return r
def put(self, key, dat): def put(self, key, dat):

@ -143,3 +143,20 @@ TEST_CASE("util::create_directories") {
REQUIRE(util::create_directories("", 0755) == false); REQUIRE(util::create_directories("", 0755) == false);
} }
} }
TEST_CASE("util::remove_files_in_dir") {
std::string tmp_dir = "/tmp/test_remove_all_in_dir";
system("rm /tmp/test_remove_all_in_dir -rf");
REQUIRE(util::create_directories(tmp_dir, 0755));
const int tmp_file_cnt = 10;
for (int i = 0; i < tmp_file_cnt; ++i) {
std::string tmp_file = tmp_dir + "/test_XXXXXX";
int fd = mkstemp((char*)tmp_file.c_str());
close(fd);
REQUIRE(util::file_exists(tmp_file.c_str()));
}
REQUIRE(util::read_files_in_dir(tmp_dir).size() == tmp_file_cnt);
util::remove_files_in_dir(tmp_dir);
REQUIRE(util::read_files_in_dir(tmp_dir).empty());
}

@ -97,6 +97,22 @@ std::map<std::string, std::string> read_files_in_dir(const std::string &path) {
return ret; return ret;
} }
void remove_files_in_dir(const std::string &path) {
DIR *d = opendir(path.c_str());
if (!d) return;
std::string fn;
struct dirent *de = NULL;
while ((de = readdir(d))) {
if (de->d_type != DT_DIR) {
fn = path + "/" + de->d_name;
unlink(fn.c_str());
}
}
closedir(d);
}
int write_file(const char* path, const void* data, size_t size, int flags, mode_t mode) { int write_file(const char* path, const void* data, size_t size, int flags, mode_t mode) {
int fd = HANDLE_EINTR(open(path, flags, mode)); int fd = HANDLE_EINTR(open(path, flags, mode));
if (fd == -1) { if (fd == -1) {

@ -80,6 +80,7 @@ std::string dir_name(std::string const& path);
// **** file fhelpers ***** // **** file fhelpers *****
std::string read_file(const std::string& fn); std::string read_file(const std::string& fn);
std::map<std::string, std::string> read_files_in_dir(const std::string& path); std::map<std::string, std::string> read_files_in_dir(const std::string& path);
void remove_files_in_dir(const std::string& path);
int write_file(const char* path, const void* data, size_t size, int flags = O_WRONLY, mode_t mode = 0664); int write_file(const char* path, const void* data, size_t size, int flags = O_WRONLY, mode_t mode = 0664);
FILE* safe_fopen(const char* filename, const char* mode); FILE* safe_fopen(const char* filename, const char* mode);

@ -4,7 +4,7 @@
A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system. A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system.
# 204 Supported Cars # 206 Supported Cars
|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness| |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness|
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|
@ -110,9 +110,11 @@ A supported vehicle is one that just works when you install a comma three. All s
|Lexus|NX Hybrid 2018-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| |Lexus|NX Hybrid 2018-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|
|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|
|Lexus|RC 2017-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| |Lexus|RC 2017-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|
|Lexus|RX 2016-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| |Lexus|RX 2016|Lexus Safety System+|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|
|Lexus|RX 2017-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|
|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|
|Lexus|RX Hybrid 2016-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| |Lexus|RX Hybrid 2016|Lexus Safety System+|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|
|Lexus|RX Hybrid 2017-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|
|Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|
|Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|
|Mazda|CX-5 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda| |Mazda|CX-5 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda|

@ -1 +1 @@
Subproject commit c8bc1fa01be9f22592efb991ee52d3d965d21968 Subproject commit e1049cde0a68f7d4a70b1ebd76befdc0e163ad55

@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1
export VECLIB_MAXIMUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1
if [ -z "$AGNOS_VERSION" ]; then if [ -z "$AGNOS_VERSION" ]; then
export AGNOS_VERSION="6" export AGNOS_VERSION="6.1"
fi fi
if [ -z "$PASSIVE" ]; then if [ -z "$PASSIVE" ]; then

@ -1 +1 @@
Subproject commit eaac172af9cb342204e69ec52339cdf3c6a8ac4e Subproject commit c35e8139bf9e9d87b9efb6764ab7e65983e8d33e

@ -1 +1 @@
Subproject commit 1910db8d4c3f932fe85b186fba1d24795cb2b742 Subproject commit 1303af2db29a72eee180b10c6097fa5b19c29207

@ -305,6 +305,8 @@ selfdrive/ui/soundd/soundd
selfdrive/ui/soundd/.gitignore selfdrive/ui/soundd/.gitignore
selfdrive/ui/translations/*.ts selfdrive/ui/translations/*.ts
selfdrive/ui/translations/languages.json selfdrive/ui/translations/languages.json
selfdrive/ui/update_translations.py
selfdrive/ui/tests/test_translations.py
selfdrive/ui/qt/*.cc selfdrive/ui/qt/*.cc
selfdrive/ui/qt/*.h selfdrive/ui/qt/*.h
@ -572,3 +574,4 @@ tinygrad_repo/tinygrad/ops.py
tinygrad_repo/tinygrad/shapetracker.py tinygrad_repo/tinygrad/shapetracker.py
tinygrad_repo/tinygrad/tensor.py tinygrad_repo/tinygrad/tensor.py
tinygrad_repo/tinygrad/nn/__init__.py tinygrad_repo/tinygrad/nn/__init__.py
tinygrad_repo/tinygrad/nn/optim.py

@ -3,4 +3,5 @@
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
export FINGERPRINT="TOYOTA COROLLA TSS2 2019" export FINGERPRINT="TOYOTA COROLLA TSS2 2019"
export SKIP_FW_QUERY="1"
$DIR/../launch_openpilot.sh $DIR/../launch_openpilot.sh

@ -381,6 +381,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt)); ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt));
ps.setInterruptLoad(health.interrupt_load); ps.setInterruptLoad(health.interrupt_load);
ps.setFanPower(health.fan_power); ps.setFanPower(health.fan_power);
ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid));
std::array<cereal::PandaState::PandaCanState::Builder, PANDA_CAN_CNT> cs = {ps.initCanState0(), ps.initCanState1(), ps.initCanState2()}; std::array<cereal::PandaState::PandaCanState::Builder, PANDA_CAN_CNT> cs = {ps.initCanState0(), ps.initCanState1(), ps.initCanState2()};

@ -81,7 +81,7 @@ def fingerprint(logcan, sendcan):
skip_fw_query = os.environ.get('SKIP_FW_QUERY', False) skip_fw_query = os.environ.get('SKIP_FW_QUERY', False)
ecu_rx_addrs = set() ecu_rx_addrs = set()
if not fixed_fingerprint and not skip_fw_query: if not skip_fw_query:
# Vin query only reliably works through OBDII # Vin query only reliably works through OBDII
bus = 1 bus = 1
@ -95,16 +95,19 @@ def fingerprint(logcan, sendcan):
cloudlog.warning("Using cached CarParams") cloudlog.warning("Using cached CarParams")
vin, vin_rx_addr = cached_params.carVin, 0 vin, vin_rx_addr = cached_params.carVin, 0
car_fw = list(cached_params.carFw) car_fw = list(cached_params.carFw)
cached = True
else: else:
cloudlog.warning("Getting VIN & FW versions") cloudlog.warning("Getting VIN & FW versions")
vin_rx_addr, vin = get_vin(logcan, sendcan, bus) vin_rx_addr, vin = get_vin(logcan, sendcan, bus)
ecu_rx_addrs = get_present_ecus(logcan, sendcan) ecu_rx_addrs = get_present_ecus(logcan, sendcan)
car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs) car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs)
cached = False
exact_fw_match, fw_candidates = match_fw_to_car(car_fw) exact_fw_match, fw_candidates = match_fw_to_car(car_fw)
else: else:
vin, vin_rx_addr = VIN_UNKNOWN, 0 vin, vin_rx_addr = VIN_UNKNOWN, 0
exact_fw_match, fw_candidates, car_fw = True, set(), [] exact_fw_match, fw_candidates, car_fw = True, set(), []
cached = False
if not is_valid_vin(vin): if not is_valid_vin(vin):
cloudlog.event("Malformed VIN", vin=vin, error=True) cloudlog.event("Malformed VIN", vin=vin, error=True)
@ -165,7 +168,7 @@ def fingerprint(logcan, sendcan):
car_fingerprint = fixed_fingerprint car_fingerprint = fixed_fingerprint
source = car.CarParams.FingerprintSource.fixed source = car.CarParams.FingerprintSource.fixed
cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, cached=cached,
fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, error=True) fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, error=True)
return car_fingerprint, finger, vin, car_fw, source, exact_match return car_fingerprint, finger, vin, car_fw, source, exact_match

@ -40,9 +40,12 @@ class CarState(CarStateBase):
else: else:
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None)) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None))
# Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed. # Some Volt 2016-17 have loose brake pedal push rod retainers which causes the ECM to believe
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0 # that the brake is being intermittently pressed without user interaction.
ret.brakePressed = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] >= 10 # To avoid a cruise fault we need to match the ECM's brake pressed signal and threshold
# https://static.nhtsa.gov/odi/tsbs/2017/MC-10137629-9999.pdf
ret.brake = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"] / 0xd0
ret.brakePressed = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"] >= 8
# Regen braking is braking # Regen braking is braking
if self.CP.transmissionType == TransmissionType.direct: if self.CP.transmissionType == TransmissionType.direct:
@ -100,7 +103,7 @@ class CarState(CarStateBase):
def get_can_parser(CP): def get_can_parser(CP):
signals = [ signals = [
# sig_name, sig_address # sig_name, sig_address
("BrakePedalPosition", "EBCMBrakePedalPosition"), ("BrakePedalPos", "ECMAcceleratorPos"),
("FrontLeftDoor", "BCMDoorBeltStatus"), ("FrontLeftDoor", "BCMDoorBeltStatus"),
("FrontRightDoor", "BCMDoorBeltStatus"), ("FrontRightDoor", "BCMDoorBeltStatus"),
("RearLeftDoor", "BCMDoorBeltStatus"), ("RearLeftDoor", "BCMDoorBeltStatus"),
@ -141,7 +144,7 @@ class CarState(CarStateBase):
("ASCMSteeringButton", 33), ("ASCMSteeringButton", 33),
("ECMEngineStatus", 100), ("ECMEngineStatus", 100),
("PSCMSteeringAngle", 100), ("PSCMSteeringAngle", 100),
("EBCMBrakePedalPosition", 100), ("ECMAcceleratorPos", 80),
] ]
if CP.transmissionType == TransmissionType.direct: if CP.transmissionType == TransmissionType.direct:

@ -23,11 +23,12 @@ class CarControllerParams:
ADAS_KEEPALIVE_STEP = 100 ADAS_KEEPALIVE_STEP = 100
CAMERA_KEEPALIVE_STEP = 100 CAMERA_KEEPALIVE_STEP = 100
# Volt gasbrake lookups # Volt gas/brake lookups
# TODO: These values should be confirmed on non-Volt vehicles # TODO: These values should be confirmed on non-Volt vehicles.
# MAX_GAS should achieve 2 m/s^2 and MAX_BRAKE with regen should achieve -4.0 m/s^2
MAX_GAS = 3072 # Safety limit, not ACC max. Stock ACC >4096 from standstill. MAX_GAS = 3072 # Safety limit, not ACC max. Stock ACC >4096 from standstill.
ZERO_GAS = 2048 # Coasting ZERO_GAS = 2048 # Coasting
MAX_BRAKE = 350 # ~ -3.5 m/s^2 with regen MAX_BRAKE = 400 # ~ -4.0 m/s^2 with regen
MAX_ACC_REGEN = 1404 # Max ACC regen is slightly less than max paddle regen MAX_ACC_REGEN = 1404 # Max ACC regen is slightly less than max paddle regen
# Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we # Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
@ -38,15 +39,14 @@ class CarControllerParams:
ACCEL_MAX = 2. # m/s^2 ACCEL_MAX = 2. # m/s^2
ACCEL_MIN = -4. # m/s^2 ACCEL_MIN = -4. # m/s^2
EV_GAS_LOOKUP_BP = [-1., 0., ACCEL_MAX]
EV_BRAKE_LOOKUP_BP = [ACCEL_MIN, -1.]
# ICE has much less engine braking force compared to regen in EVs, # ICE has much less engine braking force compared to regen in EVs,
# lower threshold removes some braking deadzone # lower threshold removes some braking deadzone
GAS_LOOKUP_BP = [-0.1, 0., ACCEL_MAX] GAS_LOOKUP_BP = [-0.1, 0., ACCEL_MAX]
BRAKE_LOOKUP_BP = [ACCEL_MIN, -0.1] EV_GAS_LOOKUP_BP = [-1., 0., ACCEL_MAX]
GAS_LOOKUP_V = [MAX_ACC_REGEN, ZERO_GAS, MAX_GAS] GAS_LOOKUP_V = [MAX_ACC_REGEN, ZERO_GAS, MAX_GAS]
BRAKE_LOOKUP_BP = [ACCEL_MIN, -0.1]
EV_BRAKE_LOOKUP_BP = [ACCEL_MIN, -1.]
BRAKE_LOOKUP_V = [MAX_BRAKE, 0.] BRAKE_LOOKUP_V = [MAX_BRAKE, 0.]

@ -31,7 +31,6 @@ class CarState(CarStateBase):
self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"] self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
self.brake_error = False self.brake_error = False
self.park_brake = False
self.buttons_counter = 0 self.buttons_counter = 0
# On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz # On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz
@ -127,12 +126,12 @@ class CarState(CarStateBase):
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear)) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
if not self.CP.openpilotLongitudinalControl: if not self.CP.openpilotLongitudinalControl:
if self.CP.carFingerprint in FEATURES["use_fca"]: aeb_src = "FCA11" if self.CP.carFingerprint in FEATURES["use_fca"] else "SCC12"
ret.stockAeb = cp_cruise.vl["FCA11"]["FCA_CmdAct"] != 0 aeb_sig = "FCA_CmdAct" if self.CP.carFingerprint in FEATURES["use_fca"] else "AEB_CmdAct"
ret.stockFcw = cp_cruise.vl["FCA11"]["CF_VSM_Warn"] == 2 aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0
else: aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0
ret.stockAeb = cp_cruise.vl["SCC12"]["AEB_CmdAct"] != 0 ret.stockFcw = aeb_warning and not aeb_braking
ret.stockFcw = cp_cruise.vl["SCC12"]["CF_VSM_Warn"] == 2 ret.stockAeb = aeb_warning and aeb_braking
if self.CP.enableBsm: if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0 ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
@ -181,6 +180,7 @@ class CarState(CarStateBase):
ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"] ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"]
ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"] ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"]
ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD
ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"], ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"],
cp.vl["BLINKERS"]["RIGHT_LAMP"]) cp.vl["BLINKERS"]["RIGHT_LAMP"])
@ -295,12 +295,14 @@ class CarState(CarStateBase):
signals += [ signals += [
("FCA_CmdAct", "FCA11"), ("FCA_CmdAct", "FCA11"),
("CF_VSM_Warn", "FCA11"), ("CF_VSM_Warn", "FCA11"),
("CF_VSM_DecCmdAct", "FCA11"),
] ]
checks.append(("FCA11", 50)) checks.append(("FCA11", 50))
else: else:
signals += [ signals += [
("AEB_CmdAct", "SCC12"), ("AEB_CmdAct", "SCC12"),
("CF_VSM_Warn", "SCC12"), ("CF_VSM_Warn", "SCC12"),
("CF_VSM_DecCmdAct", "SCC12"),
] ]
if CP.enableBsm: if CP.enableBsm:
@ -384,12 +386,14 @@ class CarState(CarStateBase):
signals += [ signals += [
("FCA_CmdAct", "FCA11"), ("FCA_CmdAct", "FCA11"),
("CF_VSM_Warn", "FCA11"), ("CF_VSM_Warn", "FCA11"),
("CF_VSM_DecCmdAct", "FCA11"),
] ]
checks.append(("FCA11", 50)) checks.append(("FCA11", 50))
else: else:
signals += [ signals += [
("AEB_CmdAct", "SCC12"), ("AEB_CmdAct", "SCC12"),
("CF_VSM_Warn", "SCC12"), ("CF_VSM_Warn", "SCC12"),
("CF_VSM_DecCmdAct", "SCC12"),
] ]
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
@ -411,6 +415,7 @@ class CarState(CarStateBase):
("STEERING_ANGLE", "STEERING_SENSORS"), ("STEERING_ANGLE", "STEERING_SENSORS"),
("STEERING_COL_TORQUE", "MDPS"), ("STEERING_COL_TORQUE", "MDPS"),
("STEERING_OUT_TORQUE", "MDPS"), ("STEERING_OUT_TORQUE", "MDPS"),
("LKA_FAULT", "MDPS"),
("CRUISE_ACTIVE", "SCC1"), ("CRUISE_ACTIVE", "SCC1"),
("COUNTER", cruise_btn_msg), ("COUNTER", cruise_btn_msg),

@ -38,7 +38,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0 values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0
# Likely cars lacking the ability to show individual lane lines in the dash # Likely cars lacking the ability to show individual lane lines in the dash
elif car_fingerprint in (CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_2019): elif car_fingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL):
# SysWarning 4 = keep hands on wheel + beep # SysWarning 4 = keep hands on wheel + beep
values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0 values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0
@ -132,18 +132,16 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s
} }
commands.append(packer.make_can_msg("SCC14", 0, scc14_values)) commands.append(packer.make_can_msg("SCC14", 0, scc14_values))
# note that some vehicles most likely have an alternate checksum/counter definition
# https://github.com/commaai/opendbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602
fca11_values = { fca11_values = {
# seems to count 2,1,0,3,2,1,0,3,2,1,0,3,2,1,0,repeat... "CR_FCA_Alive": idx % 0xF,
# (where first value is aligned to Supplemental_Counter == 0)
# test: [(idx % 0xF, -((idx % 0xF) + 2) % 4) for idx in range(0x14)]
"CR_FCA_Alive": ((-((idx % 0xF) + 2) % 4) << 2) + 1,
"Supplemental_Counter": idx % 0xF,
"PAINT1_Status": 1, "PAINT1_Status": 1,
"FCA_DrvSetStatus": 1, "FCA_DrvSetStatus": 1,
"FCA_Status": 1, # AEB disabled "FCA_Status": 1, # AEB disabled
} }
fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2] fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2]
fca11_values["CR_FCA_ChkSum"] = 0x10 - sum(sum(divmod(i, 16)) for i in fca11_dat) % 0x10 fca11_values["CR_FCA_ChkSum"] = hyundai_checksum(fca11_dat[:7])
commands.append(packer.make_can_msg("FCA11", 0, fca11_values)) commands.append(packer.make_can_msg("FCA11", 0, fca11_values))
return commands return commands

@ -202,12 +202,12 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.indi.timeConstantV = [1.4] ret.lateralTuning.indi.timeConstantV = [1.4]
ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] ret.lateralTuning.indi.actuatorEffectivenessBP = [0.]
ret.lateralTuning.indi.actuatorEffectivenessV = [1.8] ret.lateralTuning.indi.actuatorEffectivenessV = [1.8]
elif candidate in (CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_2019, CAR.KIA_OPTIMA_H): elif candidate in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H):
ret.mass = 3558. * CV.LB_TO_KG ret.mass = 3558. * CV.LB_TO_KG
ret.wheelbase = 2.80 ret.wheelbase = 2.80
ret.steerRatio = 13.75 ret.steerRatio = 13.75
tire_stiffness_factor = 0.5 tire_stiffness_factor = 0.5
if candidate == CAR.KIA_OPTIMA: if candidate == CAR.KIA_OPTIMA_G4:
ret.minSteerSpeed = 32 * CV.MPH_TO_MS ret.minSteerSpeed = 32 * CV.MPH_TO_MS
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.KIA_STINGER: elif candidate == CAR.KIA_STINGER:

@ -85,8 +85,8 @@ class CAR:
KIA_NIRO_EV = "KIA NIRO EV 2020" KIA_NIRO_EV = "KIA NIRO EV 2020"
KIA_NIRO_PHEV = "KIA NIRO HYBRID 2019" KIA_NIRO_PHEV = "KIA NIRO HYBRID 2019"
KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021" KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021"
KIA_OPTIMA = "KIA OPTIMA 2016" KIA_OPTIMA_G4 = "KIA OPTIMA 4TH GEN"
KIA_OPTIMA_2019 = "KIA OPTIMA 2019" KIA_OPTIMA_G4_FL = "KIA OPTIMA 4TH GEN FACELIFT"
KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019" KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019"
KIA_SELTOS = "KIA SELTOS 2021" KIA_SELTOS = "KIA SELTOS 2021"
KIA_SORENTO = "KIA SORENTO GT LINE 2018" KIA_SORENTO = "KIA SORENTO GT LINE 2018"
@ -155,8 +155,8 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
HyundaiCarInfo("Kia Niro Hybrid 2021", harness=Harness.hyundai_f), # TODO: could be hyundai_d, verify HyundaiCarInfo("Kia Niro Hybrid 2021", harness=Harness.hyundai_f), # TODO: could be hyundai_d, verify
HyundaiCarInfo("Kia Niro Hybrid 2022", harness=Harness.hyundai_h), HyundaiCarInfo("Kia Niro Hybrid 2022", harness=Harness.hyundai_h),
], ],
CAR.KIA_OPTIMA: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", harness=Harness.hyundai_b), # TODO: may support 2016, 2018 CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", harness=Harness.hyundai_b), # TODO: may support 2016, 2018
CAR.KIA_OPTIMA_2019: HyundaiCarInfo("Kia Optima 2019-20", harness=Harness.hyundai_g), CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", harness=Harness.hyundai_g),
CAR.KIA_OPTIMA_H: [ CAR.KIA_OPTIMA_H: [
HyundaiCarInfo("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control"), # TODO: may support adjacent years HyundaiCarInfo("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control"), # TODO: may support adjacent years
HyundaiCarInfo("Kia Optima Hybrid 2019"), HyundaiCarInfo("Kia Optima Hybrid 2019"),
@ -1127,7 +1127,7 @@ FW_VERSIONS = {
b'\xf1\x87954A22D200\xf1\x81T01950A1 \xf1\000T0190XBL T01950A1 DSP2T16X4X950NS8\r\xfe\x9c\x8b', b'\xf1\x87954A22D200\xf1\x81T01950A1 \xf1\000T0190XBL T01950A1 DSP2T16X4X950NS8\r\xfe\x9c\x8b',
], ],
}, },
CAR.KIA_OPTIMA: { CAR.KIA_OPTIMA_G4: {
(Ecu.fwdRadar, 0x7d0, None): [ (Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00JF__ SCC F-CUP 1.00 1.00 96400-D4100 ', b'\xf1\x00JF__ SCC F-CUP 1.00 1.00 96400-D4100 ',
], ],
@ -1141,7 +1141,7 @@ FW_VERSIONS = {
b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6J0051\x00\x00\xf1\x006T6J0_C2\x00\x006T6J0051\x00\x00TJF0T20NSB\x00\x00\x00\x00', b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6J0051\x00\x00\xf1\x006T6J0_C2\x00\x006T6J0051\x00\x00TJF0T20NSB\x00\x00\x00\x00',
], ],
}, },
CAR.KIA_OPTIMA_2019: { CAR.KIA_OPTIMA_G4_FL: {
(Ecu.fwdRadar, 0x7d0, None): [ (Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00JF__ SCC F-CUP 1.00 1.00 96400-D4110 ', b'\xf1\x00JF__ SCC F-CUP 1.00 1.00 96400-D4110 ',
], ],
@ -1375,7 +1375,7 @@ CHECKSUM = {
FEATURES = { FEATURES = {
# which message has the gear # which message has the gear
"use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_I30_3G, CAR.KONA}, "use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_I30_3G, CAR.KONA},
"use_tcu_gears": {CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_2019, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON}, "use_tcu_gears": {CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON},
"use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KONA_EV_2022}, "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KONA_EV_2022},
# these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12 # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
@ -1391,7 +1391,7 @@ HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_N
EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KONA_EV_2022} EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KONA_EV_2022}
# these cars require a special panda safety mode due to missing counters and checksums in the messages # these cars require a special panda safety mode due to missing counters and checksums in the messages
LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_2019, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022} LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022}
# If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points. # If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points.
# If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py # If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py
@ -1416,8 +1416,8 @@ DBC = {
CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.KIA_NIRO_PHEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), CAR.KIA_NIRO_PHEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None), CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None), CAR.KIA_OPTIMA_G4: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_2019: dbc_dict('hyundai_kia_generic', None), CAR.KIA_OPTIMA_G4_FL: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None), CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None), CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format

@ -68,7 +68,10 @@ class IsoTpParallelQuery:
self.bus, sub_addr=sub_addr, debug=self.debug) self.bus, sub_addr=sub_addr, debug=self.debug)
max_len = 8 if sub_addr is None else 7 max_len = 8 if sub_addr is None else 7
return IsoTpMessage(can_client, timeout=0, max_len=max_len, debug=self.debug) # uses iso-tp frame separation time of 10 ms
# TODO: use single_frame_mode so ECUs can send as fast as they want,
# as well as reduces chances we process messages from previous queries
return IsoTpMessage(can_client, timeout=0, separation_time=0.01, debug=self.debug, max_len=max_len)
def get_data(self, timeout, total_timeout=60.): def get_data(self, timeout, total_timeout=60.):
self._drain_rx() self._drain_rx()

@ -20,8 +20,7 @@ class CarInterface(CarInterfaceBase):
cloudlog.debug("Using Mock Car Interface") cloudlog.debug("Using Mock Car Interface")
self.gyro = messaging.sub_sock('gyroscope') self.sm = messaging.SubMaster(['gyroscope', 'gpsLocation', 'gpsLocationExternal'])
self.gps = messaging.sub_sock('gpsLocationExternal')
self.speed = 0. self.speed = 0.
self.prev_speed = 0. self.prev_speed = 0.
@ -45,15 +44,16 @@ class CarInterface(CarInterfaceBase):
# returns a car.CarState # returns a car.CarState
def _update(self, c): def _update(self, c):
self.sm.update(0)
# get basic data from phone and gps since CAN isn't connected # get basic data from phone and gps since CAN isn't connected
gyro_sensor = messaging.recv_sock(self.gyro) if self.sm.updated['gyroscope']:
if gyro_sensor is not None: self.yaw_rate_meas = -self.sm['gyroscope'].gyroUncalibrated.v[0]
self.yaw_rate_meas = -gyro_sensor.gyroscope.gyroUncalibrated.v[0]
gps = messaging.recv_sock(self.gps) gps_sock = 'gpsLocationExternal' if self.sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation'
if gps is not None: if self.sm.updated[gps_sock]:
self.prev_speed = self.speed self.prev_speed = self.speed
self.speed = gps.gpsLocationExternal.speed self.speed = self.sm[gps_sock].speed
# create message # create message
ret = car.CarState.new_message() ret = car.CarState.new_message()

@ -58,9 +58,6 @@ class CarState(CarStateBase):
ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0 ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0
ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS
if self.car_fingerprint not in PREGLOBAL_CARS:
ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3
if (self.car_fingerprint in PREGLOBAL_CARS and cp.vl["Dash_State2"]["UNITS"] == 1) or \ if (self.car_fingerprint in PREGLOBAL_CARS and cp.vl["Dash_State2"]["UNITS"] == 1) or \
(self.car_fingerprint not in PREGLOBAL_CARS and cp.vl["Dashlights"]["UNITS"] == 1): (self.car_fingerprint not in PREGLOBAL_CARS and cp.vl["Dashlights"]["UNITS"] == 1):
ret.cruiseState.speed *= CV.MPH_TO_KPH ret.cruiseState.speed *= CV.MPH_TO_KPH
@ -78,6 +75,8 @@ class CarState(CarStateBase):
else: else:
ret.steerFaultTemporary = cp.vl["Steering_Torque"]["Steer_Warning"] == 1 ret.steerFaultTemporary = cp.vl["Steering_Torque"]["Steer_Warning"] == 1
ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1 ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1
ret.cruiseState.standstill = cp_cam.vl["ES_DashStatus"]["Cruise_State"] == 3
ret.stockFcw = cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2
self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"]) self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
cp_es_distance = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam cp_es_distance = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp_cam

@ -87,8 +87,8 @@ routes = [
CarTestRoute("37398f32561a23ad|2021-11-18--00-11-35", HYUNDAI.SANTA_FE_HEV_2022), CarTestRoute("37398f32561a23ad|2021-11-18--00-11-35", HYUNDAI.SANTA_FE_HEV_2022),
CarTestRoute("656ac0d830792fcc|2021-12-28--14-45-56", HYUNDAI.SANTA_FE_PHEV_2022, segment=1), CarTestRoute("656ac0d830792fcc|2021-12-28--14-45-56", HYUNDAI.SANTA_FE_PHEV_2022, segment=1),
CarTestRoute("e0e98335f3ebc58f|2021-03-07--16-38-29", HYUNDAI.KIA_CEED), CarTestRoute("e0e98335f3ebc58f|2021-03-07--16-38-29", HYUNDAI.KIA_CEED),
CarTestRoute("7653b2bce7bcfdaa|2020-03-04--15-34-32", HYUNDAI.KIA_OPTIMA), CarTestRoute("7653b2bce7bcfdaa|2020-03-04--15-34-32", HYUNDAI.KIA_OPTIMA_G4),
CarTestRoute("018654717bc93d7d|2022-09-19--23-11-10", HYUNDAI.KIA_OPTIMA_2019, segment=0), CarTestRoute("018654717bc93d7d|2022-09-19--23-11-10", HYUNDAI.KIA_OPTIMA_G4_FL, segment=0),
CarTestRoute("c75a59efa0ecd502|2021-03-11--20-52-55", HYUNDAI.KIA_SELTOS), CarTestRoute("c75a59efa0ecd502|2021-03-11--20-52-55", HYUNDAI.KIA_SELTOS),
CarTestRoute("5b7c365c50084530|2020-04-15--16-13-24", HYUNDAI.SONATA), CarTestRoute("5b7c365c50084530|2020-04-15--16-13-24", HYUNDAI.SONATA),
CarTestRoute("b2a38c712dcf90bd|2020-05-18--18-12-48", HYUNDAI.SONATA_LF), CarTestRoute("b2a38c712dcf90bd|2020-05-18--18-12-48", HYUNDAI.SONATA_LF),

@ -1,4 +1,5 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
from collections import defaultdict
import re import re
import unittest import unittest
@ -20,6 +21,15 @@ class TestCarDocs(unittest.TestCase):
self.assertEqual(generated_cars_md, current_cars_md, self.assertEqual(generated_cars_md, current_cars_md,
"Run selfdrive/car/docs.py to update the compatibility documentation") "Run selfdrive/car/docs.py to update the compatibility documentation")
def test_duplicate_years(self):
make_model_years = defaultdict(list)
for car in self.all_cars:
with self.subTest(car_info_name=car.name):
make_model = (car.make, car.model)
for year in car.year_list:
self.assertNotIn(year, make_model_years[make_model], f"{car.name}: Duplicate model year")
make_model_years[make_model].append(year)
def test_missing_car_info(self): def test_missing_car_info(self):
all_car_info_platforms = get_interface_attr("CAR_INFO", combine_brands=True).keys() all_car_info_platforms = get_interface_attr("CAR_INFO", combine_brands=True).keys()
for platform in sorted(interfaces.keys()): for platform in sorted(interfaces.keys()):

@ -17,8 +17,8 @@ LEXUS RC 2020: LEXUS NX 2020
TOYOTA AVALON HYBRID 2019: TOYOTA AVALON 2019 TOYOTA AVALON HYBRID 2019: TOYOTA AVALON 2019
TOYOTA AVALON HYBRID 2022: TOYOTA AVALON 2022 TOYOTA AVALON HYBRID 2022: TOYOTA AVALON 2022
KIA OPTIMA 2016: HYUNDAI SONATA 2020 KIA OPTIMA 4TH GEN: HYUNDAI SONATA 2020
KIA OPTIMA 2019: HYUNDAI SONATA 2020 KIA OPTIMA 4TH GEN FACELIFT: HYUNDAI SONATA 2020
KIA OPTIMA HYBRID 2017 & SPORTS 2019: HYUNDAI SONATA 2020 KIA OPTIMA HYBRID 2017 & SPORTS 2019: HYUNDAI SONATA 2020
KIA FORTE E 2018 & GT 2021: HYUNDAI SONATA 2020 KIA FORTE E 2018 & GT 2021: HYUNDAI SONATA 2020
KIA CEED INTRO ED 2019: HYUNDAI SONATA 2020 KIA CEED INTRO ED 2019: HYUNDAI SONATA 2020

@ -171,8 +171,14 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
CAR.LEXUS_NX_TSS2: ToyotaCarInfo("Lexus NX 2020-21"), CAR.LEXUS_NX_TSS2: ToyotaCarInfo("Lexus NX 2020-21"),
CAR.LEXUS_NXH_TSS2: ToyotaCarInfo("Lexus NX Hybrid 2020-21"), CAR.LEXUS_NXH_TSS2: ToyotaCarInfo("Lexus NX Hybrid 2020-21"),
CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2017-20"), CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2017-20"),
CAR.LEXUS_RX: ToyotaCarInfo("Lexus RX 2016-19", footnotes=[Footnote.DSU]), CAR.LEXUS_RX: [
CAR.LEXUS_RXH: ToyotaCarInfo("Lexus RX Hybrid 2016-19", footnotes=[Footnote.DSU]), ToyotaCarInfo("Lexus RX 2016", "Lexus Safety System+", footnotes=[Footnote.DSU]),
ToyotaCarInfo("Lexus RX 2017-19", footnotes=[Footnote.DSU]),
],
CAR.LEXUS_RXH: [
ToyotaCarInfo("Lexus RX Hybrid 2016", "Lexus Safety System+", footnotes=[Footnote.DSU]),
ToyotaCarInfo("Lexus RX Hybrid 2017-19", footnotes=[Footnote.DSU]),
],
CAR.LEXUS_RX_TSS2: ToyotaCarInfo("Lexus RX 2020-22"), CAR.LEXUS_RX_TSS2: ToyotaCarInfo("Lexus RX 2020-22"),
CAR.LEXUS_RXH_TSS2: ToyotaCarInfo("Lexus RX Hybrid 2020-21"), CAR.LEXUS_RXH_TSS2: ToyotaCarInfo("Lexus RX Hybrid 2020-21"),
} }

@ -27,6 +27,10 @@ def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False):
for addr in valid_vin_addrs: for addr in valid_vin_addrs:
vin = results.get((addr, None)) vin = results.get((addr, None))
if vin is not None: if vin is not None:
# Ford pads with null bytes
if len(vin) == 24:
vin = re.sub(b'\x00*$', b'', vin)
# Honda Bosch response starts with a length, trim to correct length # Honda Bosch response starts with a length, trim to correct length
if vin.startswith(b'\x11'): if vin.startswith(b'\x11'):
vin = vin[1:18] vin = vin[1:18]

@ -170,7 +170,7 @@ class CarState(CarStateBase):
ret.gas = pt_cp.vl["Motor_3"]["Fahrpedal_Rohsignal"] / 100.0 ret.gas = pt_cp.vl["Motor_3"]["Fahrpedal_Rohsignal"] / 100.0
ret.gasPressed = ret.gas > 0 ret.gasPressed = ret.gas > 0
ret.brake = pt_cp.vl["Bremse_5"]["Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects ret.brake = pt_cp.vl["Bremse_5"]["Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects
ret.brakePressed = bool(pt_cp.vl["Motor_2"]["Bremstestschalter"]) ret.brakePressed = bool(pt_cp.vl["Motor_2"]["Bremslichtschalter"])
ret.parkingBrake = bool(pt_cp.vl["Kombi_1"]["Bremsinfo"]) ret.parkingBrake = bool(pt_cp.vl["Kombi_1"]["Bremsinfo"])
# Update gear and/or clutch position data. # Update gear and/or clutch position data.

@ -143,6 +143,12 @@ class Controls:
put_nonblocking("CarParamsCache", cp_bytes) put_nonblocking("CarParamsCache", cp_bytes)
put_nonblocking("CarParamsPersistent", cp_bytes) put_nonblocking("CarParamsPersistent", cp_bytes)
# cleanup old params
if not self.CP.experimentalLongitudinalAvailable:
params.remove("ExperimentalLongitudinalEnabled")
if not self.CP.openpilotLongitudinalControl:
params.remove("EndToEndLong")
self.CC = car.CarControl.new_message() self.CC = car.CarControl.new_message()
self.CS_prev = car.CarState.new_message() self.CS_prev = car.CarState.new_message()
self.AM = AlertManager() self.AM = AlertManager()
@ -313,7 +319,7 @@ class Controls:
else: else:
safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
if safety_mismatch or self.mismatch_counter >= 200: if safety_mismatch or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200:
self.events.add(EventName.controlsMismatch) self.events.add(EventName.controlsMismatch)
if log.PandaState.FaultType.relayMalfunction in pandaState.faults: if log.PandaState.FaultType.relayMalfunction in pandaState.faults:

@ -33,12 +33,6 @@ CRUISE_INTERVAL_SIGN = {
} }
class MPC_COST_LAT:
PATH = 1.0
HEADING = 1.0
STEER_RATE = 1.0
def apply_deadzone(error, deadzone): def apply_deadzone(error, deadzone):
if error > deadzone: if error > deadzone:
error -= deadzone error -= deadzone

@ -5,7 +5,6 @@ import numpy as np
from casadi import SX, vertcat, sin, cos from casadi import SX, vertcat, sin, cos
from common.realtime import sec_since_boot from common.realtime import sec_since_boot
from selfdrive.controls.lib.drive_helpers import LAT_MPC_N as N
from selfdrive.modeld.constants import T_IDXS from selfdrive.modeld.constants import T_IDXS
if __name__ == '__main__': # generating code if __name__ == '__main__': # generating code
@ -18,6 +17,10 @@ EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json") JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json")
X_DIM = 4 X_DIM = 4
P_DIM = 2 P_DIM = 2
N = 16
COST_E_DIM = 3
COST_DIM = COST_E_DIM + 1
SPEED_OFFSET = 10.0
MODEL_NAME = 'lat' MODEL_NAME = 'lat'
ACADOS_SOLVER_TYPE = 'SQP_RTI' ACADOS_SOLVER_TYPE = 'SQP_RTI'
@ -29,8 +32,8 @@ def gen_lat_model():
x_ego = SX.sym('x_ego') x_ego = SX.sym('x_ego')
y_ego = SX.sym('y_ego') y_ego = SX.sym('y_ego')
psi_ego = SX.sym('psi_ego') psi_ego = SX.sym('psi_ego')
curv_ego = SX.sym('curv_ego') psi_rate_ego = SX.sym('psi_rate_ego')
model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego) model.x = vertcat(x_ego, y_ego, psi_ego, psi_rate_ego)
# parameters # parameters
v_ego = SX.sym('v_ego') v_ego = SX.sym('v_ego')
@ -38,22 +41,22 @@ def gen_lat_model():
model.p = vertcat(v_ego, rotation_radius) model.p = vertcat(v_ego, rotation_radius)
# controls # controls
curv_rate = SX.sym('curv_rate') psi_accel_ego = SX.sym('psi_accel_ego')
model.u = vertcat(curv_rate) model.u = vertcat(psi_accel_ego)
# xdot # xdot
x_ego_dot = SX.sym('x_ego_dot') x_ego_dot = SX.sym('x_ego_dot')
y_ego_dot = SX.sym('y_ego_dot') y_ego_dot = SX.sym('y_ego_dot')
psi_ego_dot = SX.sym('psi_ego_dot') psi_ego_dot = SX.sym('psi_ego_dot')
curv_ego_dot = SX.sym('curv_ego_dot') psi_rate_ego_dot = SX.sym('psi_rate_ego_dot')
model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot) model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, psi_rate_ego_dot)
# dynamics model # dynamics model
f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego), f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * psi_rate_ego,
v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego), v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * psi_rate_ego,
v_ego * curv_ego, psi_rate_ego,
curv_rate) psi_accel_ego)
model.f_impl_expr = model.xdot - f_expl model.f_impl_expr = model.xdot - f_expl
model.f_expl_expr = f_expl model.f_expl_expr = f_expl
return model return model
@ -72,26 +75,27 @@ def gen_lat_ocp():
ocp.cost.cost_type = 'NONLINEAR_LS' ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.cost_type_e = 'NONLINEAR_LS' ocp.cost.cost_type_e = 'NONLINEAR_LS'
Q = np.diag([0.0, 0.0]) Q = np.diag(np.zeros(COST_E_DIM))
QR = np.diag([0.0, 0.0, 0.0]) QR = np.diag(np.zeros(COST_DIM))
ocp.cost.W = QR ocp.cost.W = QR
ocp.cost.W_e = Q ocp.cost.W_e = Q
y_ego, psi_ego = ocp.model.x[1], ocp.model.x[2] y_ego, psi_ego, psi_rate_ego = ocp.model.x[1], ocp.model.x[2], ocp.model.x[3]
curv_rate = ocp.model.u[0] psi_rate_ego_dot = ocp.model.u[0]
v_ego = ocp.model.p[0] v_ego = ocp.model.p[0]
ocp.parameter_values = np.zeros((P_DIM, )) ocp.parameter_values = np.zeros((P_DIM, ))
ocp.cost.yref = np.zeros((3, )) ocp.cost.yref = np.zeros((COST_DIM, ))
ocp.cost.yref_e = np.zeros((2, )) ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
# TODO hacky weights to keep behavior the same
ocp.model.cost_y_expr = vertcat(y_ego, ocp.model.cost_y_expr = vertcat(y_ego,
((v_ego +5.0) * psi_ego), ((v_ego + SPEED_OFFSET) * psi_ego),
((v_ego + 5.0) * 4.0 * curv_rate)) ((v_ego + SPEED_OFFSET) * psi_rate_ego),
((v_ego + SPEED_OFFSET) * psi_rate_ego_dot))
ocp.model.cost_y_expr_e = vertcat(y_ego, ocp.model.cost_y_expr_e = vertcat(y_ego,
((v_ego +5.0) * psi_ego)) ((v_ego + SPEED_OFFSET) * psi_ego),
((v_ego + SPEED_OFFSET) * psi_rate_ego))
# set constraints # set constraints
ocp.constraints.constr_type = 'BGH' ocp.constraints.constr_type = 'BGH'
@ -124,10 +128,10 @@ class LateralMpc():
def reset(self, x0=np.zeros(X_DIM)): def reset(self, x0=np.zeros(X_DIM)):
self.x_sol = np.zeros((N+1, X_DIM)) self.x_sol = np.zeros((N+1, X_DIM))
self.u_sol = np.zeros((N, 1)) self.u_sol = np.zeros((N, 1))
self.yref = np.zeros((N+1, 3)) self.yref = np.zeros((N+1, COST_DIM))
for i in range(N): for i in range(N):
self.solver.cost_set(i, "yref", self.yref[i]) self.solver.cost_set(i, "yref", self.yref[i])
self.solver.cost_set(N, "yref", self.yref[N][:2]) self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
# Somehow needed for stable init # Somehow needed for stable init
for i in range(N+1): for i in range(N+1):
@ -140,14 +144,13 @@ class LateralMpc():
self.solve_time = 0.0 self.solve_time = 0.0
self.cost = 0 self.cost = 0
def set_weights(self, path_weight, heading_weight, steer_rate_weight): def set_weights(self, path_weight, heading_weight, yaw_rate_weight, yaw_accel_cost):
W = np.asfortranarray(np.diag([path_weight, heading_weight, steer_rate_weight])) W = np.asfortranarray(np.diag([path_weight, heading_weight, yaw_rate_weight, yaw_accel_cost]))
for i in range(N): for i in range(N):
self.solver.cost_set(i, 'W', W) self.solver.cost_set(i, 'W', W)
#TODO hacky weights to keep behavior the same self.solver.cost_set(N, 'W', W[:COST_E_DIM,:COST_E_DIM])
self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2])
def run(self, x0, p, y_pts, heading_pts, curv_rate_pts): def run(self, x0, p, y_pts, heading_pts, yaw_rate_pts):
x0_cp = np.copy(x0) x0_cp = np.copy(x0)
p_cp = np.copy(p) p_cp = np.copy(p)
self.solver.constraints_set(0, "lbx", x0_cp) self.solver.constraints_set(0, "lbx", x0_cp)
@ -155,13 +158,13 @@ class LateralMpc():
self.yref[:,0] = y_pts self.yref[:,0] = y_pts
v_ego = p_cp[0] v_ego = p_cp[0]
# rotation_radius = p_cp[1] # rotation_radius = p_cp[1]
self.yref[:,1] = heading_pts*(v_ego+5.0) self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET)
self.yref[:,2] = curv_rate_pts * (v_ego+5.0) * 4.0 self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET)
for i in range(N): for i in range(N):
self.solver.cost_set(i, "yref", self.yref[i]) self.solver.cost_set(i, "yref", self.yref[i])
self.solver.set(i, "p", p_cp) self.solver.set(i, "p", p_cp)
self.solver.set(N, "p", p_cp) self.solver.set(N, "p", p_cp)
self.solver.cost_set(N, "yref", self.yref[N][:2]) self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
t = sec_since_boot() t = sec_since_boot()
self.solution_status = self.solver.solve() self.solution_status = self.solver.solve()

@ -3,7 +3,8 @@ from common.realtime import sec_since_boot, DT_MDL
from common.numpy_fast import interp from common.numpy_fast import interp
from system.swaglog import cloudlog from system.swaglog import cloudlog
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
from selfdrive.controls.lib.drive_helpers import CONTROL_N
from selfdrive.controls.lib.desire_helper import DesireHelper from selfdrive.controls.lib.desire_helper import DesireHelper
import cereal.messaging as messaging import cereal.messaging as messaging
from cereal import log from cereal import log
@ -11,6 +12,15 @@ from cereal import log
TRAJECTORY_SIZE = 33 TRAJECTORY_SIZE = 33
CAMERA_OFFSET = 0.04 CAMERA_OFFSET = 0.04
PATH_COST = 1.0
LATERAL_MOTION_COST = 0.11
LATERAL_ACCEL_COST = 0.0
LATERAL_JERK_COST = 0.05
MIN_SPEED = 1.5
class LateralPlanner: class LateralPlanner:
def __init__(self, CP): def __init__(self, CP):
self.DH = DesireHelper() self.DH = DesireHelper()
@ -22,9 +32,8 @@ class LateralPlanner:
self.solution_invalid_cnt = 0 self.solution_invalid_cnt = 0
self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3)) self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
self.plan_curv_rate = np.zeros((TRAJECTORY_SIZE,)) self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,))
self.t_idxs = np.arange(TRAJECTORY_SIZE) self.t_idxs = np.arange(TRAJECTORY_SIZE)
self.y_pts = np.zeros(TRAJECTORY_SIZE) self.y_pts = np.zeros(TRAJECTORY_SIZE)
@ -36,7 +45,8 @@ class LateralPlanner:
self.lat_mpc.reset(x0=self.x0) self.lat_mpc.reset(x0=self.x0)
def update(self, sm): def update(self, sm):
v_ego = sm['carState'].vEgo # clip speed , lateral planning is not possible at 0 speed
self.v_ego = max(MIN_SPEED, sm['carState'].vEgo)
measured_curvature = sm['controlsState'].curvature measured_curvature = sm['controlsState'].curvature
# Parse model predictions # Parse model predictions
@ -45,8 +55,7 @@ class LateralPlanner:
self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z]) self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
self.t_idxs = np.array(md.position.t) self.t_idxs = np.array(md.position.t)
self.plan_yaw = np.array(md.orientation.z) self.plan_yaw = np.array(md.orientation.z)
if len(md.position.xStd) == TRAJECTORY_SIZE: self.plan_yaw_rate = np.array(md.orientationRate.z)
self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd])
# Lane change logic # Lane change logic
desire_state = md.meta.desireState desire_state = md.meta.desireState
@ -57,29 +66,28 @@ class LateralPlanner:
self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob) self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
d_path_xyz = self.path_xyz d_path_xyz = self.path_xyz
# Heading cost is useful at low speed, otherwise end of plan can be off-heading self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST,
heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15]) LATERAL_ACCEL_COST, LATERAL_JERK_COST)
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE)
y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1]) y_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) heading_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
curv_rate_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_curv_rate) yaw_rate_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate)
self.y_pts = y_pts self.y_pts = y_pts
assert len(y_pts) == LAT_MPC_N + 1 assert len(y_pts) == LAT_MPC_N + 1
assert len(heading_pts) == LAT_MPC_N + 1 assert len(heading_pts) == LAT_MPC_N + 1
assert len(curv_rate_pts) == LAT_MPC_N + 1 assert len(yaw_rate_pts) == LAT_MPC_N + 1
lateral_factor = max(0, self.factor1 - (self.factor2 * v_ego**2)) lateral_factor = max(0, self.factor1 - (self.factor2 * self.v_ego**2))
p = np.array([v_ego, lateral_factor]) p = np.array([self.v_ego, lateral_factor])
self.lat_mpc.run(self.x0, self.lat_mpc.run(self.x0,
p, p,
y_pts, y_pts,
heading_pts, heading_pts,
curv_rate_pts) yaw_rate_pts)
# init state for next # init state for next iteration
# mpc.u_sol is the desired curvature rate given x0 curv state. # mpc.u_sol is the desired second derivative of psi given x0 curv state.
# with x0[3] = measured_curvature, this would be the actual desired rate. # with x0[3] = measured_yaw_rate, this would be the actual desired yaw rate.
# instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control. # instead, interpolate x_sol so that x0[3] is the desired yaw rate for lat_control.
self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3]) self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])
# Check for infeasible MPC solution # Check for infeasible MPC solution
@ -87,7 +95,7 @@ class LateralPlanner:
t = sec_since_boot() t = sec_since_boot()
if mpc_nans or self.lat_mpc.solution_status != 0: if mpc_nans or self.lat_mpc.solution_status != 0:
self.reset_mpc() self.reset_mpc()
self.x0[3] = measured_curvature self.x0[3] = measured_curvature * self.v_ego
if t > self.last_cloudlog_t + 5.0: if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True") cloudlog.warning("Lateral mpc - nan: True")
@ -106,8 +114,9 @@ class LateralPlanner:
lateralPlan.modelMonoTime = sm.logMonoTime['modelV2'] lateralPlan.modelMonoTime = sm.logMonoTime['modelV2']
lateralPlan.dPathPoints = self.y_pts.tolist() lateralPlan.dPathPoints = self.y_pts.tolist()
lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist() lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
lateralPlan.curvatures = self.lat_mpc.x_sol[0:CONTROL_N, 3].tolist()
lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0] lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist()
lateralPlan.curvatureRates = [float(x/self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
lateralPlan.mpcSolutionValid = bool(plan_solution_valid) lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
lateralPlan.solverExecutionTime = self.lat_mpc.solve_time lateralPlan.solverExecutionTime = self.lat_mpc.solve_time

@ -7,9 +7,6 @@ from selfdrive.modeld.constants import T_IDXS
LongCtrlState = car.CarControl.Actuators.LongControlState LongCtrlState = car.CarControl.Actuators.LongControlState
# As per ISO 15622:2018 for all speeds
ACCEL_MIN_ISO = -3.5 # m/s^2
ACCEL_MAX_ISO = 2.0 # m/s^2
def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
v_target_1sec, brake_pressed, cruise_standstill): v_target_1sec, brake_pressed, cruise_standstill):

@ -253,11 +253,8 @@ class LongitudinalMpc:
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST] cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST]
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
elif self.mode == 'blended': elif self.mode == 'blended':
cost_weights = [0., 0.2, 0.25, 1.0, 0.0, 1.0] cost_weights = [0., 0.1, 0.2, 5.0, 0.0, 1.0]
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0]
elif self.mode == 'e2e':
cost_weights = [0., 0.2, 0.25, 1., 0.0, .1]
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0]
else: else:
raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set') raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set')
self.set_cost_weights(cost_weights, constraint_cost_weights) self.set_cost_weights(cost_weights, constraint_cost_weights)
@ -386,28 +383,6 @@ class LongitudinalMpc:
(lead_1_obstacle[0] - lead_0_obstacle[0]): (lead_1_obstacle[0] - lead_0_obstacle[0]):
self.source = 'lead1' self.source = 'lead1'
def update_with_xva(self, x, v, a):
self.params[:,0] = -10.
self.params[:,1] = 10.
self.params[:,2] = 1e5
self.params[:,4] = T_FOLLOW
self.params[:,5] = LEAD_DANGER_FACTOR
# v, and a are in local frame, but x is wrt the x[0] position
# In >90degree turns, x goes to 0 (and may even be -ve)
# So, we use integral(v) + x[0] to obtain the forward-distance
xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])
x = np.cumsum(np.insert(xforward, 0, x[0]))
self.yref[:,1] = x
self.yref[:,2] = v
self.yref[:,3] = a
for i in range(N):
self.solver.cost_set(i, "yref", self.yref[i])
self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
self.params[:,3] = np.copy(self.prev_a)
self.run()
def run(self): def run(self):
# t0 = sec_since_boot() # t0 = sec_since_boot()
# reset = 0 # reset = 0

@ -58,7 +58,6 @@ class LongitudinalPlanner:
self.a_desired = init_a self.a_desired = init_a
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL) self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL)
self.t_uniform = np.arange(0.0, T_IDXS_MPC[-1] + 0.5, 0.5)
self.v_desired_trajectory = np.zeros(CONTROL_N) self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N)
@ -76,10 +75,7 @@ class LongitudinalPlanner:
x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x) x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x)
v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x)
a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x) a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x)
# Uniform interp so gradient is less noisy j = np.zeros(len(T_IDXS_MPC))
a_sparse = np.interp(self.t_uniform, T_IDXS, model_msg.acceleration.x)
j_sparse = np.gradient(a_sparse, self.t_uniform)
j = np.interp(T_IDXS_MPC, self.t_uniform, j_sparse)
else: else:
x = np.zeros(len(T_IDXS_MPC)) x = np.zeros(len(T_IDXS_MPC))
v = np.zeros(len(T_IDXS_MPC)) v = np.zeros(len(T_IDXS_MPC))
@ -87,8 +83,8 @@ class LongitudinalPlanner:
j = np.zeros(len(T_IDXS_MPC)) j = np.zeros(len(T_IDXS_MPC))
return x, v, a, j return x, v, a, j
def update(self, sm): def update(self, sm, read=True):
if self.param_read_counter % 50 == 0: if self.param_read_counter % 50 == 0 and read:
self.read_param() self.read_param()
self.param_read_counter += 1 self.param_read_counter += 1

@ -1,7 +1,8 @@
import unittest import unittest
import numpy as np import numpy as np
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
from selfdrive.controls.lib.drive_helpers import LAT_MPC_N, CAR_ROTATION_RADIUS from selfdrive.controls.lib.drive_helpers import CAR_ROTATION_RADIUS
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0., def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0.,
@ -9,7 +10,7 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur
if lat_mpc is None: if lat_mpc is None:
lat_mpc = LateralMpc() lat_mpc = LateralMpc()
lat_mpc.set_weights(1., 1., 1.) lat_mpc.set_weights(1., 1., 0.0, 1.)
y_pts = poly_shift * np.ones(LAT_MPC_N + 1) y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
heading_pts = np.zeros(LAT_MPC_N + 1) heading_pts = np.zeros(LAT_MPC_N + 1)

@ -70,8 +70,8 @@ if __name__ == "__main__":
coding_variant, current_coding_array, coding_byte, coding_bit = None, None, 0, 0 coding_variant, current_coding_array, coding_byte, coding_bit = None, None, 0, 0
coding_length = len(current_coding) coding_length = len(current_coding)
# EV_SteerAssisMQB covers the majority of MQB racks (EPS_MQB_ZFLS) # EV_SteerAssisMQB/MNB cover the majority of MQB racks (EPS_MQB_ZFLS)
if odx_file == "EV_SteerAssisMQB\x00": if odx_file in ("EV_SteerAssisMQB\x00", "EV_SteerAssisMNB\x00"):
coding_variant = "ZF" coding_variant = "ZF"
coding_byte = 0 coding_byte = 0
coding_bit = 4 coding_bit = 4
@ -111,7 +111,7 @@ if __name__ == "__main__":
if args.action in ["enable", "disable"]: if args.action in ["enable", "disable"]:
print("\nAttempting configuration update") print("\nAttempting configuration update")
assert(coding_variant in ("ZF", "APA")) assert(coding_variant in ("ZF", "APA"))
# ZF EPS config coding length can be anywhere from 1 to 4 bytes, but the # ZF EPS config coding length can be anywhere from 1 to 4 bytes, but the
# bit we care about is always in the same place in the first byte # bit we care about is always in the same place in the first byte
if args.action == "enable": if args.action == "enable":

@ -16,7 +16,7 @@ from common.params import Params, put_nonblocking
from laika import AstroDog from laika import AstroDog
from laika.constants import SECS_IN_HR, SECS_IN_MIN from laika.constants import SECS_IN_HR, SECS_IN_MIN
from laika.downloader import DownloadFailed from laika.downloader import DownloadFailed
from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem, parse_qcom_ephem
from laika.gps_time import GPSTime from laika.gps_time import GPSTime
from laika.helpers import ConstellationId from laika.helpers import ConstellationId
from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom
@ -61,6 +61,7 @@ class Laikad:
self.last_pos_fix = [] self.last_pos_fix = []
self.last_pos_residual = [] self.last_pos_residual = []
self.last_pos_fix_t = None self.last_pos_fix_t = None
self.gps_week = None
self.use_qcom = use_qcom self.use_qcom = use_qcom
def load_cache(self): def load_cache(self):
@ -107,11 +108,11 @@ class Laikad:
return self.last_pos_fix return self.last_pos_fix
def is_good_report(self, gnss_msg): def is_good_report(self, gnss_msg):
if gnss_msg.which == 'drMeasurementReport' and self.use_qcom: if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom:
constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source) constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source)
# TODO support GLONASS # TODO support GLONASS
return constellation_id in [ConstellationId.GPS, ConstellationId.SBAS] return constellation_id in [ConstellationId.GPS, ConstellationId.SBAS]
elif gnss_msg.which == 'measurementReport' and not self.use_qcom: elif gnss_msg.which() == 'measurementReport' and not self.use_qcom:
return True return True
else: else:
return False return False
@ -129,9 +130,28 @@ class Laikad:
new_meas = read_raw_ublox(report) new_meas = read_raw_ublox(report)
return week, tow, new_meas return week, tow, new_meas
def is_ephemeris(self, gnss_msg):
if self.use_qcom:
return gnss_msg.which() == 'drSvPoly'
else:
return gnss_msg.which() == 'ephemeris'
def read_ephemeris(self, gnss_msg):
# TODO this only works on GLONASS
if self.use_qcom:
# TODO this is not robust to gps week rollover
if self.gps_week is None:
return
ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week)
else:
ephem = convert_ublox_ephem(gnss_msg.ephemeris)
self.astro_dog.add_navs({ephem.prn: [ephem]})
self.cache_ephemeris(t=ephem.epoch)
def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False): def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False):
if self.is_good_report(gnss_msg): if self.is_good_report(gnss_msg):
week, tow, new_meas = self.read_report(gnss_msg) week, tow, new_meas = self.read_report(gnss_msg)
self.gps_week = week
t = gnss_mono_time * 1e-9 t = gnss_mono_time * 1e-9
if week > 0: if week > 0:
@ -172,12 +192,10 @@ class Laikad:
"correctedMeasurements": meas_msgs "correctedMeasurements": meas_msgs
} }
return dat return dat
# TODO this only works on GLONASS, qcom needs live ephemeris parsing too elif self.is_ephemeris(gnss_msg):
elif gnss_msg.which == 'ephemeris': self.read_ephemeris(gnss_msg)
ephem = convert_ublox_ephem(gnss_msg.ephemeris)
self.astro_dog.add_navs({ephem.prn: [ephem]}) #elif gnss_msg.which() == 'ionoData':
self.cache_ephemeris(t=ephem.epoch)
#elif gnss_msg.which == 'ionoData':
# todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them.
def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]): def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]):
@ -265,9 +283,11 @@ def create_measurement_msg(meas: GNSSMeasurement):
c.satVel = meas.sat_vel.tolist() c.satVel = meas.sat_vel.tolist()
ephem = meas.sat_ephemeris ephem = meas.sat_ephemeris
assert ephem is not None assert ephem is not None
week, time_of_week = -1, -1
if ephem.eph_type == EphemerisType.NAV: if ephem.eph_type == EphemerisType.NAV:
source_type = EphemerisSourceType.nav source_type = EphemerisSourceType.nav
week, time_of_week = -1, -1 elif ephem.eph_type == EphemerisType.QCOM_POLY:
source_type = EphemerisSourceType.qcom
else: else:
assert ephem.file_epoch is not None assert ephem.file_epoch is not None
week = ephem.file_epoch.week week = ephem.file_epoch.week
@ -325,10 +345,11 @@ class EphemerisSourceType(IntEnum):
nav = 0 nav = 0
nasaUltraRapid = 1 nasaUltraRapid = 1
glonassIacUltraRapid = 2 glonassIacUltraRapid = 2
qcom = 3
def main(sm=None, pm=None): def main(sm=None, pm=None):
use_qcom = os.path.isfile("/persist/comma/use-quectel-rawgps") use_qcom = not Params().get_bool("UbloxAvailable", block=True)
if use_qcom: if use_qcom:
raw_gnss_socket = "qcomGnss" raw_gnss_socket = "qcomGnss"
else: else:
@ -348,6 +369,17 @@ def main(sm=None, pm=None):
if sm.updated[raw_gnss_socket]: if sm.updated[raw_gnss_socket]:
gnss_msg = sm[raw_gnss_socket] gnss_msg = sm[raw_gnss_socket]
# TODO: Understand and use remaining unknown constellations
if gnss_msg.which() == "drMeasurementReport":
if getattr(gnss_msg, gnss_msg.which()).source not in ['glonass', 'gps', 'beidou', 'sbas']:
continue
if getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max:
# gpsWeek 65535 is received rarely from quectel, this cannot be
# passed to GnssMeasurements's gpsWeek (Int16)
continue
msg = laikad.process_gnss_msg(gnss_msg, sm.logMonoTime[raw_gnss_socket], block=replay) msg = laikad.process_gnss_msg(gnss_msg, sm.logMonoTime[raw_gnss_socket], block=replay)
if msg is not None: if msg is not None:
pm.send('gnssMeasurements', msg) pm.send('gnssMeasurements', msg)

@ -492,10 +492,10 @@ void Localizer::determine_gps_mode(double current_time) {
int Localizer::locationd_thread() { int Localizer::locationd_thread() {
const char* gps_location_socket; const char* gps_location_socket;
if (util::file_exists("/persist/comma/use-quectel-rawgps")) { if (Params().getBool("UbloxAvailable", true)) {
gps_location_socket = "gpsLocation";
} else {
gps_location_socket = "gpsLocationExternal"; gps_location_socket = "gpsLocationExternal";
} else {
gps_location_socket = "gpsLocation";
} }
const std::initializer_list<const char *> service_list = {gps_location_socket, const std::initializer_list<const char *> service_list = {gps_location_socket,
"cameraOdometry", "liveCalibration", "carState", "carParams", "cameraOdometry", "liveCalibration", "carState", "carParams",

@ -22,6 +22,7 @@ class TestLocationdProc(unittest.TestCase):
self.pm = messaging.PubMaster(self.LLD_MSGS) self.pm = messaging.PubMaster(self.LLD_MSGS)
Params().put_bool("UbloxAvailable", True)
managed_processes['locationd'].prepare() managed_processes['locationd'].prepare()
managed_processes['locationd'].start() managed_processes['locationd'].start()

@ -12,7 +12,6 @@ from common.realtime import config_realtime_process, DT_MDL
from common.filter_simple import FirstOrderFilter from common.filter_simple import FirstOrderFilter
from system.swaglog import cloudlog from system.swaglog import cloudlog
from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
from selfdrive.car.toyota.values import CAR as TOYOTA
HISTORY = 5 # secs HISTORY = 5 # secs
POINTS_PER_BUCKET = 1500 POINTS_PER_BUCKET = 1500
@ -33,7 +32,7 @@ MAX_INVALID_THRESHOLD = 10
MIN_ENGAGE_BUFFER = 2 # secs MIN_ENGAGE_BUFFER = 2 # secs
VERSION = 1 # bump this to invalidate old parameter caches VERSION = 1 # bump this to invalidate old parameter caches
ALLOWED_FINGERPRINTS = [TOYOTA.COROLLA_TSS2, TOYOTA.COROLLA, TOYOTA.COROLLAH_TSS2] ALLOWED_CARS = ['toyota', 'hyundai']
def slope2rot(slope): def slope2rot(slope):
@ -98,7 +97,7 @@ class TorqueEstimator:
self.offline_friction = 0.0 self.offline_friction = 0.0
self.offline_latAccelFactor = 0.0 self.offline_latAccelFactor = 0.0
self.resets = 0.0 self.resets = 0.0
self.use_params = CP.carFingerprint in ALLOWED_FINGERPRINTS self.use_params = CP.carName in ALLOWED_CARS
if CP.lateralTuning.which() == 'torque': if CP.lateralTuning.which() == 'torque':
self.offline_friction = CP.lateralTuning.torque.friction self.offline_friction = CP.lateralTuning.torque.friction

@ -110,6 +110,8 @@ class TestLoggerd(unittest.TestCase):
self.assertEqual(getattr(initData, initData_key), v) self.assertEqual(getattr(initData, initData_key), v)
self.assertEqual(logged_params[param_key].decode(), v) self.assertEqual(logged_params[param_key].decode(), v)
params.put("LaikadEphemeris", "")
def test_rotation(self): def test_rotation(self):
os.environ["LOGGERD_TEST"] = "1" os.environ["LOGGERD_TEST"] = "1"
Params().put("RecordFront", "1") Params().put("RecordFront", "1")

@ -43,6 +43,7 @@ procs = [
PythonProcess("deleter", "selfdrive.loggerd.deleter", offroad=True), PythonProcess("deleter", "selfdrive.loggerd.deleter", offroad=True),
PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), callback=driverview), PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), callback=driverview),
PythonProcess("laikad", "selfdrive.locationd.laikad"), PythonProcess("laikad", "selfdrive.locationd.laikad"),
PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=TICI),
PythonProcess("navd", "selfdrive.navd.navd"), PythonProcess("navd", "selfdrive.navd.navd"),
PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True), PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True),
PythonProcess("paramsd", "selfdrive.locationd.paramsd"), PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
@ -55,11 +56,9 @@ procs = [
PythonProcess("uploader", "selfdrive.loggerd.uploader", offroad=True), PythonProcess("uploader", "selfdrive.loggerd.uploader", offroad=True),
PythonProcess("statsd", "selfdrive.statsd", offroad=True), PythonProcess("statsd", "selfdrive.statsd", offroad=True),
# debug procs
NativeProcess("bridge", "cereal/messaging", ["./bridge"], onroad=False, callback=notcar), NativeProcess("bridge", "cereal/messaging", ["./bridge"], onroad=False, callback=notcar),
PythonProcess("webjoystick", "tools.joystick.web", onroad=False, callback=notcar), PythonProcess("webjoystick", "tools.joystick.web", onroad=False, callback=notcar),
# Experimental
PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=(TICI and os.path.isfile("/persist/comma/use-quectel-rawgps"))),
] ]
managed_processes = {p.name: p for p in procs} managed_processes = {p.name: p for p in procs}

@ -71,9 +71,9 @@ if use_thneed and arch == "larch64" or GetOption('pc_thneed'):
fn = File("models/supercombo").abspath fn = File("models/supercombo").abspath
if GetOption('pc_thneed'): if GetOption('pc_thneed'):
cmd = f"cd {Dir('#').abspath}/tinygrad_repo && NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed" cmd = f"cd {Dir('#').abspath}/tinygrad_repo && GPU=1 NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed"
else: else:
cmd = f"cd {Dir('#').abspath}/tinygrad_repo && FLOAT16=1 PYOPENCL_NO_CACHE=1 MATMUL=1 NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed" cmd = f"cd {Dir('#').abspath}/tinygrad_repo && FLOAT16=1 MATMUL=1 PYOPENCL_NO_CACHE=1 NATIVE_EXPLOG=1 OPTWG=1 UNSAFE_FLOAT4=1 DEBUGCL=1 python3 openpilot/compile.py {fn}.onnx {fn}.thneed"
# is there a better way then listing all of tinygrad? # is there a better way then listing all of tinygrad?
lenv.Command(fn + ".thneed", [fn + ".onnx", lenv.Command(fn + ".thneed", [fn + ".onnx",

@ -41,11 +41,11 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
&s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, true, false, context); &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, true, false, context);
#ifdef TEMPORAL #ifdef TEMPORAL
s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE); s->m->addRecurrent(&s->feature_buffer[0], TEMPORAL_SIZE);
#endif #endif
#ifdef DESIRE #ifdef DESIRE
s->m->addDesire(s->pulse_desire, DESIRE_LEN); s->m->addDesire(s->pulse_desire, DESIRE_LEN*(HISTORY_BUFFER_LEN+1));
#endif #endif
#ifdef TRAFFIC_CONVENTION #ifdef TRAFFIC_CONVENTION
@ -56,18 +56,20 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf, ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf,
const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only) { const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only) {
#ifdef DESIRE #ifdef DESIRE
std::memmove(&s->pulse_desire[0], &s->pulse_desire[DESIRE_LEN], sizeof(float) * DESIRE_LEN*HISTORY_BUFFER_LEN);
if (desire_in != NULL) { if (desire_in != NULL) {
for (int i = 1; i < DESIRE_LEN; i++) { for (int i = 1; i < DESIRE_LEN; i++) {
// Model decides when action is completed // Model decides when action is completed
// so desire input is just a pulse triggered on rising edge // so desire input is just a pulse triggered on rising edge
if (desire_in[i] - s->prev_desire[i] > .99) { if (desire_in[i] - s->prev_desire[i] > .99) {
s->pulse_desire[i] = desire_in[i]; s->pulse_desire[DESIRE_LEN*(HISTORY_BUFFER_LEN-1)+i] = desire_in[i];
} else { } else {
s->pulse_desire[i] = 0.0; s->pulse_desire[DESIRE_LEN*(HISTORY_BUFFER_LEN-1)+i] = 0.0;
} }
s->prev_desire[i] = desire_in[i]; s->prev_desire[i] = desire_in[i];
} }
} }
LOGT("Desire enqueued");
#endif #endif
int rhd_idx = is_rhd; int rhd_idx = is_rhd;
@ -92,6 +94,12 @@ ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf,
s->m->execute(); s->m->execute();
LOGT("Execution finished"); LOGT("Execution finished");
#ifdef TEMPORAL
std::memmove(&s->feature_buffer[0], &s->feature_buffer[FEATURE_LEN], sizeof(float) * FEATURE_LEN*(HISTORY_BUFFER_LEN-1));
std::memcpy(&s->feature_buffer[FEATURE_LEN*(HISTORY_BUFFER_LEN-1)], &s->output[OUTPUT_SIZE], sizeof(float) * FEATURE_LEN);
LOGT("Features enqueued");
#endif
return (ModelOutput*)&s->output; return (ModelOutput*)&s->output;
} }

@ -16,6 +16,8 @@
#include "selfdrive/modeld/models/commonmodel.h" #include "selfdrive/modeld/models/commonmodel.h"
#include "selfdrive/modeld/runners/run.h" #include "selfdrive/modeld/runners/run.h"
constexpr int FEATURE_LEN = 128;
constexpr int HISTORY_BUFFER_LEN = 99;
constexpr int DESIRE_LEN = 8; constexpr int DESIRE_LEN = 8;
constexpr int DESIRE_PRED_LEN = 4; constexpr int DESIRE_PRED_LEN = 4;
constexpr int TRAFFIC_CONVENTION_LEN = 2; constexpr int TRAFFIC_CONVENTION_LEN = 2;
@ -233,6 +235,11 @@ struct ModelOutputMeta {
}; };
static_assert(sizeof(ModelOutputMeta) == sizeof(ModelOutputDesireProb) + sizeof(float) + (sizeof(ModelOutputDisengageProb)*DISENGAGE_LEN) + (sizeof(ModelOutputBlinkerProb)*BLINKER_LEN) + (sizeof(ModelOutputDesireProb)*DESIRE_PRED_LEN)); static_assert(sizeof(ModelOutputMeta) == sizeof(ModelOutputDesireProb) + sizeof(float) + (sizeof(ModelOutputDisengageProb)*DISENGAGE_LEN) + (sizeof(ModelOutputBlinkerProb)*BLINKER_LEN) + (sizeof(ModelOutputDesireProb)*DESIRE_PRED_LEN));
struct ModelOutputFeatures {
std::array<float, FEATURE_LEN> feature;
};
static_assert(sizeof(ModelOutputFeatures) == (sizeof(float)*FEATURE_LEN));
struct ModelOutput { struct ModelOutput {
const ModelOutputPlans plans; const ModelOutputPlans plans;
const ModelOutputLaneLines lane_lines; const ModelOutputLaneLines lane_lines;
@ -244,22 +251,24 @@ struct ModelOutput {
}; };
constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float); constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float);
#ifdef TEMPORAL #ifdef TEMPORAL
constexpr int TEMPORAL_SIZE = 512; constexpr int TEMPORAL_SIZE = HISTORY_BUFFER_LEN * FEATURE_LEN;
#else #else
constexpr int TEMPORAL_SIZE = 0; constexpr int TEMPORAL_SIZE = 0;
#endif #endif
constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + TEMPORAL_SIZE; constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + FEATURE_LEN;
// TODO: convert remaining arrays to std::array and update model runners // TODO: convert remaining arrays to std::array and update model runners
struct ModelState { struct ModelState {
ModelFrame *frame = nullptr; ModelFrame *frame = nullptr;
ModelFrame *wide_frame = nullptr; ModelFrame *wide_frame = nullptr;
std::array<float, HISTORY_BUFFER_LEN * FEATURE_LEN> feature_buffer = {};
std::array<float, NET_OUTPUT_SIZE> output = {}; std::array<float, NET_OUTPUT_SIZE> output = {};
std::unique_ptr<RunModel> m; std::unique_ptr<RunModel> m;
#ifdef DESIRE #ifdef DESIRE
float prev_desire[DESIRE_LEN] = {}; float prev_desire[DESIRE_LEN] = {};
float pulse_desire[DESIRE_LEN] = {}; float pulse_desire[DESIRE_LEN*(HISTORY_BUFFER_LEN+1)] = {};
#endif #endif
#ifdef TRAFFIC_CONVENTION #ifdef TRAFFIC_CONVENTION
float traffic_convention[TRAFFIC_CONVENTION_LEN] = {}; float traffic_convention[TRAFFIC_CONVENTION_LEN] = {};

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:50c7fc8565ac69a4b9a0de122e961326820e78bf13659255a89d0ed04be030d5 oid sha256:c4d37af666344af6bb218e0b939b1152ad3784c15ac79e37bcf0124643c8286a
size 95167481 size 58539563

@ -9,6 +9,8 @@ os.environ["OMP_WAIT_POLICY"] = "PASSIVE"
import onnxruntime as ort # pylint: disable=import-error import onnxruntime as ort # pylint: disable=import-error
ORT_TYPES_TO_NP_TYPES = {'tensor(float16)': np.float16, 'tensor(float)': np.float32, 'tensor(uint8)': np.uint8}
def read(sz, tf8=False): def read(sz, tf8=False):
dd = [] dd = []
gt = 0 gt = 0
@ -18,7 +20,7 @@ def read(sz, tf8=False):
assert(len(st) > 0) assert(len(st) > 0)
dd.append(st) dd.append(st)
gt += len(st) gt += len(st)
r = np.frombuffer(b''.join(dd), dtype=np.uint8 if tf8 else np.float32).astype(np.float32) r = np.frombuffer(b''.join(dd), dtype=np.uint8 if tf8 else np.float32)
if tf8: if tf8:
r = r / 255. r = r / 255.
return r return r
@ -29,22 +31,23 @@ def write(d):
def run_loop(m, tf8_input=False): def run_loop(m, tf8_input=False):
ishapes = [[1]+ii.shape[1:] for ii in m.get_inputs()] ishapes = [[1]+ii.shape[1:] for ii in m.get_inputs()]
keys = [x.name for x in m.get_inputs()] keys = [x.name for x in m.get_inputs()]
itypes = [ORT_TYPES_TO_NP_TYPES[x.type] for x in m.get_inputs()]
# run once to initialize CUDA provider # run once to initialize CUDA provider
if "CUDAExecutionProvider" in m.get_providers(): if "CUDAExecutionProvider" in m.get_providers():
m.run(None, dict(zip(keys, [np.zeros(shp, dtype=np.float32) for shp in ishapes]))) m.run(None, dict(zip(keys, [np.zeros(shp, dtype=itp) for shp, itp in zip(ishapes, itypes)])))
print("ready to run onnx model", keys, ishapes, file=sys.stderr) print("ready to run onnx model", keys, ishapes, file=sys.stderr)
while 1: while 1:
inputs = [] inputs = []
for k, shp in zip(keys, ishapes): for k, shp, itp in zip(keys, ishapes, itypes):
ts = np.product(shp) ts = np.product(shp)
#print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr) #print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr)
inputs.append(read(ts, (k=='input_img' and tf8_input)).reshape(shp)) inputs.append(read(ts, (k=='input_img' and tf8_input)).reshape(shp).astype(itp))
ret = m.run(None, dict(zip(keys, inputs))) ret = m.run(None, dict(zip(keys, inputs)))
#print(ret, file=sys.stderr) #print(ret, file=sys.stderr)
for r in ret: for r in ret:
write(r) write(r.astype(np.float32))
if __name__ == "__main__": if __name__ == "__main__":

@ -12,7 +12,7 @@ map<pair<cl_kernel, int>, int> g_args_size;
map<cl_program, string> g_program_source; map<cl_program, string> g_program_source;
void Thneed::stop() { void Thneed::stop() {
printf("Thneed::stop: recorded %lu commands\n", cmds.size()); //printf("Thneed::stop: recorded %lu commands\n", cmds.size());
record = false; record = false;
} }

@ -116,7 +116,7 @@ class TTYPigeon():
time.sleep(0.001) time.sleep(0.001)
def initialize_pigeon(pigeon: TTYPigeon) -> None: def initialize_pigeon(pigeon: TTYPigeon) -> bool:
# try initializing a few times # try initializing a few times
for _ in range(10): for _ in range(10):
try: try:
@ -198,6 +198,10 @@ def initialize_pigeon(pigeon: TTYPigeon) -> None:
break break
except TimeoutError: except TimeoutError:
cloudlog.warning("Initialization failed, trying again!") cloudlog.warning("Initialization failed, trying again!")
else:
cloudlog.warning("Failed to initialize pigeon")
return False
return True
def deinitialize_and_exit(pigeon: Optional[TTYPigeon]): def deinitialize_and_exit(pigeon: Optional[TTYPigeon]):
cloudlog.warning("Storing almanac in ublox flash") cloudlog.warning("Storing almanac in ublox flash")
@ -236,7 +240,8 @@ def main():
time.sleep(0.5) time.sleep(0.5)
pigeon = TTYPigeon() pigeon = TTYPigeon()
initialize_pigeon(pigeon) r = initialize_pigeon(pigeon)
Params().put_bool("UbloxAvailable", r)
# start receiving data # start receiving data
while True: while True:
@ -251,6 +256,9 @@ def main():
msg = messaging.new_message('ubloxRaw', len(dat)) msg = messaging.new_message('ubloxRaw', len(dat))
msg.ubloxRaw = dat[:] msg.ubloxRaw = dat[:]
pm.send('ubloxRaw', msg) pm.send('ubloxRaw', msg)
else:
# prevent locking up a CPU core if ublox disconnects
time.sleep(0.001)
if __name__ == "__main__": if __name__ == "__main__":
main() main()

@ -1,5 +1,3 @@
import os
import time
import select import select
from serial import Serial from serial import Serial
from crcmod import mkCrcFun from crcmod import mkCrcFun
@ -11,18 +9,7 @@ class ModemDiag:
self.pend = b'' self.pend = b''
def open_serial(self): def open_serial(self):
def op(): serial = Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True, timeout=0, exclusive=True)
return Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True, timeout=0)
try:
serial = op()
except Exception:
# TODO: this is a hack to get around modemmanager's exclusive open
print("unlocking serial...")
os.system('sudo su -c \'echo "1-1.1:1.0" > /sys/bus/usb/drivers/option/unbind\'')
os.system('sudo su -c \'echo "1-1.1:1.0" > /sys/bus/usb/drivers/option/bind\'')
time.sleep(0.5)
os.system("sudo chmod 666 /dev/ttyUSB0")
serial = op()
serial.flush() serial.flush()
serial.reset_input_buffer() serial.reset_input_buffer()
serial.reset_output_buffer() serial.reset_output_buffer()

@ -5,23 +5,34 @@ import signal
import itertools import itertools
import math import math
import time import time
import subprocess
from typing import NoReturn from typing import NoReturn
from struct import unpack_from, calcsize, pack from struct import unpack_from, calcsize, pack
import cereal.messaging as messaging
from cereal import log from cereal import log
from system.swaglog import cloudlog import cereal.messaging as messaging
from laika.gps_time import GPSTime from laika.gps_time import GPSTime
from system.swaglog import cloudlog
from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
from selfdrive.sensord.rawgps.structs import dict_unpacker from selfdrive.sensord.rawgps.structs import (dict_unpacker, position_report, relist,
from selfdrive.sensord.rawgps.structs import gps_measurement_report, gps_measurement_report_sv gps_measurement_report, gps_measurement_report_sv,
from selfdrive.sensord.rawgps.structs import glonass_measurement_report, glonass_measurement_report_sv glonass_measurement_report, glonass_measurement_report_sv,
from selfdrive.sensord.rawgps.structs import oemdre_measurement_report, oemdre_measurement_report_sv oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report,
from selfdrive.sensord.rawgps.structs import LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
from selfdrive.sensord.rawgps.structs import position_report, LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
LOG_GNSS_OEMDRE_SVPOLY_REPORT)
DEBUG = int(os.getenv("DEBUG", "0"))==1 DEBUG = int(os.getenv("DEBUG", "0"))==1
LOG_TYPES = [
LOG_GNSS_GPS_MEASUREMENT_REPORT,
LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
LOG_GNSS_POSITION_REPORT,
LOG_GNSS_OEMDRE_SVPOLY_REPORT,
]
miscStatusFields = { miscStatusFields = {
"multipathEstimateIsValid": 0, "multipathEstimateIsValid": 0,
"directionIsValid": 1, "directionIsValid": 1,
@ -65,59 +76,43 @@ measurementStatusGlonassFields = {
"glonassTimeMarkValid": 17 "glonassTimeMarkValid": 17
} }
def main() -> NoReturn:
unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True)
unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(gps_measurement_report_sv, True)
unpack_glonass_meas, size_glonass_meas = dict_unpacker(glonass_measurement_report, True)
unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(glonass_measurement_report_sv, True)
unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True) def try_setup_logs(diag, log_types):
unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True) for _ in range(5):
try:
log_types = [ setup_logs(diag, log_types)
LOG_GNSS_GPS_MEASUREMENT_REPORT, break
LOG_GNSS_GLONASS_MEASUREMENT_REPORT, except Exception:
LOG_GNSS_OEMDRE_MEASUREMENT_REPORT, cloudlog.exception("setup logs failed, trying again")
] else:
pub_types = ['qcomGnss'] raise Exception(f"setup logs failed, {log_types=}")
unpack_position, _ = dict_unpacker(position_report)
log_types.append(LOG_GNSS_POSITION_REPORT) def mmcli(cmd: str) -> None:
pub_types.append("gpsLocation") for _ in range(5):
try:
# connect to modem subprocess.check_call(f"mmcli -m any --timeout 30 {cmd}", shell=True)
diag = ModemDiag() break
except subprocess.CalledProcessError:
# NV enable OEMDRE cloudlog.exception("rawgps.mmcli_command_failed")
else:
raise Exception(f"failed to execute mmcli command {cmd=}")
def setup_quectel(diag: ModemDiag):
# enable OEMDRE in the NV
# TODO: it has to reboot for this to take effect # TODO: it has to reboot for this to take effect
DIAG_NV_READ_F = 38 DIAG_NV_READ_F = 38
DIAG_NV_WRITE_F = 39 DIAG_NV_WRITE_F = 39
NV_GNSS_OEM_FEATURE_MASK = 7165 NV_GNSS_OEM_FEATURE_MASK = 7165
send_recv(diag, DIAG_NV_WRITE_F, pack('<HI', NV_GNSS_OEM_FEATURE_MASK, 1))
send_recv(diag, DIAG_NV_READ_F, pack('<H', NV_GNSS_OEM_FEATURE_MASK))
opcode, payload = send_recv(diag, DIAG_NV_WRITE_F, pack('<HI', NV_GNSS_OEM_FEATURE_MASK, 1)) setup_logs(diag, LOG_TYPES)
opcode, payload = send_recv(diag, DIAG_NV_READ_F, pack('<H', NV_GNSS_OEM_FEATURE_MASK))
def try_setup_logs(diag, log_types):
for _ in range(5):
try:
setup_logs(diag, log_types)
break
except Exception:
pass
def disable_logs(sig, frame):
os.system("mmcli -m 0 --location-disable-gps-raw --location-disable-gps-nmea")
cloudlog.warning("rawgpsd: shutting down")
try_setup_logs(diag, [])
cloudlog.warning("rawgpsd: logs disabled")
sys.exit(0)
signal.signal(signal.SIGINT, disable_logs)
try_setup_logs(diag, log_types)
cloudlog.warning("rawgpsd: setup logs done")
# disable DPO power savings for more accuracy # disable DPO power savings for more accuracy
os.system("mmcli -m 0 --command='AT+QGPSCFG=\"dpoenable\",0'") mmcli("--command='AT+QGPSCFG=\"dpoenable\",0'")
os.system("mmcli -m 0 --location-enable-gps-raw --location-enable-gps-nmea") # don't automatically turn on GNSS on powerup
mmcli("--command='AT+QGPSCFG=\"autogps\",0'")
mmcli("--location-enable-gps-raw --location-enable-gps-nmea")
# enable OEMDRE mode # enable OEMDRE mode
DIAG_SUBSYS_CMD_F = 75 DIAG_SUBSYS_CMD_F = 75
@ -128,31 +123,78 @@ def main() -> NoReturn:
GPSDIAG_OEM_DRE_ON = 1 GPSDIAG_OEM_DRE_ON = 1
# gpsdiag_OemControlReqType # gpsdiag_OemControlReqType
opcode, payload = send_recv(diag, DIAG_SUBSYS_CMD_F, pack('<BHBBIIII', send_recv(diag, DIAG_SUBSYS_CMD_F, pack('<BHBBIIII',
DIAG_SUBSYS_GPS, # Subsystem Id DIAG_SUBSYS_GPS, # Subsystem Id
CGPS_DIAG_PDAPI_CMD, # Subsystem Command Code CGPS_DIAG_PDAPI_CMD, # Subsystem Command Code
CGPS_OEM_CONTROL, # CGPS Command Code CGPS_OEM_CONTROL, # CGPS Command Code
0, # Version 0, # Version
GPSDIAG_OEMFEATURE_DRE, GPSDIAG_OEMFEATURE_DRE,
GPSDIAG_OEM_DRE_ON, GPSDIAG_OEM_DRE_ON,
0,0 0,0
)) ))
pm = messaging.PubMaster(pub_types) def teardown_quectel(diag):
mmcli("--location-disable-gps-raw --location-disable-gps-nmea")
try_setup_logs(diag, [])
def main() -> NoReturn:
unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True)
unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(gps_measurement_report_sv, True)
unpack_glonass_meas, size_glonass_meas = dict_unpacker(glonass_measurement_report, True)
unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(glonass_measurement_report_sv, True)
unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True)
unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True)
unpack_svpoly, _ = dict_unpacker(oemdre_svpoly_report, True)
unpack_position, _ = dict_unpacker(position_report)
unpack_position, _ = dict_unpacker(position_report)
# wait for ModemManager to come up
cloudlog.warning("waiting for modem to come up")
while True:
ret = subprocess.call("mmcli -m any --timeout 10 --location-status", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True)
if ret == 0:
break
time.sleep(0.1)
# connect to modem
diag = ModemDiag()
def cleanup(sig, frame):
cloudlog.warning(f"caught sig {sig}, disabling quectel gps")
teardown_quectel(diag)
cloudlog.warning("quectel cleanup done")
sys.exit(0)
signal.signal(signal.SIGINT, cleanup)
signal.signal(signal.SIGTERM, cleanup)
setup_quectel(diag)
cloudlog.warning("quectel setup done")
pm = messaging.PubMaster(['qcomGnss', 'gpsLocation'])
while 1: while 1:
opcode, payload = diag.recv() opcode, payload = diag.recv()
assert opcode == DIAG_LOG_F assert opcode == DIAG_LOG_F
(pending_msgs, log_outer_length), inner_log_packet = unpack_from('<BH', payload), payload[calcsize('<BH'):] (pending_msgs, log_outer_length), inner_log_packet = unpack_from('<BH', payload), payload[calcsize('<BH'):]
if pending_msgs > 0: if pending_msgs > 0:
cloudlog.debug("have %d pending messages" % pending_msgs) cloudlog.debug("have %d pending messages" % pending_msgs)
assert log_outer_length == len(inner_log_packet) assert log_outer_length == len(inner_log_packet)
(log_inner_length, log_type, log_time), log_payload = unpack_from('<HHQ', inner_log_packet), inner_log_packet[calcsize('<HHQ'):] (log_inner_length, log_type, log_time), log_payload = unpack_from('<HHQ', inner_log_packet), inner_log_packet[calcsize('<HHQ'):]
assert log_inner_length == len(inner_log_packet) assert log_inner_length == len(inner_log_packet)
if log_type not in log_types:
if log_type not in LOG_TYPES:
continue continue
if DEBUG: if DEBUG:
print("%.4f: got log: %x len %d" % (time.time(), log_type, len(log_payload))) print("%.4f: got log: %x len %d" % (time.time(), log_type, len(log_payload)))
if log_type == LOG_GNSS_OEMDRE_MEASUREMENT_REPORT: if log_type == LOG_GNSS_OEMDRE_MEASUREMENT_REPORT:
msg = messaging.new_message('qcomGnss') msg = messaging.new_message('qcomGnss')
@ -221,7 +263,24 @@ def main() -> NoReturn:
pm.send('gpsLocation', msg) pm.send('gpsLocation', msg)
if log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT:
msg = messaging.new_message('qcomGnss')
dat = unpack_svpoly(log_payload)
dat = relist(dat)
gnss = msg.qcomGnss
gnss.logTs = log_time
gnss.init('drSvPoly')
poly = gnss.drSvPoly
for k,v in dat.items():
if k == "version":
assert v == 2
elif k == "flags":
pass
else:
setattr(poly, k, v)
pm.send('qcomGnss', msg)
elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]:
msg = messaging.new_message('qcomGnss') msg = messaging.new_message('qcomGnss')
gnss = msg.qcomGnss gnss = msg.qcomGnss

@ -56,6 +56,29 @@ oemdre_measurement_report = """
uint8_t source; uint8_t source;
""" """
oemdre_svpoly_report = """
uint8_t version;
uint16_t sv_id;
int8_t frequency_index;
uint8_t flags;
uint16_t iode;
double t0;
double xyz0[3];
double xyzN[9];
float other[4];
float position_uncertainty;
float iono_delay;
float iono_dot;
float sbas_iono_delay;
float sbas_iono_dot;
float tropo_delay;
float elevation;
float elevation_dot;
float elevation_uncertainty;
double velocity_coeff[12];
"""
oemdre_measurement_report_sv = """ oemdre_measurement_report_sv = """
uint8_t sv_id; uint8_t sv_id;
uint8_t unkn; uint8_t unkn;
@ -311,3 +334,21 @@ def dict_unpacker(ss, camelcase = False):
nams = [name_to_camelcase(x) for x in nams] nams = [name_to_camelcase(x) for x in nams]
sz = calcsize(st) sz = calcsize(st)
return lambda x: dict(zip(nams, unpack_from(st, x))), sz return lambda x: dict(zip(nams, unpack_from(st, x))), sz
def relist(dat):
list_keys = set()
for key in dat.keys():
if '[' in key:
list_keys.add(key.split('[')[0])
list_dict = {}
for list_key in list_keys:
list_dict[list_key] = []
i = 0
while True:
key = list_key + f'[{i}]'
if key not in dat:
break
list_dict[list_key].append(dat[key])
del dat[key]
i += 1
return {**dat, **list_dict}

@ -0,0 +1,49 @@
#!/usr/bin/env python3
import json
import time
import unittest
import subprocess
import cereal.messaging as messaging
from system.hardware import TICI
from selfdrive.manager.process_config import managed_processes
class TestRawgpsd(unittest.TestCase):
@classmethod
def setUpClass(cls):
if not TICI:
raise unittest.SkipTest
def tearDown(self):
managed_processes['rawgpsd'].stop()
def test_startup_time(self):
for _ in range(5):
sm = messaging.SubMaster(['qcomGnss'])
managed_processes['rawgpsd'].start()
start_time = time.monotonic()
for __ in range(10):
sm.update(1 * 1000)
if sm.updated['qcomGnss']:
break
assert sm.rcv_frame['qcomGnss'] > 0, "rawgpsd didn't start outputting messages in time"
et = time.monotonic() - start_time
assert et < 5, f"rawgpsd took {et:.1f}s to start"
managed_processes['rawgpsd'].stop()
def test_turns_off_gnss(self):
for s in (0.1, 0.5, 1, 5):
managed_processes['rawgpsd'].start()
time.sleep(s)
managed_processes['rawgpsd'].stop()
ls = subprocess.check_output("mmcli -m any --location-status --output-json", shell=True, encoding='utf-8')
loc_status = json.loads(ls)
assert set(loc_status['modem']['location']['enabled']) <= {'3gpp-lac-ci'}
if __name__ == "__main__":
unittest.main()

@ -41,7 +41,7 @@ def remove_ignored_fields(msg, ignore):
elif isinstance(v, numbers.Number): elif isinstance(v, numbers.Number):
val = 0 val = 0
else: else:
raise NotImplementedError raise NotImplementedError('Error ignoring field')
setattr(attr, keys[-1], val) setattr(attr, keys[-1], val)
return msg.as_reader() return msg.as_reader()

@ -22,7 +22,7 @@ from tools.lib.logreader import LogReader
TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36" TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"
SEGMENT = 0 SEGMENT = 0
MAX_FRAMES = 10 if PC else 1300 MAX_FRAMES = 100 if PC else 1300
SEND_EXTRA_INPUTS = bool(os.getenv("SEND_EXTRA_INPUTS", "0")) SEND_EXTRA_INPUTS = bool(os.getenv("SEND_EXTRA_INPUTS", "0"))
@ -174,7 +174,7 @@ if __name__ == "__main__":
'driverStateV2.dspExecutionTime' 'driverStateV2.dspExecutionTime'
] ]
# TODO this tolerance is absurdly large # TODO this tolerance is absurdly large
tolerance = 5e-1 if PC else None tolerance = 2.0 if PC else None
results: Any = {TEST_ROUTE: {}} results: Any = {TEST_ROUTE: {}}
log_paths: Any = {TEST_ROUTE: {"models": {'ref': BASE_URL + log_fn, 'new': log_fn}}} log_paths: Any = {TEST_ROUTE: {"models": {'ref': BASE_URL + log_fn, 'new': log_fn}}}
results[TEST_ROUTE]["models"] = compare_logs(cmp_log, log_msgs, tolerance=tolerance, ignore_fields=ignore) results[TEST_ROUTE]["models"] = compare_logs(cmp_log, log_msgs, tolerance=tolerance, ignore_fields=ignore)

@ -1 +1 @@
c40319a454840d8a2196ec1227d27b536ee14375 c171250d2cc013b3eca1cda4fb62f3d0dda28d4d

@ -417,6 +417,7 @@ def setup_env(simulation=False, CP=None, cfg=None, controlsState=None):
params.put_bool("DisengageOnAccelerator", True) params.put_bool("DisengageOnAccelerator", True)
params.put_bool("WideCameraOnly", False) params.put_bool("WideCameraOnly", False)
params.put_bool("DisableLogging", False) params.put_bool("DisableLogging", False)
params.put_bool("UbloxAvailable", True)
os.environ["NO_RADAR_SLEEP"] = "1" os.environ["NO_RADAR_SLEEP"] = "1"
os.environ["REPLAY"] = "1" os.environ["REPLAY"] = "1"

@ -1 +1 @@
53079010a5db8105854212157b5ee90029df7b92 f636c68e2b4ed88d3731930cf15b6dee984eb6dd

@ -190,7 +190,7 @@ def migrate_carparams(lr):
return all_msgs return all_msgs
def migrate_sensorEvents(lr): def migrate_sensorEvents(lr, old_logtime=False):
all_msgs = [] all_msgs = []
for msg in lr: for msg in lr:
if msg.which() != 'sensorEventsDEPRECATED': if msg.which() != 'sensorEventsDEPRECATED':
@ -214,6 +214,8 @@ def migrate_sensorEvents(lr):
m = messaging.new_message(sensor_service) m = messaging.new_message(sensor_service)
m.valid = True m.valid = True
if old_logtime:
m.logMonoTime = msg.logMonoTime
m_dat = getattr(m, sensor_service) m_dat = getattr(m, sensor_service)
m_dat.version = evt.version m_dat.version = evt.version

@ -53,6 +53,7 @@ def profile(proc, func, car='toyota'):
msgs = list(LogReader(rlog_url)) * int(os.getenv("LOOP", "1")) msgs = list(LogReader(rlog_url)) * int(os.getenv("LOOP", "1"))
os.environ['FINGERPRINT'] = fingerprint os.environ['FINGERPRINT'] = fingerprint
os.environ['SKIP_FW_QUERY'] = "1"
os.environ['REPLAY'] = "1" os.environ['REPLAY'] = "1"
def run(sm, pm, can_sock): def run(sm, pm, can_sock):

@ -177,7 +177,8 @@ def thermald_thread(end_event, hw_queue):
modem_temps=[], modem_temps=[],
) )
temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML) all_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML)
offroad_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML)
should_start_prev = False should_start_prev = False
in_car = False in_car = False
engaged_prev = False engaged_prev = False
@ -239,24 +240,32 @@ def thermald_thread(end_event, hw_queue):
msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness() msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness()
max_comp_temp = temp_filter.update( # this one is only used for offroad
max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC)) temp_sources = [
) msg.deviceState.memoryTempC,
max(msg.deviceState.cpuTempC),
max(msg.deviceState.gpuTempC),
]
offroad_comp_temp = offroad_temp_filter.update(max(temp_sources))
# this drives the thermal status while onroad
temp_sources.append(max(msg.deviceState.pmicTempC))
all_comp_temp = all_temp_filter.update(max(temp_sources))
if fan_controller is not None: if fan_controller is not None:
msg.deviceState.fanSpeedPercentDesired = fan_controller.update(max_comp_temp, onroad_conditions["ignition"]) msg.deviceState.fanSpeedPercentDesired = fan_controller.update(all_comp_temp, onroad_conditions["ignition"])
is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5)) is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5))
if is_offroad_for_5_min and max_comp_temp > OFFROAD_DANGER_TEMP: if is_offroad_for_5_min and offroad_comp_temp > OFFROAD_DANGER_TEMP:
# If device is offroad we want to cool down before going onroad # If device is offroad we want to cool down before going onroad
# since going onroad increases load and can make temps go over 107 # since going onroad increases load and can make temps go over 107
thermal_status = ThermalStatus.danger thermal_status = ThermalStatus.danger
else: else:
current_band = THERMAL_BANDS[thermal_status] current_band = THERMAL_BANDS[thermal_status]
band_idx = list(THERMAL_BANDS.keys()).index(thermal_status) band_idx = list(THERMAL_BANDS.keys()).index(thermal_status)
if current_band.min_temp is not None and max_comp_temp < current_band.min_temp: if current_band.min_temp is not None and all_comp_temp < current_band.min_temp:
thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1] thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1]
elif current_band.max_temp is not None and max_comp_temp > current_band.max_temp: elif current_band.max_temp is not None and all_comp_temp > current_band.max_temp:
thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1] thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1]
# **** starting logic **** # **** starting logic ****
@ -269,6 +278,7 @@ def thermald_thread(end_event, hw_queue):
startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate") startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate")
startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall") startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall")
startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version
startup_conditions["offroad_min_time"] = (not started_seen) or ((off_ts is not None) and (sec_since_boot() - off_ts) > 5.)
# with 2% left, we killall, otherwise the phone will take a long time to boot # with 2% left, we killall, otherwise the phone will take a long time to boot
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2 startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
@ -329,7 +339,8 @@ def thermald_thread(end_event, hw_queue):
started_seen = True started_seen = True
else: else:
if onroad_conditions["ignition"] and (startup_conditions != startup_conditions_prev): if onroad_conditions["ignition"] and (startup_conditions != startup_conditions_prev):
cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions) cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions, error=True)
startup_conditions_prev = startup_conditions.copy()
started_ts = None started_ts = None
if off_ts is None: if off_ts is None:
@ -363,7 +374,6 @@ def thermald_thread(end_event, hw_queue):
pm.send("deviceState", msg) pm.send("deviceState", msg)
should_start_prev = should_start should_start_prev = should_start
startup_conditions_prev = startup_conditions.copy()
# Log to statsd # Log to statsd
statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent) statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent)

@ -62,47 +62,6 @@ def get_tombstones():
return files return files
def report_tombstone_android(fn):
f_size = os.path.getsize(fn)
if f_size > MAX_SIZE:
cloudlog.error(f"Tombstone {fn} too big, {f_size}. Skipping...")
return
with open(fn, encoding='ISO-8859-1') as f:
contents = f.read()
message = " ".join(contents.split('\n')[5:7])
# Cut off pid/tid, since that varies per run
name_idx = message.find('name')
if name_idx >= 0:
message = message[name_idx:]
executable = ""
start_exe_idx = message.find('>>> ')
end_exe_idx = message.find(' <<<')
if start_exe_idx >= 0 and end_exe_idx >= 0:
executable = message[start_exe_idx + 4:end_exe_idx]
# Cut off fault addr
fault_idx = message.find(', fault addr')
if fault_idx >= 0:
message = message[:fault_idx]
sentry.report_tombstone(fn, message, contents)
# Copy crashlog to upload folder
clean_path = executable.replace('./', '').replace('/', '_')
date = datetime.datetime.now().strftime("%Y-%m-%d--%H-%M-%S")
new_fn = f"{date}_{get_commit(default='nocommit')[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN]
crashlog_dir = os.path.join(ROOT, "crash")
mkdirs_exists_ok(crashlog_dir)
shutil.copy(fn, os.path.join(crashlog_dir, new_fn))
def report_tombstone_apport(fn): def report_tombstone_apport(fn):
f_size = os.path.getsize(fn) f_size = os.path.getsize(fn)
if f_size > MAX_SIZE: if f_size > MAX_SIZE:
@ -199,7 +158,7 @@ def main() -> NoReturn:
if fn.endswith(".crash"): if fn.endswith(".crash"):
report_tombstone_apport(fn) report_tombstone_apport(fn)
else: else:
report_tombstone_android(fn) cloudlog.error(f"unknown crash type: {fn}")
except Exception: except Exception:
cloudlog.exception(f"Error reporting tombstone {fn}") cloudlog.exception(f"Error reporting tombstone {fn}")

@ -32,6 +32,7 @@ if maps:
qt_env['CPPDEFINES'] += ["ENABLE_MAPS"] qt_env['CPPDEFINES'] += ["ENABLE_MAPS"]
widgets = qt_env.Library("qt_widgets", widgets_src, LIBS=base_libs) widgets = qt_env.Library("qt_widgets", widgets_src, LIBS=base_libs)
Export('widgets')
qt_libs = [widgets, qt_util] + base_libs qt_libs = [widgets, qt_util] + base_libs
# build assets # build assets

@ -115,7 +115,9 @@ MapPanel::MapPanel(QWidget* parent) : QWidget(parent) {
stack->addWidget(main_widget); stack->addWidget(main_widget);
stack->addWidget(no_prime_widget); stack->addWidget(no_prime_widget);
stack->setCurrentIndex(uiState()->prime_type ? 0 : 1); connect(uiState(), &UIState::primeTypeChanged, [=](int prime_type) {
stack->setCurrentIndex(prime_type ? 0 : 1);
});
QVBoxLayout *wrapper = new QVBoxLayout(this); QVBoxLayout *wrapper = new QVBoxLayout(this);
wrapper->addWidget(stack); wrapper->addWidget(stack);
@ -194,7 +196,6 @@ void MapPanel::parseResponse(const QString &response, bool success) {
} }
void MapPanel::refresh() { void MapPanel::refresh() {
stack->setCurrentIndex(uiState()->prime_type ? 0 : 1);
if (cur_destinations == prev_destinations) return; if (cur_destinations == prev_destinations) return;
QJsonDocument doc = QJsonDocument::fromJson(cur_destinations.trimmed().toUtf8()); QJsonDocument doc = QJsonDocument::fromJson(cur_destinations.trimmed().toUtf8());

@ -8,9 +8,11 @@
#include <QPainter> #include <QPainter>
#include <QScrollBar> #include <QScrollBar>
#include "selfdrive/ui/ui.h"
#include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/qt/util.h"
#include "selfdrive/ui/qt/qt_window.h" #include "selfdrive/ui/qt/qt_window.h"
#include "selfdrive/ui/qt/widgets/controls.h" #include "selfdrive/ui/qt/widgets/controls.h"
#include "selfdrive/ui/qt/widgets/prime.h"
#include "selfdrive/ui/qt/widgets/scrollview.h" #include "selfdrive/ui/qt/widgets/scrollview.h"
@ -150,7 +152,7 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid
// Roaming toggle // Roaming toggle
const bool roamingEnabled = params.getBool("GsmRoaming"); const bool roamingEnabled = params.getBool("GsmRoaming");
ToggleControl *roamingToggle = new ToggleControl(tr("Enable Roaming"), "", "", roamingEnabled); roamingToggle = new ToggleControl(tr("Enable Roaming"), "", "", roamingEnabled);
QObject::connect(roamingToggle, &ToggleControl::toggleFlipped, [=](bool state) { QObject::connect(roamingToggle, &ToggleControl::toggleFlipped, [=](bool state) {
params.putBool("GsmRoaming", state); params.putBool("GsmRoaming", state);
wifi->updateGsmSettings(state, QString::fromStdString(params.get("GsmApn")), params.getBool("GsmMetered")); wifi->updateGsmSettings(state, QString::fromStdString(params.get("GsmApn")), params.getBool("GsmMetered"));
@ -158,7 +160,7 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid
list->addItem(roamingToggle); list->addItem(roamingToggle);
// APN settings // APN settings
ButtonControl *editApnButton = new ButtonControl(tr("APN Setting"), tr("EDIT")); editApnButton = new ButtonControl(tr("APN Setting"), tr("EDIT"));
connect(editApnButton, &ButtonControl::clicked, [=]() { connect(editApnButton, &ButtonControl::clicked, [=]() {
const QString cur_apn = QString::fromStdString(params.get("GsmApn")); const QString cur_apn = QString::fromStdString(params.get("GsmApn"));
QString apn = InputDialog::getText(tr("Enter APN"), this, tr("leave blank for automatic configuration"), false, -1, cur_apn).trimmed(); QString apn = InputDialog::getText(tr("Enter APN"), this, tr("leave blank for automatic configuration"), false, -1, cur_apn).trimmed();
@ -174,7 +176,7 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid
// Metered toggle // Metered toggle
const bool metered = params.getBool("GsmMetered"); const bool metered = params.getBool("GsmMetered");
ToggleControl *meteredToggle = new ToggleControl(tr("Cellular Metered"), tr("Prevent large data uploads when on a metered connection"), "", metered); meteredToggle = new ToggleControl(tr("Cellular Metered"), tr("Prevent large data uploads when on a metered connection"), "", metered);
QObject::connect(meteredToggle, &SshToggle::toggleFlipped, [=](bool state) { QObject::connect(meteredToggle, &SshToggle::toggleFlipped, [=](bool state) {
params.putBool("GsmMetered", state); params.putBool("GsmMetered", state);
wifi->updateGsmSettings(params.getBool("GsmRoaming"), QString::fromStdString(params.get("GsmApn")), state); wifi->updateGsmSettings(params.getBool("GsmRoaming"), QString::fromStdString(params.get("GsmApn")), state);
@ -184,6 +186,13 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid
// Set initial config // Set initial config
wifi->updateGsmSettings(roamingEnabled, QString::fromStdString(params.get("GsmApn")), metered); wifi->updateGsmSettings(roamingEnabled, QString::fromStdString(params.get("GsmApn")), metered);
connect(uiState(), &UIState::primeTypeChanged, this, [=](int prime_type) {
bool gsmVisible = prime_type == PrimeType::NONE || prime_type == PrimeType::LITE;
roamingToggle->setVisible(gsmVisible);
editApnButton->setVisible(gsmVisible);
meteredToggle->setVisible(gsmVisible);
});
main_layout->addWidget(new ScrollView(list, this)); main_layout->addWidget(new ScrollView(list, this));
main_layout->addStretch(1); main_layout->addStretch(1);
} }

@ -40,6 +40,9 @@ public:
private: private:
LabelControl* ipLabel; LabelControl* ipLabel;
ToggleControl* tetheringToggle; ToggleControl* tetheringToggle;
ToggleControl* roamingToggle;
ButtonControl* editApnButton;
ToggleControl* meteredToggle;
WifiManager* wifi = nullptr; WifiManager* wifi = nullptr;
Params params; Params params;

@ -415,16 +415,16 @@ void WifiManager::addTetheringConnection() {
} }
void WifiManager::tetheringActivated(QDBusPendingCallWatcher *call) { void WifiManager::tetheringActivated(QDBusPendingCallWatcher *call) {
int prime_type = uiState()->prime_type; int prime_type = uiState()->prime_type;
int ipv4_forward = (prime_type == PrimeType::NONE || prime_type == PrimeType::LITE); int ipv4_forward = (prime_type == PrimeType::NONE || prime_type == PrimeType::LITE);
if (!ipv4_forward) { if (!ipv4_forward) {
QTimer::singleShot(5000, this, [=] { QTimer::singleShot(5000, this, [=] {
qWarning() << "net.ipv4.ip_forward = 0"; qWarning() << "net.ipv4.ip_forward = 0";
std::system("sudo sysctl net.ipv4.ip_forward=0"); std::system("sudo sysctl net.ipv4.ip_forward=0");
}); });
} }
call->deleteLater(); call->deleteLater();
} }
void WifiManager::setTetheringEnabled(bool enabled) { void WifiManager::setTetheringEnabled(bool enabled) {

@ -312,11 +312,7 @@ void SetupWidget::replyFinished(const QString &response, bool success) {
QJsonObject json = doc.object(); QJsonObject json = doc.object();
int prime_type = json["prime_type"].toInt(); int prime_type = json["prime_type"].toInt();
uiState()->prime_type = prime_type;
if (uiState()->prime_type != prime_type) {
uiState()->prime_type = prime_type;
Params().put("PrimeType", std::to_string(prime_type));
}
if (!json["is_paired"].toBool()) { if (!json["is_paired"].toBool()) {
mainLayout->setCurrentIndex(0); mainLayout->setCurrentIndex(0);

@ -201,6 +201,13 @@ void UIState::updateStatus() {
started_prev = scene.started; started_prev = scene.started;
emit offroadTransition(!scene.started); emit offroadTransition(!scene.started);
} }
// Handle prime type change
if (prime_type != prime_type_prev) {
prime_type_prev = prime_type;
emit primeTypeChanged(prime_type);
Params().put("PrimeType", std::to_string(prime_type));
}
} }
UIState::UIState(QObject *parent) : QObject(parent) { UIState::UIState(QObject *parent) : QObject(parent) {

@ -126,7 +126,7 @@ public:
UIScene scene = {}; UIScene scene = {};
bool awake; bool awake;
int prime_type = 0; int prime_type;
QString language; QString language;
QTransform car_space_transform; QTransform car_space_transform;
@ -135,6 +135,7 @@ public:
signals: signals:
void uiUpdate(const UIState &s); void uiUpdate(const UIState &s);
void offroadTransition(bool offroad); void offroadTransition(bool offroad);
void primeTypeChanged(int prime_type);
private slots: private slots:
void update(); void update();
@ -142,6 +143,7 @@ private slots:
private: private:
QTimer *timer; QTimer *timer;
bool started_prev = false; bool started_prev = false;
int prime_type_prev = -1;
}; };
UIState *uiState(); UIState *uiState();
@ -155,9 +157,6 @@ public:
Device(QObject *parent = 0); Device(QObject *parent = 0);
private: private:
// auto brightness
const float accel_samples = 5*UI_FREQ;
bool awake = false; bool awake = false;
int interactive_timeout = 0; int interactive_timeout = 0;
bool ignition_on = false; bool ignition_on = false;

@ -327,7 +327,7 @@ class Updater:
self._has_internet = False self._has_internet = False
setup_git_options(OVERLAY_MERGED) setup_git_options(OVERLAY_MERGED)
output = run(["git", "ls-remote", "--heads", "origin"], OVERLAY_MERGED) output = run(["git", "ls-remote", "--heads"], OVERLAY_MERGED)
self.branches = defaultdict(lambda: None) self.branches = defaultdict(lambda: None)
for line in output.split('\n'): for line in output.split('\n'):
@ -363,7 +363,6 @@ class Updater:
cloudlog.info("git reset in progress") cloudlog.info("git reset in progress")
cmds = [ cmds = [
["git", "checkout", "--force", "--no-recurse-submodules", "-B", branch, "FETCH_HEAD"], ["git", "checkout", "--force", "--no-recurse-submodules", "-B", branch, "FETCH_HEAD"],
["git", "branch", "--set-upstream-to", f"origin/{branch}"],
["git", "reset", "--hard"], ["git", "reset", "--hard"],
["git", "clean", "-xdff"], ["git", "clean", "-xdff"],
["git", "submodule", "init"], ["git", "submodule", "init"],

@ -41,9 +41,9 @@
}, },
{ {
"name": "system", "name": "system",
"url": "https://commadist.azureedge.net/agnosupdate/system-9db38e27c912005472f3ac02be336af4f82307295118b6db22921479d44a941d.img.xz", "url": "https://commadist.azureedge.net/agnosupdate/system-b40b08912576bb972907acba7c201c1399395cbc0cba06ce6e5e3f70ab565cb5.img.xz",
"hash": "05e7ce440b33721b020a249043d9568a5898080e26411ca250fb330ad2e5ed8e", "hash": "6e8fbcc21a265f7f58062abce7675dc05540e2b60cee2df56992a151ba64936f",
"hash_raw": "9db38e27c912005472f3ac02be336af4f82307295118b6db22921479d44a941d", "hash_raw": "b40b08912576bb972907acba7c201c1399395cbc0cba06ce6e5e3f70ab565cb5",
"size": 10737418240, "size": 10737418240,
"sparse": true, "sparse": true,
"full_check": false, "full_check": false,

@ -480,7 +480,7 @@ class Tici(HardwareBase):
# blue prime config # blue prime config
if sim_id.startswith('8901410'): if sim_id.startswith('8901410'):
os.system('mmcli -m 0 --3gpp-set-initial-eps-bearer-settings="apn=Broadband"') os.system('mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn=Broadband"')
def get_networks(self): def get_networks(self):
r = {} r = {}

@ -20,7 +20,7 @@ class Proc:
PROCS = [ PROCS = [
Proc('camerad', 2.15), Proc('camerad', 2.15),
Proc('modeld', 1.0, atol=0.15), Proc('modeld', 1.15, atol=0.2),
Proc('dmonitoringmodeld', 0.35), Proc('dmonitoringmodeld', 0.35),
Proc('encoderd', 0.23), Proc('encoderd', 0.23),
] ]

@ -95,7 +95,7 @@ std::optional<ProcStat> procStat(std::string stat) {
.num_threads = stol(v[StatPos::num_threads - 1]), .num_threads = stol(v[StatPos::num_threads - 1]),
.starttime = stoull(v[StatPos::starttime - 1]), .starttime = stoull(v[StatPos::starttime - 1]),
.vms = stoul(v[StatPos::vsize - 1]), .vms = stoul(v[StatPos::vsize - 1]),
.rss = stoul(v[StatPos::rss - 1]), .rss = stol(v[StatPos::rss - 1]),
.processor = stoi(v[StatPos::processor - 1]), .processor = stoi(v[StatPos::processor - 1]),
}; };
return p; return p;

@ -20,8 +20,8 @@ struct ProcCache {
struct ProcStat { struct ProcStat {
int pid, ppid, processor; int pid, ppid, processor;
char state; char state;
long cutime, cstime, priority, nice, num_threads; long cutime, cstime, priority, nice, num_threads, rss;
unsigned long utime, stime, vms, rss; unsigned long utime, stime, vms;
unsigned long long starttime; unsigned long long starttime;
std::string name; std::string name;
}; };

@ -1 +1 @@
Subproject commit 2e9b7637b3c3c8895fda9f964215db3a35fe3441 Subproject commit 870ea766eec7a38d7d590c81436f15271ba2667e

@ -0,0 +1,4 @@
moc_*
*.moc
_cabana

@ -0,0 +1,9 @@
# Cabana
<img src="https://cabana.comma.ai/img/cabana.jpg" width="640" height="267" />
Cabana is a tool developed to view raw CAN data. One use for this is creating and editing [CAN Dictionaries](http://socialledge.com/sjsu/index.php/DBC_Format) (DBC files), and the tool provides direct integration with [commaai/opendbc](https://github.com/commaai/opendbc) (a collection of DBC files), allowing you to load the DBC files direct from source, and save to your fork. In addition, you can load routes from [comma connect](https://connect.comma.ai).
## Usage Instructions
See [openpilot wiki](https://github.com/commaai/openpilot/wiki/Cabana)

@ -0,0 +1,20 @@
import os
Import('env', 'qt_env', 'arch', 'common', 'messaging', 'visionipc',
'cereal', 'transformations', 'widgets', 'opendbc')
base_frameworks = qt_env['FRAMEWORKS']
base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq',
'capnp', 'kj', 'm', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"]
if arch == "Darwin":
base_frameworks.append('OpenCL')
else:
base_libs.append('OpenCL')
qt_libs = ['qt_util', 'Qt5Charts'] + base_libs
if arch in ['x86_64', 'Darwin'] and GetOption('extras'):
qt_env['CXXFLAGS'] += ["-Wno-deprecated-declarations"]
Import('replay_lib')
cabana_libs = [widgets, cereal, messaging, visionipc, replay_lib, opendbc,'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv'] + qt_libs
qt_env.Program('_cabana', ['cabana.cc', 'mainwin.cc', 'chartswidget.cc', 'videowidget.cc', 'signaledit.cc', 'parser.cc', 'messageswidget.cc', 'detailwidget.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks)

@ -0,0 +1,4 @@
#!/bin/sh
cd "$(dirname "$0")"
export LD_LIBRARY_PATH="../../opendbc/can:$LD_LIBRARY_PATH"
exec ./_cabana "$1"

@ -0,0 +1,32 @@
#include <QApplication>
#include <QCommandLineParser>
#include "selfdrive/ui/qt/util.h"
#include "tools/cabana/mainwin.h"
const QString DEMO_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36";
int main(int argc, char *argv[]) {
initApp(argc, argv);
QApplication app(argc, argv);
QCommandLineParser cmd_parser;
cmd_parser.addHelpOption();
cmd_parser.addPositionalArgument("route", "the drive to replay. find your drives at connect.comma.ai");
cmd_parser.addOption({"demo", "use a demo route instead of providing your own"});
cmd_parser.addOption({"data_dir", "local directory with routes", "data_dir"});
cmd_parser.process(app);
const QStringList args = cmd_parser.positionalArguments();
if (args.empty() && !cmd_parser.isSet("demo")) {
cmd_parser.showHelp();
}
const QString route = args.empty() ? DEMO_ROUTE : args.first();
Parser p(&app);
if (!p.loadRoute(route, cmd_parser.value("data_dir"), true)) {
return 0;
}
MainWindow w;
w.showMaximized();
return app.exec();
}

@ -0,0 +1,271 @@
#include "tools/cabana/chartswidget.h"
#include <QGraphicsLayout>
#include <QLabel>
#include <QRubberBand>
#include <QStackedLayout>
#include <QtCharts/QLineSeries>
#include <QtCharts/QValueAxis>
int64_t get_raw_value(uint8_t *data, size_t data_size, const Signal &sig) {
int64_t ret = 0;
int i = sig.msb / 8;
int bits = sig.size;
while (i >= 0 && i < data_size && bits > 0) {
int lsb = (int)(sig.lsb / 8) == i ? sig.lsb : i * 8;
int msb = (int)(sig.msb / 8) == i ? sig.msb : (i + 1) * 8 - 1;
int size = msb - lsb + 1;
uint64_t d = (data[i] >> (lsb - (i * 8))) & ((1ULL << size) - 1);
ret |= d << (bits - size);
bits -= size;
i = sig.is_little_endian ? i - 1 : i + 1;
}
return ret;
}
// ChartsWidget
ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) {
QVBoxLayout *main_layout = new QVBoxLayout(this);
main_layout->setContentsMargins(0, 0, 0, 0);
// title bar
title_bar = new QWidget(this);
QHBoxLayout *title_layout = new QHBoxLayout(title_bar);
title_label = new QLabel(tr("Charts"));
title_layout->addWidget(title_label);
title_layout->addStretch();
reset_zoom_btn = new QPushButton("", this);
reset_zoom_btn->setVisible(false);
reset_zoom_btn->setFixedSize(30, 30);
reset_zoom_btn->setToolTip(tr("Reset zoom (drag on chart to zoom X-Axis)"));
title_layout->addWidget(reset_zoom_btn);
remove_all_btn = new QPushButton(tr(""));
remove_all_btn->setVisible(false);
remove_all_btn->setToolTip(tr("Remove all charts"));
remove_all_btn->setFixedSize(30, 30);
title_layout->addWidget(remove_all_btn);
dock_btn = new QPushButton();
dock_btn->setFixedSize(30, 30);
updateDockButton();
title_layout->addWidget(dock_btn);
main_layout->addWidget(title_bar, 0, Qt::AlignTop);
// charts
QWidget *charts_container = new QWidget(this);
QVBoxLayout *charts_main = new QVBoxLayout(charts_container);
charts_layout = new QVBoxLayout();
charts_main->addLayout(charts_layout);
charts_main->addStretch();
QScrollArea *charts_scroll = new QScrollArea(this);
charts_scroll->setWidgetResizable(true);
charts_scroll->setWidget(charts_container);
charts_scroll->setFrameShape(QFrame::NoFrame);
charts_scroll->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
main_layout->addWidget(charts_scroll);
QObject::connect(parser, &Parser::showPlot, this, &ChartsWidget::addChart);
QObject::connect(parser, &Parser::hidePlot, this, &ChartsWidget::removeChart);
QObject::connect(parser, &Parser::signalRemoved, this, &ChartsWidget::removeChart);
QObject::connect(reset_zoom_btn, &QPushButton::clicked, parser, &Parser::resetRange);
QObject::connect(remove_all_btn, &QPushButton::clicked, this, &ChartsWidget::removeAll);
QObject::connect(dock_btn, &QPushButton::clicked, [=]() {
emit dock(!docking);
docking = !docking;
updateDockButton();
});
}
void ChartsWidget::updateDockButton() {
dock_btn->setText(docking ? "" : "");
dock_btn->setToolTip(docking ? tr("Undock charts") : tr("Dock charts"));
}
void ChartsWidget::addChart(const QString &id, const QString &sig_name) {
const QString char_name = id + sig_name;
if (charts.find(char_name) == charts.end()) {
auto chart = new ChartWidget(id, sig_name, this);
charts_layout->insertWidget(0, chart);
charts[char_name] = chart;
}
remove_all_btn->setVisible(true);
reset_zoom_btn->setVisible(true);
title_label->setText(tr("Charts (%1)").arg(charts.size()));
}
void ChartsWidget::removeChart(const QString &id, const QString &sig_name) {
if (auto it = charts.find(id + sig_name); it != charts.end()) {
it->second->deleteLater();
charts.erase(it);
if (charts.empty()) {
remove_all_btn->setVisible(false);
reset_zoom_btn->setVisible(false);
}
}
title_label->setText(tr("Charts (%1)").arg(charts.size()));
}
void ChartsWidget::removeAll() {
for (auto [_, chart] : charts)
chart->deleteLater();
charts.clear();
remove_all_btn->setVisible(false);
reset_zoom_btn->setVisible(false);
}
// ChartWidget
ChartWidget::ChartWidget(const QString &id, const QString &sig_name, QWidget *parent) : id(id), sig_name(sig_name), QWidget(parent) {
QStackedLayout *stacked = new QStackedLayout(this);
stacked->setStackingMode(QStackedLayout::StackAll);
QWidget *chart_widget = new QWidget(this);
QVBoxLayout *chart_layout = new QVBoxLayout(chart_widget);
chart_layout->setSpacing(0);
chart_layout->setContentsMargins(0, 0, 0, 0);
QWidget *header = new QWidget(this);
header->setStyleSheet("background-color:white");
QHBoxLayout *header_layout = new QHBoxLayout(header);
header_layout->setContentsMargins(11, 11, 11, 0);
QLabel *title = new QLabel(tr("%1 %2").arg(parser->getMsg(id)->name.c_str()).arg(id));
header_layout->addWidget(title);
header_layout->addStretch();
QPushButton *remove_btn = new QPushButton("", this);
remove_btn->setFixedSize(30, 30);
remove_btn->setToolTip(tr("Remove chart"));
QObject::connect(remove_btn, &QPushButton::clicked, [=]() {
emit parser->hidePlot(id, sig_name);
});
header_layout->addWidget(remove_btn);
chart_layout->addWidget(header);
QLineSeries *series = new QLineSeries();
series->setUseOpenGL(true);
auto chart = new QChart();
chart->setTitle(sig_name);
chart->addSeries(series);
chart->createDefaultAxes();
chart->legend()->hide();
QFont font;
font.setBold(true);
chart->setTitleFont(font);
chart->setMargins({0, 0, 0, 0});
chart->layout()->setContentsMargins(0, 0, 0, 0);
QObject::connect(dynamic_cast<QValueAxis *>(chart->axisX()), &QValueAxis::rangeChanged, parser, &Parser::setRange);
chart_view = new QChartView(chart);
chart_view->setFixedHeight(300);
chart_view->setRenderHint(QPainter::Antialiasing);
chart_view->setRubberBand(QChartView::HorizontalRubberBand);
if (auto rubber = chart_view->findChild<QRubberBand *>()) {
QPalette pal;
pal.setBrush(QPalette::Base, QColor(0, 0, 0, 80));
rubber->setPalette(pal);
}
chart_layout->addWidget(chart_view);
chart_layout->addStretch();
stacked->addWidget(chart_widget);
line_marker = new LineMarker(this);
stacked->addWidget(line_marker);
line_marker->setAttribute(Qt::WA_TransparentForMouseEvents, true);
line_marker->raise();
setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
QObject::connect(parser, &Parser::updated, this, &ChartWidget::updateState);
QObject::connect(parser, &Parser::rangeChanged, this, &ChartWidget::rangeChanged);
QObject::connect(parser, &Parser::eventsMerged, this, &ChartWidget::updateSeries);
updateSeries();
}
void ChartWidget::updateState() {
auto chart = chart_view->chart();
auto axis_x = dynamic_cast<QValueAxis *>(chart->axisX());
int x = chart->plotArea().left() + chart->plotArea().width() * (parser->currentSec() - axis_x->min()) / (axis_x->max() - axis_x->min());
if (line_marker_x != x) {
line_marker->setX(x);
line_marker_x = x;
}
}
void ChartWidget::updateSeries() {
const Signal *sig = parser->getSig(id, sig_name);
auto events = parser->replay->events();
if (!sig || !events) return;
auto l = id.split(':');
int bus = l[0].toInt();
uint32_t address = l[1].toUInt(nullptr, 16);
vals.clear();
vals.reserve(3 * 60 * 100);
uint64_t route_start_time = parser->replay->routeStartTime();
for (auto &evt : *events) {
if (evt->which == cereal::Event::Which::CAN) {
for (auto c : evt->event.getCan()) {
if (bus == c.getSrc() && address == c.getAddress()) {
auto dat = c.getDat();
int64_t val = get_raw_value((uint8_t *)dat.begin(), dat.size(), *sig);
if (sig->is_signed) {
val -= ((val >> (sig->size - 1)) & 0x1) ? (1ULL << sig->size) : 0;
}
double value = val * sig->factor + sig->offset;
double ts = (evt->mono_time - route_start_time) / (double)1e9; // seconds
vals.push_back({ts, value});
}
}
}
}
QLineSeries *series = (QLineSeries *)chart_view->chart()->series()[0];
series->replace(vals);
auto [begin, end] = parser->range();
chart_view->chart()->axisX()->setRange(begin, end);
updateAxisY();
}
void ChartWidget::rangeChanged(qreal min, qreal max) {
auto axis_x = dynamic_cast<QValueAxis *>(chart_view->chart()->axisX());
if (axis_x->min() != min || axis_x->max() != max) {
axis_x->setRange(min, max);
}
updateAxisY();
}
// auto zoom on yaxis
void ChartWidget::updateAxisY() {
const auto axis_x = dynamic_cast<QValueAxis *>(chart_view->chart()->axisX());
// vals is a sorted list
auto begin = std::lower_bound(vals.begin(), vals.end(), axis_x->min(), [](auto &p, double x) { return p.x() < x; });
if (begin == vals.end())
return;
auto end = std::upper_bound(vals.begin(), vals.end(), axis_x->max(), [](double x, auto &p) { return x < p.x(); });
const auto [min, max] = std::minmax_element(begin, end, [](auto &p1, auto &p2) { return p1.y() < p2.y(); });
chart_view->chart()->axisY()->setRange(min->y(), max->y());
}
// LineMarker
void LineMarker::setX(double x) {
x_pos = x;
update();
}
void LineMarker::paintEvent(QPaintEvent *event) {
QPainter p(this);
p.setPen(QPen(Qt::black, 2));
p.drawLine(QPointF{x_pos, 50.}, QPointF{x_pos, (qreal)height() - 11});
}

@ -0,0 +1,77 @@
#pragma once
#include <map>
#include <QLabel>
#include <QPushButton>
#include <QVBoxLayout>
#include <QWidget>
#include <QtCharts/QChartView>
#include <QtCharts/QLineSeries>
#include "tools/cabana/parser.h"
using namespace QtCharts;
class LineMarker : public QWidget {
Q_OBJECT
public:
LineMarker(QWidget *parent) : QWidget(parent) {}
void setX(double x);
private:
void paintEvent(QPaintEvent *event) override;
double x_pos = 0.0;
};
class ChartWidget : public QWidget {
Q_OBJECT
public:
ChartWidget(const QString &id, const QString &sig_name, QWidget *parent);
inline QChart *chart() const { return chart_view->chart(); }
private:
void updateState();
void addData(const CanData &can_data, const Signal &sig);
void updateSeries();
void rangeChanged(qreal min, qreal max);
void updateAxisY();
QString id;
QString sig_name;
QChartView *chart_view = nullptr;
LineMarker *line_marker = nullptr;
double line_marker_x = 0.0;
QList<QPointF> vals;
};
class ChartsWidget : public QWidget {
Q_OBJECT
public:
ChartsWidget(QWidget *parent = nullptr);
void addChart(const QString &id, const QString &sig_name);
void removeChart(const QString &id, const QString &sig_name);
void removeAll();
inline bool hasChart(const QString &id, const QString &sig_name) {
return charts.find(id + sig_name) != charts.end();
}
signals:
void dock(bool floating);
private:
void updateState();
void updateDockButton();
QWidget *title_bar;
QLabel *title_label;
bool docking = true;
QPushButton *dock_btn;
QPushButton *reset_zoom_btn;
QPushButton *remove_all_btn;
QVBoxLayout *charts_layout;
std::map<QString, ChartWidget *> charts;
};

@ -0,0 +1,264 @@
#include "tools/cabana/detailwidget.h"
#include <QDebug>
#include <QDialogButtonBox>
#include <QFormLayout>
#include <QHeaderView>
#include <QTimer>
#include <QVBoxLayout>
#include <bitset>
#include "selfdrive/ui/qt/util.h"
#include "selfdrive/ui/qt/widgets/scrollview.h"
inline const QString &getColor(int i) {
static const QString SIGNAL_COLORS[] = {"#9FE2BF", "#40E0D0", "#6495ED", "#CCCCFF", "#FF7F50", "#FFBF00"};
return SIGNAL_COLORS[i % std::size(SIGNAL_COLORS)];
}
// DetailWidget
DetailWidget::DetailWidget(QWidget *parent) : QWidget(parent) {
QVBoxLayout *main_layout = new QVBoxLayout(this);
name_label = new QLabel(this);
name_label->setStyleSheet("font-weight:bold;");
name_label->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
name_label->setAlignment(Qt::AlignCenter);
main_layout->addWidget(name_label);
// title
QHBoxLayout *title_layout = new QHBoxLayout();
time_label = new QLabel(this);
title_layout->addWidget(time_label);
title_layout->addStretch();
edit_btn = new QPushButton(tr("Edit"), this);
edit_btn->setVisible(false);
title_layout->addWidget(edit_btn);
main_layout->addLayout(title_layout);
// binary view
binary_view = new BinaryView(this);
main_layout->addWidget(binary_view);
// scroll area
QHBoxLayout *signals_layout = new QHBoxLayout();
signals_layout->addWidget(new QLabel(tr("Signals")));
signals_layout->addStretch();
add_sig_btn = new QPushButton(tr("Add signal"), this);
add_sig_btn->setVisible(false);
signals_layout->addWidget(add_sig_btn);
main_layout->addLayout(signals_layout);
QWidget *container = new QWidget(this);
QVBoxLayout *container_layout = new QVBoxLayout(container);
signal_edit_layout = new QVBoxLayout();
signal_edit_layout->setSpacing(2);
container_layout->addLayout(signal_edit_layout);
history_log = new HistoryLog(this);
container_layout->addWidget(history_log);
QScrollArea *scroll = new QScrollArea(this);
scroll->setWidget(container);
scroll->setWidgetResizable(true);
scroll->setFrameShape(QFrame::NoFrame);
scroll->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
scroll->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding);
main_layout->addWidget(scroll);
QObject::connect(add_sig_btn, &QPushButton::clicked, this, &DetailWidget::addSignal);
QObject::connect(edit_btn, &QPushButton::clicked, this, &DetailWidget::editMsg);
QObject::connect(parser, &Parser::updated, this, &DetailWidget::updateState);
}
void DetailWidget::setMsg(const CanData *c) {
can_data = c;
clearLayout(signal_edit_layout);
edit_btn->setVisible(true);
if (auto msg = parser->getMsg(can_data->address)) {
name_label->setText(msg->name.c_str());
add_sig_btn->setVisible(true);
for (int i = 0; i < msg->sigs.size(); ++i) {
signal_edit_layout->addWidget(new SignalEdit(can_data->id, msg->sigs[i], getColor(i)));
}
} else {
name_label->setText(tr("untitled"));
add_sig_btn->setVisible(false);
}
binary_view->setMsg(can_data);
history_log->clear();
}
void DetailWidget::updateState() {
if (!can_data) return;
time_label->setText(QString("time: %1").arg(can_data->ts, 0, 'f', 3));
binary_view->setData(can_data->dat);
history_log->updateState();
}
void DetailWidget::editMsg() {
EditMessageDialog dlg(can_data->id, this);
if (dlg.exec()) {
setMsg(can_data);
}
}
void DetailWidget::addSignal() {
AddSignalDialog dlg(can_data->id, this);
if (dlg.exec()) {
setMsg(can_data);
}
}
// BinaryView
BinaryView::BinaryView(QWidget *parent) {
QVBoxLayout *main_layout = new QVBoxLayout(this);
table = new QTableWidget(this);
table->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
table->horizontalHeader()->hide();
table->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
table->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
main_layout->addWidget(table);
table->setColumnCount(9);
setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed);
}
void BinaryView::setMsg(const CanData *can_data) {
const Msg *msg = parser->getMsg(can_data->address);
int row_count = msg ? msg->size : can_data->dat.size();
table->setRowCount(row_count);
table->setColumnCount(9);
for (int i = 0; i < table->rowCount(); ++i) {
for (int j = 0; j < table->columnCount(); ++j) {
auto item = new QTableWidgetItem();
item->setFlags(item->flags() ^ Qt::ItemIsEditable);
item->setTextAlignment(Qt::AlignCenter);
if (j == 8) {
QFont font;
font.setBold(true);
item->setFont(font);
}
table->setItem(i, j, item);
}
}
if (msg) {
// set background color
for (int i = 0; i < msg->sigs.size(); ++i) {
const auto &sig = msg->sigs[i];
int start = sig.is_little_endian ? sig.start_bit : bigEndianBitIndex(sig.start_bit);
for (int j = start; j <= start + sig.size - 1; ++j) {
table->item(j / 8, j % 8)->setBackground(QColor(getColor(i)));
}
}
}
setFixedHeight(table->rowHeight(0) * table->rowCount() + 25);
}
void BinaryView::setData(const QByteArray &binary) {
std::string s;
for (int j = 0; j < binary.size(); ++j) {
s += std::bitset<8>(binary[j]).to_string();
}
setUpdatesEnabled(false);
char hex[3] = {'\0'};
for (int i = 0; i < binary.size(); ++i) {
for (int j = 0; j < 8; ++j) {
table->item(i, j)->setText(QChar(s[i * 8 + j]));
}
sprintf(&hex[0], "%02X", (unsigned char)binary[i]);
table->item(i, 8)->setText(hex);
}
setUpdatesEnabled(true);
}
// HistoryLog
HistoryLog::HistoryLog(QWidget *parent) : QWidget(parent) {
QVBoxLayout *main_layout = new QVBoxLayout(this);
QLabel *title = new QLabel("TIME BYTES");
main_layout->addWidget(title);
QVBoxLayout *message_layout = new QVBoxLayout();
for (int i = 0; i < std::size(labels); ++i) {
labels[i] = new QLabel();
labels[i]->setVisible(false);
message_layout->addWidget(labels[i]);
}
main_layout->addLayout(message_layout);
main_layout->addStretch();
}
void HistoryLog::updateState() {
int i = 0;
for (; i < parser->history_log.size(); ++i) {
const auto &c = parser->history_log[i];
auto label = labels[i];
label->setVisible(true);
label->setText(QString("%1 %2").arg(c.ts, 0, 'f', 3).arg(toHex(c.dat)));
}
for (; i < std::size(labels); ++i) {
labels[i]->setVisible(false);
}
}
void HistoryLog::clear() {
setUpdatesEnabled(false);
for (auto l : labels) l->setVisible(false);
setUpdatesEnabled(true);
}
// EditMessageDialog
EditMessageDialog::EditMessageDialog(const QString &id, QWidget *parent) : id(id), QDialog(parent) {
setWindowTitle(tr("Edit message"));
QVBoxLayout *main_layout = new QVBoxLayout(this);
QFormLayout *form_layout = new QFormLayout();
form_layout->addRow("ID", new QLabel(id));
auto msg = const_cast<Msg *>(parser->getMsg(id));
name_edit = new QLineEdit(this);
name_edit->setText(msg ? msg->name.c_str() : "untitled");
form_layout->addRow(tr("Name"), name_edit);
size_spin = new QSpinBox(this);
size_spin->setValue(msg ? msg->size : parser->can_msgs[id].dat.size());
form_layout->addRow(tr("Size"), size_spin);
main_layout->addLayout(form_layout);
auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel);
main_layout->addWidget(buttonBox);
connect(buttonBox, &QDialogButtonBox::accepted, this, &EditMessageDialog::save);
connect(buttonBox, &QDialogButtonBox::rejected, this, &QDialog::reject);
}
void EditMessageDialog::save() {
if (size_spin->value() <= 0 || name_edit->text().isEmpty()) return;
if (auto msg = const_cast<Msg *>(parser->getMsg(id))) {
msg->name = name_edit->text().toStdString();
msg->size = size_spin->value();
} else {
Msg m = {};
m.address = Parser::addressFromId(id);
m.name = name_edit->text().toStdString();
m.size = size_spin->value();
parser->addNewMsg(m);
}
QDialog::accept();
}

@ -0,0 +1,70 @@
#pragma once
#include <QDialog>
#include <QLabel>
#include <QPushButton>
#include <QTableWidget>
#include <QVBoxLayout>
#include <QWidget>
#include "opendbc/can/common.h"
#include "opendbc/can/common_dbc.h"
#include "tools/cabana/parser.h"
#include "tools/cabana/signaledit.h"
class HistoryLog : public QWidget {
Q_OBJECT
public:
HistoryLog(QWidget *parent);
void clear();
void updateState();
private:
QLabel *labels[LOG_SIZE] = {};
};
class BinaryView : public QWidget {
Q_OBJECT
public:
BinaryView(QWidget *parent);
void setMsg(const CanData *can_data);
void setData(const QByteArray &binary);
QTableWidget *table;
};
class EditMessageDialog : public QDialog {
Q_OBJECT
public:
EditMessageDialog(const QString &id, QWidget *parent);
protected:
void save();
QLineEdit *name_edit;
QSpinBox *size_spin;
QString id;
};
class DetailWidget : public QWidget {
Q_OBJECT
public:
DetailWidget(QWidget *parent);
void setMsg(const CanData *c);
private:
void updateState();
void addSignal();
void editMsg();
const CanData *can_data = nullptr;
QLabel *name_label, *time_label;
QPushButton *edit_btn, *add_sig_btn;
QVBoxLayout *signal_edit_layout;
HistoryLog *history_log;
BinaryView *binary_view;
};

@ -0,0 +1,58 @@
#include "tools/cabana/mainwin.h"
#include <QHBoxLayout>
#include <QScreen>
#include <QVBoxLayout>
MainWindow::MainWindow() : QWidget() {
QVBoxLayout *main_layout = new QVBoxLayout(this);
QHBoxLayout *h_layout = new QHBoxLayout();
main_layout->addLayout(h_layout);
messages_widget = new MessagesWidget(this);
h_layout->addWidget(messages_widget);
detail_widget = new DetailWidget(this);
detail_widget->setFixedWidth(600);
h_layout->addWidget(detail_widget);
// right widgets
QWidget *right_container = new QWidget(this);
right_container->setFixedWidth(640);
r_layout = new QVBoxLayout(right_container);
video_widget = new VideoWidget(this);
r_layout->addWidget(video_widget, 0, Qt::AlignTop);
charts_widget = new ChartsWidget(this);
r_layout->addWidget(charts_widget);
h_layout->addWidget(right_container);
QObject::connect(messages_widget, &MessagesWidget::msgChanged, detail_widget, &DetailWidget::setMsg);
QObject::connect(charts_widget, &ChartsWidget::dock, this, &MainWindow::dockCharts);
}
void MainWindow::dockCharts(bool dock) {
charts_widget->setUpdatesEnabled(false);
if (dock && floating_window) {
r_layout->addWidget(charts_widget);
floating_window->deleteLater();
floating_window = nullptr;
} else if (!dock && !floating_window) {
floating_window = new QWidget(nullptr);
floating_window->setLayout(new QVBoxLayout());
floating_window->layout()->addWidget(charts_widget);
floating_window->setWindowFlags(Qt::WindowTitleHint | Qt::WindowMaximizeButtonHint | Qt::WindowMinimizeButtonHint);
floating_window->setMinimumSize(QGuiApplication::primaryScreen()->size() / 2);
floating_window->showMaximized();
}
charts_widget->setUpdatesEnabled(true);
}
void MainWindow::closeEvent(QCloseEvent *event) {
if (floating_window)
floating_window->deleteLater();
QWidget::closeEvent(event);
}

@ -0,0 +1,25 @@
#pragma once
#include "tools/cabana/chartswidget.h"
#include "tools/cabana/detailwidget.h"
#include "tools/cabana/messageswidget.h"
#include "tools/cabana/parser.h"
#include "tools/cabana/videowidget.h"
class MainWindow : public QWidget {
Q_OBJECT
public:
MainWindow();
void dockCharts(bool dock);
protected:
void closeEvent(QCloseEvent *event) override;
VideoWidget *video_widget;
MessagesWidget *messages_widget;
DetailWidget *detail_widget;
ChartsWidget *charts_widget;
QWidget *floating_window = nullptr;
QVBoxLayout *r_layout;
};

@ -0,0 +1,93 @@
#include "tools/cabana/messageswidget.h"
#include <QComboBox>
#include <QHeaderView>
#include <QPushButton>
#include <QVBoxLayout>
MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) {
QVBoxLayout *main_layout = new QVBoxLayout(this);
QHBoxLayout *dbc_file_layout = new QHBoxLayout();
QComboBox *combo = new QComboBox(this);
auto dbc_names = get_dbc_names();
for (const auto &name : dbc_names) {
combo->addItem(QString::fromStdString(name));
}
dbc_file_layout->addWidget(combo);
dbc_file_layout->addStretch();
QPushButton *save_btn = new QPushButton(tr("Save DBC"), this);
dbc_file_layout->addWidget(save_btn);
main_layout->addLayout(dbc_file_layout);
filter = new QLineEdit(this);
filter->setPlaceholderText(tr("filter messages"));
main_layout->addWidget(filter);
table_widget = new QTableWidget(this);
table_widget->setSelectionBehavior(QAbstractItemView::SelectRows);
table_widget->setSelectionMode(QAbstractItemView::SingleSelection);
table_widget->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding);
table_widget->setColumnCount(4);
table_widget->setColumnWidth(0, 250);
table_widget->setColumnWidth(1, 80);
table_widget->setColumnWidth(2, 80);
table_widget->setHorizontalHeaderLabels({tr("Name"), tr("ID"), tr("Count"), tr("Bytes")});
table_widget->horizontalHeader()->setStretchLastSection(true);
main_layout->addWidget(table_widget);
QObject::connect(parser, &Parser::updated, this, &MessagesWidget::updateState);
QObject::connect(save_btn, &QPushButton::clicked, [=]() {
// TODO: save DBC to file
});
QObject::connect(combo, &QComboBox::currentTextChanged, [=](const QString &dbc) {
parser->openDBC(dbc);
});
QObject::connect(table_widget, &QTableWidget::itemSelectionChanged, [=]() {
const CanData *c = &(parser->can_msgs[table_widget->selectedItems()[1]->text()]);
parser->setCurrentMsg(c->id);
emit msgChanged(c);
});
// For test purpose
combo->setCurrentText("toyota_nodsu_pt_generated");
}
void MessagesWidget::updateState() {
auto getTableItem = [=](int row, int col) -> QTableWidgetItem * {
auto item = table_widget->item(row, col);
if (!item) {
item = new QTableWidgetItem();
item->setFlags(item->flags() ^ Qt::ItemIsEditable);
table_widget->setItem(row, col, item);
}
return item;
};
table_widget->setRowCount(parser->can_msgs.size());
int i = 0;
QString name, untitled = tr("untitled");
const QString filter_str = filter->text();
for (const auto &[_, c] : parser->can_msgs) {
if (auto msg = parser->getMsg(c.address)) {
name = msg->name.c_str();
} else {
name = untitled;
}
if (!filter_str.isEmpty() && !name.contains(filter_str, Qt::CaseInsensitive)) {
table_widget->hideRow(i++);
continue;
}
getTableItem(i, 0)->setText(name);
getTableItem(i, 1)->setText(c.id);
getTableItem(i, 2)->setText(QString::number(parser->counters[c.id]));
getTableItem(i, 3)->setText(toHex(c.dat));
table_widget->showRow(i);
i++;
}
if (table_widget->currentRow() == -1) {
table_widget->selectRow(0);
}
}

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

Loading…
Cancel
Save