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: >
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 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 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.

@ -1,8 +1,11 @@
blank_issues_enabled: false
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
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
url: https://github.com/commaai/openpilot/wiki
about: Check out our community wiki

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

2
.gitmodules vendored

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

@ -70,3 +70,10 @@ repos:
- --quiet
- --force
- -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 {
phone_steps("tici2", [
["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 loggerd", "python selfdrive/loggerd/tests/test_loggerd.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'])
opendbc = abspath([File('opendbc/can/libdbc.so')])
Export('opendbc')
SConscript(['tools/cabana/SConscript'])
if GetOption('test'):
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},
{"Timezone", PERSISTENT},
{"TrainingVersion", PERSISTENT},
{"UbloxAvailable", PERSISTENT},
{"UpdateAvailable", CLEAR_ON_MANAGER_START},
{"UpdateFailedCount", 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) {
FileLock file_lock(params_path + "/.lock");
std::string path;
for (auto &[key, type] : keys) {
if (type & key_type) {
unlink(getParamPath(key).c_str());
if (key_type == ALL) {
util::remove_files_in_dir(getParamPath());
} else {
for (auto &[key, type] : keys) {
if (type & key_type) {
unlink(getParamPath(key).c_str());
}
}
}

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

@ -16,7 +16,7 @@ cdef extern from "common/params.h":
cdef cppclass c_Params "Params":
c_Params(string) nogil
string get(string, bool) nogil
bool getBool(string) nogil
bool getBool(string, bool) nogil
int remove(string) nogil
int put(string, string) nogil
int putBool(string, bool) nogil
@ -68,11 +68,11 @@ cdef class Params:
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 bool r
with nogil:
r = self.p.getBool(k)
r = self.p.getBool(k, block)
return r
def put(self, key, dat):

@ -143,3 +143,20 @@ TEST_CASE("util::create_directories") {
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;
}
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 fd = HANDLE_EINTR(open(path, flags, mode));
if (fd == -1) {

@ -80,6 +80,7 @@ std::string dir_name(std::string const& path);
// **** file fhelpers *****
std::string read_file(const std::string& fn);
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);
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.
# 204 Supported Cars
# 206 Supported Cars
|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 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|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 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|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|

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

@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1
export VECLIB_MAXIMUM_THREADS=1
if [ -z "$AGNOS_VERSION" ]; then
export AGNOS_VERSION="6"
export AGNOS_VERSION="6.1"
fi
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/translations/*.ts
selfdrive/ui/translations/languages.json
selfdrive/ui/update_translations.py
selfdrive/ui/tests/test_translations.py
selfdrive/ui/qt/*.cc
selfdrive/ui/qt/*.h
@ -572,3 +574,4 @@ tinygrad_repo/tinygrad/ops.py
tinygrad_repo/tinygrad/shapetracker.py
tinygrad_repo/tinygrad/tensor.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)"
export FINGERPRINT="TOYOTA COROLLA TSS2 2019"
export SKIP_FW_QUERY="1"
$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.setInterruptLoad(health.interrupt_load);
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()};

@ -81,7 +81,7 @@ def fingerprint(logcan, sendcan):
skip_fw_query = os.environ.get('SKIP_FW_QUERY', False)
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
bus = 1
@ -95,16 +95,19 @@ def fingerprint(logcan, sendcan):
cloudlog.warning("Using cached CarParams")
vin, vin_rx_addr = cached_params.carVin, 0
car_fw = list(cached_params.carFw)
cached = True
else:
cloudlog.warning("Getting VIN & FW versions")
vin_rx_addr, vin = get_vin(logcan, sendcan, bus)
ecu_rx_addrs = get_present_ecus(logcan, sendcan)
car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs)
cached = False
exact_fw_match, fw_candidates = match_fw_to_car(car_fw)
else:
vin, vin_rx_addr = VIN_UNKNOWN, 0
exact_fw_match, fw_candidates, car_fw = True, set(), []
cached = False
if not is_valid_vin(vin):
cloudlog.event("Malformed VIN", vin=vin, error=True)
@ -165,7 +168,7 @@ def fingerprint(logcan, sendcan):
car_fingerprint = fixed_fingerprint
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)
return car_fingerprint, finger, vin, car_fw, source, exact_match

@ -40,9 +40,12 @@ class CarState(CarStateBase):
else:
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.
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0
ret.brakePressed = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] >= 10
# Some Volt 2016-17 have loose brake pedal push rod retainers which causes the ECM to believe
# that the brake is being intermittently pressed without user interaction.
# 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
if self.CP.transmissionType == TransmissionType.direct:
@ -100,7 +103,7 @@ class CarState(CarStateBase):
def get_can_parser(CP):
signals = [
# sig_name, sig_address
("BrakePedalPosition", "EBCMBrakePedalPosition"),
("BrakePedalPos", "ECMAcceleratorPos"),
("FrontLeftDoor", "BCMDoorBeltStatus"),
("FrontRightDoor", "BCMDoorBeltStatus"),
("RearLeftDoor", "BCMDoorBeltStatus"),
@ -141,7 +144,7 @@ class CarState(CarStateBase):
("ASCMSteeringButton", 33),
("ECMEngineStatus", 100),
("PSCMSteeringAngle", 100),
("EBCMBrakePedalPosition", 100),
("ECMAcceleratorPos", 80),
]
if CP.transmissionType == TransmissionType.direct:

@ -23,11 +23,12 @@ class CarControllerParams:
ADAS_KEEPALIVE_STEP = 100
CAMERA_KEEPALIVE_STEP = 100
# Volt gasbrake lookups
# TODO: These values should be confirmed on non-Volt vehicles
# Volt gas/brake lookups
# 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.
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
# 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_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,
# lower threshold removes some braking deadzone
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]
BRAKE_LOOKUP_BP = [ACCEL_MIN, -0.1]
EV_BRAKE_LOOKUP_BP = [ACCEL_MIN, -1.]
BRAKE_LOOKUP_V = [MAX_BRAKE, 0.]

@ -31,7 +31,6 @@ class CarState(CarStateBase):
self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
self.brake_error = False
self.park_brake = False
self.buttons_counter = 0
# 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))
if not self.CP.openpilotLongitudinalControl:
if self.CP.carFingerprint in FEATURES["use_fca"]:
ret.stockAeb = cp_cruise.vl["FCA11"]["FCA_CmdAct"] != 0
ret.stockFcw = cp_cruise.vl["FCA11"]["CF_VSM_Warn"] == 2
else:
ret.stockAeb = cp_cruise.vl["SCC12"]["AEB_CmdAct"] != 0
ret.stockFcw = cp_cruise.vl["SCC12"]["CF_VSM_Warn"] == 2
aeb_src = "FCA11" if self.CP.carFingerprint in FEATURES["use_fca"] else "SCC12"
aeb_sig = "FCA_CmdAct" if self.CP.carFingerprint in FEATURES["use_fca"] else "AEB_CmdAct"
aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0
aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0
ret.stockFcw = aeb_warning and not aeb_braking
ret.stockAeb = aeb_warning and aeb_braking
if self.CP.enableBsm:
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.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"]
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"],
cp.vl["BLINKERS"]["RIGHT_LAMP"])
@ -295,12 +295,14 @@ class CarState(CarStateBase):
signals += [
("FCA_CmdAct", "FCA11"),
("CF_VSM_Warn", "FCA11"),
("CF_VSM_DecCmdAct", "FCA11"),
]
checks.append(("FCA11", 50))
else:
signals += [
("AEB_CmdAct", "SCC12"),
("CF_VSM_Warn", "SCC12"),
("CF_VSM_DecCmdAct", "SCC12"),
]
if CP.enableBsm:
@ -384,12 +386,14 @@ class CarState(CarStateBase):
signals += [
("FCA_CmdAct", "FCA11"),
("CF_VSM_Warn", "FCA11"),
("CF_VSM_DecCmdAct", "FCA11"),
]
checks.append(("FCA11", 50))
else:
signals += [
("AEB_CmdAct", "SCC12"),
("CF_VSM_Warn", "SCC12"),
("CF_VSM_DecCmdAct", "SCC12"),
]
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
@ -411,6 +415,7 @@ class CarState(CarStateBase):
("STEERING_ANGLE", "STEERING_SENSORS"),
("STEERING_COL_TORQUE", "MDPS"),
("STEERING_OUT_TORQUE", "MDPS"),
("LKA_FAULT", "MDPS"),
("CRUISE_ACTIVE", "SCC1"),
("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
# 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
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))
# note that some vehicles most likely have an alternate checksum/counter definition
# https://github.com/commaai/opendbc/commit/9ddcdb22c4929baf310295e832668e6e7fcfa602
fca11_values = {
# seems to count 2,1,0,3,2,1,0,3,2,1,0,3,2,1,0,repeat...
# (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,
"CR_FCA_Alive": idx % 0xF,
"PAINT1_Status": 1,
"FCA_DrvSetStatus": 1,
"FCA_Status": 1, # AEB disabled
}
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))
return commands

@ -202,12 +202,12 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.indi.timeConstantV = [1.4]
ret.lateralTuning.indi.actuatorEffectivenessBP = [0.]
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.wheelbase = 2.80
ret.steerRatio = 13.75
tire_stiffness_factor = 0.5
if candidate == CAR.KIA_OPTIMA:
if candidate == CAR.KIA_OPTIMA_G4:
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.KIA_STINGER:

@ -85,8 +85,8 @@ class CAR:
KIA_NIRO_EV = "KIA NIRO EV 2020"
KIA_NIRO_PHEV = "KIA NIRO HYBRID 2019"
KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021"
KIA_OPTIMA = "KIA OPTIMA 2016"
KIA_OPTIMA_2019 = "KIA OPTIMA 2019"
KIA_OPTIMA_G4 = "KIA OPTIMA 4TH GEN"
KIA_OPTIMA_G4_FL = "KIA OPTIMA 4TH GEN FACELIFT"
KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019"
KIA_SELTOS = "KIA SELTOS 2021"
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 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_2019: HyundaiCarInfo("Kia Optima 2019-20", harness=Harness.hyundai_g),
CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", harness=Harness.hyundai_b), # TODO: may support 2016, 2018
CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", harness=Harness.hyundai_g),
CAR.KIA_OPTIMA_H: [
HyundaiCarInfo("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control"), # TODO: may support adjacent years
HyundaiCarInfo("Kia Optima Hybrid 2019"),
@ -1127,7 +1127,7 @@ FW_VERSIONS = {
b'\xf1\x87954A22D200\xf1\x81T01950A1 \xf1\000T0190XBL T01950A1 DSP2T16X4X950NS8\r\xfe\x9c\x8b',
],
},
CAR.KIA_OPTIMA: {
CAR.KIA_OPTIMA_G4: {
(Ecu.fwdRadar, 0x7d0, None): [
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',
],
},
CAR.KIA_OPTIMA_2019: {
CAR.KIA_OPTIMA_G4_FL: {
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00JF__ SCC F-CUP 1.00 1.00 96400-D4110 ',
],
@ -1375,7 +1375,7 @@ CHECKSUM = {
FEATURES = {
# which message has the gear
"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},
# 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}
# 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 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_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_OPTIMA: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_2019: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_G4: 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_SELTOS: dbc_dict('hyundai_kia_generic', None),
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)
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.):
self._drain_rx()

@ -20,8 +20,7 @@ class CarInterface(CarInterfaceBase):
cloudlog.debug("Using Mock Car Interface")
self.gyro = messaging.sub_sock('gyroscope')
self.gps = messaging.sub_sock('gpsLocationExternal')
self.sm = messaging.SubMaster(['gyroscope', 'gpsLocation', 'gpsLocationExternal'])
self.speed = 0.
self.prev_speed = 0.
@ -45,15 +44,16 @@ class CarInterface(CarInterfaceBase):
# returns a car.CarState
def _update(self, c):
self.sm.update(0)
# get basic data from phone and gps since CAN isn't connected
gyro_sensor = messaging.recv_sock(self.gyro)
if gyro_sensor is not None:
self.yaw_rate_meas = -gyro_sensor.gyroscope.gyroUncalibrated.v[0]
if self.sm.updated['gyroscope']:
self.yaw_rate_meas = -self.sm['gyroscope'].gyroUncalibrated.v[0]
gps = messaging.recv_sock(self.gps)
if gps is not None:
gps_sock = 'gpsLocationExternal' if self.sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation'
if self.sm.updated[gps_sock]:
self.prev_speed = self.speed
self.speed = gps.gpsLocationExternal.speed
self.speed = self.sm[gps_sock].speed
# create 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.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 \
(self.car_fingerprint not in PREGLOBAL_CARS and cp.vl["Dashlights"]["UNITS"] == 1):
ret.cruiseState.speed *= CV.MPH_TO_KPH
@ -78,6 +75,8 @@ class CarState(CarStateBase):
else:
ret.steerFaultTemporary = cp.vl["Steering_Torque"]["Steer_Warning"] == 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"])
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("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("7653b2bce7bcfdaa|2020-03-04--15-34-32", HYUNDAI.KIA_OPTIMA),
CarTestRoute("018654717bc93d7d|2022-09-19--23-11-10", HYUNDAI.KIA_OPTIMA_2019, segment=0),
CarTestRoute("7653b2bce7bcfdaa|2020-03-04--15-34-32", HYUNDAI.KIA_OPTIMA_G4),
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("5b7c365c50084530|2020-04-15--16-13-24", HYUNDAI.SONATA),
CarTestRoute("b2a38c712dcf90bd|2020-05-18--18-12-48", HYUNDAI.SONATA_LF),

@ -1,4 +1,5 @@
#!/usr/bin/env python3
from collections import defaultdict
import re
import unittest
@ -20,6 +21,15 @@ class TestCarDocs(unittest.TestCase):
self.assertEqual(generated_cars_md, current_cars_md,
"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):
all_car_info_platforms = get_interface_attr("CAR_INFO", combine_brands=True).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 2022: TOYOTA AVALON 2022
KIA OPTIMA 2016: HYUNDAI SONATA 2020
KIA OPTIMA 2019: HYUNDAI SONATA 2020
KIA OPTIMA 4TH GEN: HYUNDAI SONATA 2020
KIA OPTIMA 4TH GEN FACELIFT: HYUNDAI SONATA 2020
KIA OPTIMA HYBRID 2017 & SPORTS 2019: HYUNDAI SONATA 2020
KIA FORTE E 2018 & GT 2021: 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_NXH_TSS2: ToyotaCarInfo("Lexus NX Hybrid 2020-21"),
CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2017-20"),
CAR.LEXUS_RX: ToyotaCarInfo("Lexus RX 2016-19", footnotes=[Footnote.DSU]),
CAR.LEXUS_RXH: ToyotaCarInfo("Lexus RX Hybrid 2016-19", footnotes=[Footnote.DSU]),
CAR.LEXUS_RX: [
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_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:
vin = results.get((addr, 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
if vin.startswith(b'\x11'):
vin = vin[1:18]

@ -170,7 +170,7 @@ class CarState(CarStateBase):
ret.gas = pt_cp.vl["Motor_3"]["Fahrpedal_Rohsignal"] / 100.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.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"])
# Update gear and/or clutch position data.

@ -143,6 +143,12 @@ class Controls:
put_nonblocking("CarParamsCache", 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.CS_prev = car.CarState.new_message()
self.AM = AlertManager()
@ -313,7 +319,7 @@ class Controls:
else:
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)
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):
if error > deadzone:
error -= deadzone

@ -5,7 +5,6 @@ import numpy as np
from casadi import SX, vertcat, sin, cos
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
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")
X_DIM = 4
P_DIM = 2
N = 16
COST_E_DIM = 3
COST_DIM = COST_E_DIM + 1
SPEED_OFFSET = 10.0
MODEL_NAME = 'lat'
ACADOS_SOLVER_TYPE = 'SQP_RTI'
@ -29,8 +32,8 @@ def gen_lat_model():
x_ego = SX.sym('x_ego')
y_ego = SX.sym('y_ego')
psi_ego = SX.sym('psi_ego')
curv_ego = SX.sym('curv_ego')
model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego)
psi_rate_ego = SX.sym('psi_rate_ego')
model.x = vertcat(x_ego, y_ego, psi_ego, psi_rate_ego)
# parameters
v_ego = SX.sym('v_ego')
@ -38,22 +41,22 @@ def gen_lat_model():
model.p = vertcat(v_ego, rotation_radius)
# controls
curv_rate = SX.sym('curv_rate')
model.u = vertcat(curv_rate)
psi_accel_ego = SX.sym('psi_accel_ego')
model.u = vertcat(psi_accel_ego)
# xdot
x_ego_dot = SX.sym('x_ego_dot')
y_ego_dot = SX.sym('y_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
f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego),
v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego),
v_ego * curv_ego,
curv_rate)
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) * psi_rate_ego,
psi_rate_ego,
psi_accel_ego)
model.f_impl_expr = model.xdot - f_expl
model.f_expl_expr = f_expl
return model
@ -72,26 +75,27 @@ def gen_lat_ocp():
ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.cost_type_e = 'NONLINEAR_LS'
Q = np.diag([0.0, 0.0])
QR = np.diag([0.0, 0.0, 0.0])
Q = np.diag(np.zeros(COST_E_DIM))
QR = np.diag(np.zeros(COST_DIM))
ocp.cost.W = QR
ocp.cost.W_e = Q
y_ego, psi_ego = ocp.model.x[1], ocp.model.x[2]
curv_rate = ocp.model.u[0]
y_ego, psi_ego, psi_rate_ego = ocp.model.x[1], ocp.model.x[2], ocp.model.x[3]
psi_rate_ego_dot = ocp.model.u[0]
v_ego = ocp.model.p[0]
ocp.parameter_values = np.zeros((P_DIM, ))
ocp.cost.yref = np.zeros((3, ))
ocp.cost.yref_e = np.zeros((2, ))
# TODO hacky weights to keep behavior the same
ocp.cost.yref = np.zeros((COST_DIM, ))
ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
ocp.model.cost_y_expr = vertcat(y_ego,
((v_ego +5.0) * psi_ego),
((v_ego + 5.0) * 4.0 * curv_rate))
((v_ego + SPEED_OFFSET) * psi_ego),
((v_ego + SPEED_OFFSET) * psi_rate_ego),
((v_ego + SPEED_OFFSET) * psi_rate_ego_dot))
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
ocp.constraints.constr_type = 'BGH'
@ -124,10 +128,10 @@ class LateralMpc():
def reset(self, x0=np.zeros(X_DIM)):
self.x_sol = np.zeros((N+1, X_DIM))
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):
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
for i in range(N+1):
@ -140,14 +144,13 @@ class LateralMpc():
self.solve_time = 0.0
self.cost = 0
def set_weights(self, path_weight, heading_weight, steer_rate_weight):
W = np.asfortranarray(np.diag([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, yaw_rate_weight, yaw_accel_cost]))
for i in range(N):
self.solver.cost_set(i, 'W', W)
#TODO hacky weights to keep behavior the same
self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2])
self.solver.cost_set(N, 'W', W[:COST_E_DIM,:COST_E_DIM])
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)
p_cp = np.copy(p)
self.solver.constraints_set(0, "lbx", x0_cp)
@ -155,13 +158,13 @@ class LateralMpc():
self.yref[:,0] = y_pts
v_ego = p_cp[0]
# rotation_radius = p_cp[1]
self.yref[:,1] = heading_pts*(v_ego+5.0)
self.yref[:,2] = curv_rate_pts * (v_ego+5.0) * 4.0
self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET)
self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET)
for i in range(N):
self.solver.cost_set(i, "yref", self.yref[i])
self.solver.set(i, "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()
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 system.swaglog import cloudlog
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
import cereal.messaging as messaging
from cereal import log
@ -11,6 +12,15 @@ from cereal import log
TRAJECTORY_SIZE = 33
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:
def __init__(self, CP):
self.DH = DesireHelper()
@ -22,9 +32,8 @@ class LateralPlanner:
self.solution_invalid_cnt = 0
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_curv_rate = np.zeros((TRAJECTORY_SIZE,))
self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,))
self.t_idxs = np.arange(TRAJECTORY_SIZE)
self.y_pts = np.zeros(TRAJECTORY_SIZE)
@ -36,7 +45,8 @@ class LateralPlanner:
self.lat_mpc.reset(x0=self.x0)
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
# 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.t_idxs = np.array(md.position.t)
self.plan_yaw = np.array(md.orientation.z)
if len(md.position.xStd) == TRAJECTORY_SIZE:
self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd])
self.plan_yaw_rate = np.array(md.orientationRate.z)
# Lane change logic
desire_state = md.meta.desireState
@ -57,29 +66,28 @@ class LateralPlanner:
self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
d_path_xyz = self.path_xyz
# Heading cost is useful at low speed, otherwise end of plan can be off-heading
heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15])
self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE)
self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST,
LATERAL_ACCEL_COST, LATERAL_JERK_COST)
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])
heading_pts = np.interp(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)
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(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
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
assert len(y_pts) == LAT_MPC_N + 1
assert len(heading_pts) == LAT_MPC_N + 1
assert len(curv_rate_pts) == LAT_MPC_N + 1
lateral_factor = max(0, self.factor1 - (self.factor2 * v_ego**2))
p = np.array([v_ego, lateral_factor])
assert len(yaw_rate_pts) == LAT_MPC_N + 1
lateral_factor = max(0, self.factor1 - (self.factor2 * self.v_ego**2))
p = np.array([self.v_ego, lateral_factor])
self.lat_mpc.run(self.x0,
p,
y_pts,
heading_pts,
curv_rate_pts)
# init state for next
# mpc.u_sol is the desired curvature rate given x0 curv state.
# with x0[3] = measured_curvature, this would be the actual desired rate.
# instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control.
yaw_rate_pts)
# init state for next iteration
# mpc.u_sol is the desired second derivative of psi given x0 curv state.
# 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 yaw rate for lat_control.
self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])
# Check for infeasible MPC solution
@ -87,7 +95,7 @@ class LateralPlanner:
t = sec_since_boot()
if mpc_nans or self.lat_mpc.solution_status != 0:
self.reset_mpc()
self.x0[3] = measured_curvature
self.x0[3] = measured_curvature * self.v_ego
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
@ -106,8 +114,9 @@ class LateralPlanner:
lateralPlan.modelMonoTime = sm.logMonoTime['modelV2']
lateralPlan.dPathPoints = self.y_pts.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.solverExecutionTime = self.lat_mpc.solve_time

@ -7,9 +7,6 @@ from selfdrive.modeld.constants import T_IDXS
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,
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]
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
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]
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:
raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set')
self.set_cost_weights(cost_weights, constraint_cost_weights)
@ -386,28 +383,6 @@ class LongitudinalMpc:
(lead_1_obstacle[0] - lead_0_obstacle[0]):
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):
# t0 = sec_since_boot()
# reset = 0

@ -58,7 +58,6 @@ class LongitudinalPlanner:
self.a_desired = init_a
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.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)
v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x)
a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x)
# Uniform interp so gradient is less noisy
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)
j = np.zeros(len(T_IDXS_MPC))
else:
x = 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))
return x, v, a, j
def update(self, sm):
if self.param_read_counter % 50 == 0:
def update(self, sm, read=True):
if self.param_read_counter % 50 == 0 and read:
self.read_param()
self.param_read_counter += 1

@ -1,7 +1,8 @@
import unittest
import numpy as np
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.,
@ -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:
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)
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_length = len(current_coding)
# EV_SteerAssisMQB covers the majority of MQB racks (EPS_MQB_ZFLS)
if odx_file == "EV_SteerAssisMQB\x00":
# EV_SteerAssisMQB/MNB cover the majority of MQB racks (EPS_MQB_ZFLS)
if odx_file in ("EV_SteerAssisMQB\x00", "EV_SteerAssisMNB\x00"):
coding_variant = "ZF"
coding_byte = 0
coding_bit = 4
@ -111,7 +111,7 @@ if __name__ == "__main__":
if args.action in ["enable", "disable"]:
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
# bit we care about is always in the same place in the first byte
if args.action == "enable":

@ -16,7 +16,7 @@ from common.params import Params, put_nonblocking
from laika import AstroDog
from laika.constants import SECS_IN_HR, SECS_IN_MIN
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.helpers import ConstellationId
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_residual = []
self.last_pos_fix_t = None
self.gps_week = None
self.use_qcom = use_qcom
def load_cache(self):
@ -107,11 +108,11 @@ class Laikad:
return self.last_pos_fix
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)
# TODO support GLONASS
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
else:
return False
@ -129,9 +130,28 @@ class Laikad:
new_meas = read_raw_ublox(report)
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):
if self.is_good_report(gnss_msg):
week, tow, new_meas = self.read_report(gnss_msg)
self.gps_week = week
t = gnss_mono_time * 1e-9
if week > 0:
@ -172,12 +192,10 @@ class Laikad:
"correctedMeasurements": meas_msgs
}
return dat
# TODO this only works on GLONASS, qcom needs live ephemeris parsing too
elif gnss_msg.which == 'ephemeris':
ephem = convert_ublox_ephem(gnss_msg.ephemeris)
self.astro_dog.add_navs({ephem.prn: [ephem]})
self.cache_ephemeris(t=ephem.epoch)
#elif gnss_msg.which == 'ionoData':
elif self.is_ephemeris(gnss_msg):
self.read_ephemeris(gnss_msg)
#elif gnss_msg.which() == 'ionoData':
# 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]):
@ -265,9 +283,11 @@ def create_measurement_msg(meas: GNSSMeasurement):
c.satVel = meas.sat_vel.tolist()
ephem = meas.sat_ephemeris
assert ephem is not None
week, time_of_week = -1, -1
if ephem.eph_type == EphemerisType.NAV:
source_type = EphemerisSourceType.nav
week, time_of_week = -1, -1
elif ephem.eph_type == EphemerisType.QCOM_POLY:
source_type = EphemerisSourceType.qcom
else:
assert ephem.file_epoch is not None
week = ephem.file_epoch.week
@ -325,10 +345,11 @@ class EphemerisSourceType(IntEnum):
nav = 0
nasaUltraRapid = 1
glonassIacUltraRapid = 2
qcom = 3
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:
raw_gnss_socket = "qcomGnss"
else:
@ -348,6 +369,17 @@ def main(sm=None, pm=None):
if sm.updated[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)
if msg is not None:
pm.send('gnssMeasurements', msg)

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

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

@ -12,7 +12,6 @@ from common.realtime import config_realtime_process, DT_MDL
from common.filter_simple import FirstOrderFilter
from system.swaglog import cloudlog
from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
from selfdrive.car.toyota.values import CAR as TOYOTA
HISTORY = 5 # secs
POINTS_PER_BUCKET = 1500
@ -33,7 +32,7 @@ MAX_INVALID_THRESHOLD = 10
MIN_ENGAGE_BUFFER = 2 # secs
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):
@ -98,7 +97,7 @@ class TorqueEstimator:
self.offline_friction = 0.0
self.offline_latAccelFactor = 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':
self.offline_friction = CP.lateralTuning.torque.friction

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

@ -43,6 +43,7 @@ procs = [
PythonProcess("deleter", "selfdrive.loggerd.deleter", offroad=True),
PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), callback=driverview),
PythonProcess("laikad", "selfdrive.locationd.laikad"),
PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=TICI),
PythonProcess("navd", "selfdrive.navd.navd"),
PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True),
PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
@ -55,11 +56,9 @@ procs = [
PythonProcess("uploader", "selfdrive.loggerd.uploader", offroad=True),
PythonProcess("statsd", "selfdrive.statsd", offroad=True),
# debug procs
NativeProcess("bridge", "cereal/messaging", ["./bridge"], 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}

@ -71,9 +71,9 @@ if use_thneed and arch == "larch64" or GetOption('pc_thneed'):
fn = File("models/supercombo").abspath
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:
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?
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);
#ifdef TEMPORAL
s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE);
s->m->addRecurrent(&s->feature_buffer[0], TEMPORAL_SIZE);
#endif
#ifdef DESIRE
s->m->addDesire(s->pulse_desire, DESIRE_LEN);
s->m->addDesire(s->pulse_desire, DESIRE_LEN*(HISTORY_BUFFER_LEN+1));
#endif
#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,
const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only) {
#ifdef DESIRE
std::memmove(&s->pulse_desire[0], &s->pulse_desire[DESIRE_LEN], sizeof(float) * DESIRE_LEN*HISTORY_BUFFER_LEN);
if (desire_in != NULL) {
for (int i = 1; i < DESIRE_LEN; i++) {
// Model decides when action is completed
// so desire input is just a pulse triggered on rising edge
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 {
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];
}
}
LOGT("Desire enqueued");
#endif
int rhd_idx = is_rhd;
@ -92,6 +94,12 @@ ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf,
s->m->execute();
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;
}

@ -16,6 +16,8 @@
#include "selfdrive/modeld/models/commonmodel.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_PRED_LEN = 4;
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));
struct ModelOutputFeatures {
std::array<float, FEATURE_LEN> feature;
};
static_assert(sizeof(ModelOutputFeatures) == (sizeof(float)*FEATURE_LEN));
struct ModelOutput {
const ModelOutputPlans plans;
const ModelOutputLaneLines lane_lines;
@ -244,22 +251,24 @@ struct ModelOutput {
};
constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float);
#ifdef TEMPORAL
constexpr int TEMPORAL_SIZE = 512;
constexpr int TEMPORAL_SIZE = HISTORY_BUFFER_LEN * FEATURE_LEN;
#else
constexpr int TEMPORAL_SIZE = 0;
#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
struct ModelState {
ModelFrame *frame = nullptr;
ModelFrame *wide_frame = nullptr;
std::array<float, HISTORY_BUFFER_LEN * FEATURE_LEN> feature_buffer = {};
std::array<float, NET_OUTPUT_SIZE> output = {};
std::unique_ptr<RunModel> m;
#ifdef DESIRE
float prev_desire[DESIRE_LEN] = {};
float pulse_desire[DESIRE_LEN] = {};
float pulse_desire[DESIRE_LEN*(HISTORY_BUFFER_LEN+1)] = {};
#endif
#ifdef TRAFFIC_CONVENTION
float traffic_convention[TRAFFIC_CONVENTION_LEN] = {};

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

@ -9,6 +9,8 @@ os.environ["OMP_WAIT_POLICY"] = "PASSIVE"
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):
dd = []
gt = 0
@ -18,7 +20,7 @@ def read(sz, tf8=False):
assert(len(st) > 0)
dd.append(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:
r = r / 255.
return r
@ -29,22 +31,23 @@ def write(d):
def run_loop(m, tf8_input=False):
ishapes = [[1]+ii.shape[1:] for ii 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
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)
while 1:
inputs = []
for k, shp in zip(keys, ishapes):
for k, shp, itp in zip(keys, ishapes, itypes):
ts = np.product(shp)
#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)))
#print(ret, file=sys.stderr)
for r in ret:
write(r)
write(r.astype(np.float32))
if __name__ == "__main__":

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

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

@ -1,5 +1,3 @@
import os
import time
import select
from serial import Serial
from crcmod import mkCrcFun
@ -11,18 +9,7 @@ class ModemDiag:
self.pend = b''
def open_serial(self):
def op():
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 = Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True, timeout=0, exclusive=True)
serial.flush()
serial.reset_input_buffer()
serial.reset_output_buffer()

@ -5,23 +5,34 @@ import signal
import itertools
import math
import time
import subprocess
from typing import NoReturn
from struct import unpack_from, calcsize, pack
import cereal.messaging as messaging
from cereal import log
from system.swaglog import cloudlog
import cereal.messaging as messaging
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.structs import dict_unpacker
from selfdrive.sensord.rawgps.structs import gps_measurement_report, gps_measurement_report_sv
from selfdrive.sensord.rawgps.structs import glonass_measurement_report, glonass_measurement_report_sv
from selfdrive.sensord.rawgps.structs import oemdre_measurement_report, oemdre_measurement_report_sv
from selfdrive.sensord.rawgps.structs import 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
from selfdrive.sensord.rawgps.structs import (dict_unpacker, position_report, relist,
gps_measurement_report, gps_measurement_report_sv,
glonass_measurement_report, glonass_measurement_report_sv,
oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report,
LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
LOG_GNSS_OEMDRE_SVPOLY_REPORT)
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 = {
"multipathEstimateIsValid": 0,
"directionIsValid": 1,
@ -65,59 +76,43 @@ measurementStatusGlonassFields = {
"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)
unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True)
log_types = [
LOG_GNSS_GPS_MEASUREMENT_REPORT,
LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
]
pub_types = ['qcomGnss']
unpack_position, _ = dict_unpacker(position_report)
log_types.append(LOG_GNSS_POSITION_REPORT)
pub_types.append("gpsLocation")
# connect to modem
diag = ModemDiag()
# NV enable OEMDRE
def try_setup_logs(diag, log_types):
for _ in range(5):
try:
setup_logs(diag, log_types)
break
except Exception:
cloudlog.exception("setup logs failed, trying again")
else:
raise Exception(f"setup logs failed, {log_types=}")
def mmcli(cmd: str) -> None:
for _ in range(5):
try:
subprocess.check_call(f"mmcli -m any --timeout 30 {cmd}", shell=True)
break
except subprocess.CalledProcessError:
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
DIAG_NV_READ_F = 38
DIAG_NV_WRITE_F = 39
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))
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")
setup_logs(diag, LOG_TYPES)
# disable DPO power savings for more accuracy
os.system("mmcli -m 0 --command='AT+QGPSCFG=\"dpoenable\",0'")
os.system("mmcli -m 0 --location-enable-gps-raw --location-enable-gps-nmea")
mmcli("--command='AT+QGPSCFG=\"dpoenable\",0'")
# 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
DIAG_SUBSYS_CMD_F = 75
@ -128,31 +123,78 @@ def main() -> NoReturn:
GPSDIAG_OEM_DRE_ON = 1
# gpsdiag_OemControlReqType
opcode, payload = send_recv(diag, DIAG_SUBSYS_CMD_F, pack('<BHBBIIII',
DIAG_SUBSYS_GPS, # Subsystem Id
CGPS_DIAG_PDAPI_CMD, # Subsystem Command Code
CGPS_OEM_CONTROL, # CGPS Command Code
0, # Version
GPSDIAG_OEMFEATURE_DRE,
GPSDIAG_OEM_DRE_ON,
0,0
send_recv(diag, DIAG_SUBSYS_CMD_F, pack('<BHBBIIII',
DIAG_SUBSYS_GPS, # Subsystem Id
CGPS_DIAG_PDAPI_CMD, # Subsystem Command Code
CGPS_OEM_CONTROL, # CGPS Command Code
0, # Version
GPSDIAG_OEMFEATURE_DRE,
GPSDIAG_OEM_DRE_ON,
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:
opcode, payload = diag.recv()
assert opcode == DIAG_LOG_F
(pending_msgs, log_outer_length), inner_log_packet = unpack_from('<BH', payload), payload[calcsize('<BH'):]
if pending_msgs > 0:
cloudlog.debug("have %d pending messages" % pending_msgs)
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'):]
assert log_inner_length == len(inner_log_packet)
if log_type not in log_types:
if log_type not in LOG_TYPES:
continue
if DEBUG:
print("%.4f: got log: %x len %d" % (time.time(), log_type, len(log_payload)))
if log_type == LOG_GNSS_OEMDRE_MEASUREMENT_REPORT:
msg = messaging.new_message('qcomGnss')
@ -221,7 +263,24 @@ def main() -> NoReturn:
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')
gnss = msg.qcomGnss

@ -56,6 +56,29 @@ oemdre_measurement_report = """
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 = """
uint8_t sv_id;
uint8_t unkn;
@ -311,3 +334,21 @@ def dict_unpacker(ss, camelcase = False):
nams = [name_to_camelcase(x) for x in nams]
sz = calcsize(st)
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):
val = 0
else:
raise NotImplementedError
raise NotImplementedError('Error ignoring field')
setattr(attr, keys[-1], val)
return msg.as_reader()

@ -22,7 +22,7 @@ from tools.lib.logreader import LogReader
TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"
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"))
@ -174,7 +174,7 @@ if __name__ == "__main__":
'driverStateV2.dspExecutionTime'
]
# TODO this tolerance is absurdly large
tolerance = 5e-1 if PC else None
tolerance = 2.0 if PC else None
results: Any = {TEST_ROUTE: {}}
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)

@ -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("WideCameraOnly", False)
params.put_bool("DisableLogging", False)
params.put_bool("UbloxAvailable", True)
os.environ["NO_RADAR_SLEEP"] = "1"
os.environ["REPLAY"] = "1"

@ -1 +1 @@
53079010a5db8105854212157b5ee90029df7b92
f636c68e2b4ed88d3731930cf15b6dee984eb6dd

@ -190,7 +190,7 @@ def migrate_carparams(lr):
return all_msgs
def migrate_sensorEvents(lr):
def migrate_sensorEvents(lr, old_logtime=False):
all_msgs = []
for msg in lr:
if msg.which() != 'sensorEventsDEPRECATED':
@ -214,6 +214,8 @@ def migrate_sensorEvents(lr):
m = messaging.new_message(sensor_service)
m.valid = True
if old_logtime:
m.logMonoTime = msg.logMonoTime
m_dat = getattr(m, sensor_service)
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"))
os.environ['FINGERPRINT'] = fingerprint
os.environ['SKIP_FW_QUERY'] = "1"
os.environ['REPLAY'] = "1"
def run(sm, pm, can_sock):

@ -177,7 +177,8 @@ def thermald_thread(end_event, hw_queue):
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
in_car = False
engaged_prev = False
@ -239,24 +240,32 @@ def thermald_thread(end_event, hw_queue):
msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness()
max_comp_temp = temp_filter.update(
max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC))
)
# this one is only used for offroad
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:
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))
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
# since going onroad increases load and can make temps go over 107
thermal_status = ThermalStatus.danger
else:
current_band = THERMAL_BANDS[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]
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]
# **** 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["not_uninstalling"] = not params.get_bool("DoUninstall")
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
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
@ -329,7 +339,8 @@ def thermald_thread(end_event, hw_queue):
started_seen = True
else:
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
if off_ts is None:
@ -363,7 +374,6 @@ def thermald_thread(end_event, hw_queue):
pm.send("deviceState", msg)
should_start_prev = should_start
startup_conditions_prev = startup_conditions.copy()
# Log to statsd
statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent)

@ -62,47 +62,6 @@ def get_tombstones():
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):
f_size = os.path.getsize(fn)
if f_size > MAX_SIZE:
@ -199,7 +158,7 @@ def main() -> NoReturn:
if fn.endswith(".crash"):
report_tombstone_apport(fn)
else:
report_tombstone_android(fn)
cloudlog.error(f"unknown crash type: {fn}")
except Exception:
cloudlog.exception(f"Error reporting tombstone {fn}")

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

@ -115,7 +115,9 @@ MapPanel::MapPanel(QWidget* parent) : QWidget(parent) {
stack->addWidget(main_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);
wrapper->addWidget(stack);
@ -194,7 +196,6 @@ void MapPanel::parseResponse(const QString &response, bool success) {
}
void MapPanel::refresh() {
stack->setCurrentIndex(uiState()->prime_type ? 0 : 1);
if (cur_destinations == prev_destinations) return;
QJsonDocument doc = QJsonDocument::fromJson(cur_destinations.trimmed().toUtf8());

@ -8,9 +8,11 @@
#include <QPainter>
#include <QScrollBar>
#include "selfdrive/ui/ui.h"
#include "selfdrive/ui/qt/util.h"
#include "selfdrive/ui/qt/qt_window.h"
#include "selfdrive/ui/qt/widgets/controls.h"
#include "selfdrive/ui/qt/widgets/prime.h"
#include "selfdrive/ui/qt/widgets/scrollview.h"
@ -150,7 +152,7 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid
// Roaming toggle
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) {
params.putBool("GsmRoaming", state);
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);
// APN settings
ButtonControl *editApnButton = new ButtonControl(tr("APN Setting"), tr("EDIT"));
editApnButton = new ButtonControl(tr("APN Setting"), tr("EDIT"));
connect(editApnButton, &ButtonControl::clicked, [=]() {
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();
@ -174,7 +176,7 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid
// Metered toggle
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) {
params.putBool("GsmMetered", 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
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->addStretch(1);
}

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

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

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

@ -201,6 +201,13 @@ void UIState::updateStatus() {
started_prev = 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) {

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

@ -327,7 +327,7 @@ class Updater:
self._has_internet = False
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)
for line in output.split('\n'):
@ -363,7 +363,6 @@ class Updater:
cloudlog.info("git reset in progress")
cmds = [
["git", "checkout", "--force", "--no-recurse-submodules", "-B", branch, "FETCH_HEAD"],
["git", "branch", "--set-upstream-to", f"origin/{branch}"],
["git", "reset", "--hard"],
["git", "clean", "-xdff"],
["git", "submodule", "init"],

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

@ -480,7 +480,7 @@ class Tici(HardwareBase):
# blue prime config
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):
r = {}

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

@ -95,7 +95,7 @@ std::optional<ProcStat> procStat(std::string stat) {
.num_threads = stol(v[StatPos::num_threads - 1]),
.starttime = stoull(v[StatPos::starttime - 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]),
};
return p;

@ -20,8 +20,8 @@ struct ProcCache {
struct ProcStat {
int pid, ppid, processor;
char state;
long cutime, cstime, priority, nice, num_threads;
unsigned long utime, stime, vms, rss;
long cutime, cstime, priority, nice, num_threads, rss;
unsigned long utime, stime, vms;
unsigned long long starttime;
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