diff --git a/.github/ISSUE_TEMPLATE/bug_report.yml b/.github/ISSUE_TEMPLATE/bug_report.yml index b1a14076ea..47eb6c216f 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.yml +++ b/.github/ISSUE_TEMPLATE/bug_report.yml @@ -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. diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml index 2c2deb17ba..45a8af0aaf 100644 --- a/.github/ISSUE_TEMPLATE/config.yml +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -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 diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 9606c05631..d4971a4339 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -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 diff --git a/.gitmodules b/.gitmodules index 26f93ef164..544c95c943 100644 --- a/.gitmodules +++ b/.gitmodules @@ -18,4 +18,4 @@ url = ../../commaai/body.git [submodule "tinygrad"] path = tinygrad_repo - url = https://github.com/geohot/tinygrad.git + url = ../../commaai/tinygrad.git diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b1696e823c..25b8490f92 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -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 diff --git a/Jenkinsfile b/Jenkinsfile index c4038090e1..b9b9eda667 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -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"], diff --git a/SConstruct b/SConstruct index 178b0cc872..e015218f2a 100644 --- a/SConstruct +++ b/SConstruct @@ -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') diff --git a/cereal b/cereal index d4cf8728e2..b29717c4c3 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit d4cf8728e2fa2d87d90098efa7ddeaf8f98a03db +Subproject commit b29717c4c328d5cf34d46f682f25267150f82637 diff --git a/common/params.cc b/common/params.cc index d4c5cd7caf..155bc88487 100644 --- a/common/params.cc +++ b/common/params.cc @@ -166,6 +166,7 @@ std::unordered_map 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 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()); + } } } diff --git a/common/params.h b/common/params.h index 7758a015f6..aecb3ee471 100644 --- a/common/params.h +++ b/common/params.h @@ -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 readAll(); diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index bbddda46ea..9d8933609f 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -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): diff --git a/common/tests/test_util.cc b/common/tests/test_util.cc index 25ecf09aa9..b70cc9044a 100644 --- a/common/tests/test_util.cc +++ b/common/tests/test_util.cc @@ -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()); +} diff --git a/common/util.cc b/common/util.cc index 92add63997..b6a8322a27 100644 --- a/common/util.cc +++ b/common/util.cc @@ -97,6 +97,22 @@ std::map 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) { diff --git a/common/util.h b/common/util.h index f3a24723b4..e13f4dc130 100644 --- a/common/util.h +++ b/common/util.h @@ -80,6 +80,7 @@ std::string dir_name(std::string const& path); // **** file fhelpers ***** std::string read_file(const std::string& fn); std::map 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); diff --git a/docs/CARS.md b/docs/CARS.md index ecd6932e87..82ef37ae8d 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -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[3](#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[3](#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[3](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Lexus|RX 2017-19|All|Stock[3](#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[3](#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[3](#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[3](#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| diff --git a/laika_repo b/laika_repo index c8bc1fa01b..e1049cde0a 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit c8bc1fa01be9f22592efb991ee52d3d965d21968 +Subproject commit e1049cde0a68f7d4a70b1ebd76befdc0e163ad55 diff --git a/launch_env.sh b/launch_env.sh index 48c5696b94..88e1f2a9c5 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -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 diff --git a/opendbc b/opendbc index eaac172af9..c35e8139bf 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit eaac172af9cb342204e69ec52339cdf3c6a8ac4e +Subproject commit c35e8139bf9e9d87b9efb6764ab7e65983e8d33e diff --git a/panda b/panda index 1910db8d4c..1303af2db2 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 1910db8d4c3f932fe85b186fba1d24795cb2b742 +Subproject commit 1303af2db29a72eee180b10c6097fa5b19c29207 diff --git a/release/files_common b/release/files_common index 5783edd070..07ffaf8501 100644 --- a/release/files_common +++ b/release/files_common @@ -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 diff --git a/scripts/launch_corolla.sh b/scripts/launch_corolla.sh index 0801938e71..146fbacf0a 100755 --- a/scripts/launch_corolla.sh +++ b/scripts/launch_corolla.sh @@ -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 diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 09e7137b38..3e39985c29 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -381,6 +381,7 @@ std::optional send_panda_states(PubMaster *pm, const std::vector 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 cs = {ps.initCanState0(), ps.initCanState1(), ps.initCanState2()}; diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 273364071b..4a8fd5fbd9 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -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 diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 0bba1d29b8..f96a234dbd 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -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: diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index fec21d8d24..999dabfee3 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -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.] diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 61da04d04b..4d60afe1fd 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -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), diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index b3e1aa6b66..7f3f6a72c5 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -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 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index b1e7dcbc0b..c724ab4068 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -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: diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 0e67cd84cc..b0780d154a 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -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 diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py index 94c8d052b3..4b4bdcc0ca 100644 --- a/selfdrive/car/isotp_parallel_query.py +++ b/selfdrive/car/isotp_parallel_query.py @@ -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() diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 36062da1d1..a3194cd79e 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -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() diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 128e4245b2..ba873c48d7 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -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 diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index af96529abe..54d47b99aa 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -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), diff --git a/selfdrive/car/tests/test_docs.py b/selfdrive/car/tests/test_docs.py index 191b36b8f2..b7056df5b3 100755 --- a/selfdrive/car/tests/test_docs.py +++ b/selfdrive/car/tests/test_docs.py @@ -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()): diff --git a/selfdrive/car/torque_data/substitute.yaml b/selfdrive/car/torque_data/substitute.yaml index 5411a066cb..8ae7c95fca 100644 --- a/selfdrive/car/torque_data/substitute.yaml +++ b/selfdrive/car/torque_data/substitute.yaml @@ -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 diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 1f8ab2498d..0075a483f6 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -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"), } diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py index 50c2abde46..cf1c25e851 100755 --- a/selfdrive/car/vin.py +++ b/selfdrive/car/vin.py @@ -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] diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index cf4a252b65..3e99ca8252 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -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. diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b6479e5608..6ce1156baf 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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: diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index e8f9585a6f..f81cd0c40c 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -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 diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index c0e7358160..0cbd576341 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -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() diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 3470754bc6..d4c832b53b 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -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 diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 43e1f9cc4b..db5bf4d3e6 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -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): diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 01a69d6f87..2aab4b71af 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -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 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 6ec3762a94..018136e6f2 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -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 diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 4864dbdc06..9b986c053d 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -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) diff --git a/selfdrive/debug/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py index c1068bf067..8c4dbc55ee 100755 --- a/selfdrive/debug/vw_mqb_config.py +++ b/selfdrive/debug/vw_mqb_config.py @@ -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": diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index c7f2d2ceac..2769f394c5 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -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) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index ac340fb4aa..9608f4003b 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -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 service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", "carState", "carParams", diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 8841b3e67c..e32861cfae 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -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() diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 78c3029af4..66af234590 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -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 diff --git a/selfdrive/loggerd/tests/test_loggerd.py b/selfdrive/loggerd/tests/test_loggerd.py index a2138b0aa6..9c3565d130 100755 --- a/selfdrive/loggerd/tests/test_loggerd.py +++ b/selfdrive/loggerd/tests/test_loggerd.py @@ -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") diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 06efdbb960..3f63fbb959 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -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} diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 246f8c2941..5c02e2b15f 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -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", diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 8d02eb6b2f..3316e6d114 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -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; } diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index e2ee812e44..8bb84d0245 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -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 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 feature_buffer = {}; std::array output = {}; std::unique_ptr 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] = {}; diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 7b11edbe08..aee0ac37ff 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:50c7fc8565ac69a4b9a0de122e961326820e78bf13659255a89d0ed04be030d5 -size 95167481 +oid sha256:c4d37af666344af6bb218e0b939b1152ad3784c15ac79e37bcf0124643c8286a +size 58539563 diff --git a/selfdrive/modeld/runners/onnx_runner.py b/selfdrive/modeld/runners/onnx_runner.py index ac7cc68814..d4a11a7c0b 100755 --- a/selfdrive/modeld/runners/onnx_runner.py +++ b/selfdrive/modeld/runners/onnx_runner.py @@ -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__": diff --git a/selfdrive/modeld/thneed/thneed_common.cc b/selfdrive/modeld/thneed/thneed_common.cc index 21170b13a6..ecdf1237e3 100644 --- a/selfdrive/modeld/thneed/thneed_common.cc +++ b/selfdrive/modeld/thneed/thneed_common.cc @@ -12,7 +12,7 @@ map, int> g_args_size; map 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; } diff --git a/selfdrive/sensord/pigeond.py b/selfdrive/sensord/pigeond.py index e1fa2f4cad..5fe120c061 100755 --- a/selfdrive/sensord/pigeond.py +++ b/selfdrive/sensord/pigeond.py @@ -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() diff --git a/selfdrive/sensord/rawgps/modemdiag.py b/selfdrive/sensord/rawgps/modemdiag.py index cc2bc5b261..5d72aeba9e 100644 --- a/selfdrive/sensord/rawgps/modemdiag.py +++ b/selfdrive/sensord/rawgps/modemdiag.py @@ -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() diff --git a/selfdrive/sensord/rawgps/rawgpsd.py b/selfdrive/sensord/rawgps/rawgpsd.py index 7c4582902b..c68e7aff99 100755 --- a/selfdrive/sensord/rawgps/rawgpsd.py +++ b/selfdrive/sensord/rawgps/rawgpsd.py @@ -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(' NoReturn: GPSDIAG_OEM_DRE_ON = 1 # gpsdiag_OemControlReqType - opcode, payload = send_recv(diag, DIAG_SUBSYS_CMD_F, pack(' 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(' 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(' 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 diff --git a/selfdrive/sensord/rawgps/structs.py b/selfdrive/sensord/rawgps/structs.py index 4bc9eca875..97e3d3d605 100644 --- a/selfdrive/sensord/rawgps/structs.py +++ b/selfdrive/sensord/rawgps/structs.py @@ -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} diff --git a/selfdrive/sensord/rawgps/test_rawgps.py b/selfdrive/sensord/rawgps/test_rawgps.py new file mode 100755 index 0000000000..5bd0833955 --- /dev/null +++ b/selfdrive/sensord/rawgps/test_rawgps.py @@ -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() diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py index bf6daf5fed..c14956b1b2 100755 --- a/selfdrive/test/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -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() diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index ccd89bea9a..ecfacbf5d2 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -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) diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 958d3da14d..2446ec061d 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -c40319a454840d8a2196ec1227d27b536ee14375 +c171250d2cc013b3eca1cda4fb62f3d0dda28d4d diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 10084bff9f..9d37be4a56 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -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" diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1f459b1948..a4a24ca603 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -53079010a5db8105854212157b5ee90029df7b92 \ No newline at end of file +f636c68e2b4ed88d3731930cf15b6dee984eb6dd diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index 54c84978c2..eee3745f8e 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -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 diff --git a/selfdrive/test/profiling/profiler.py b/selfdrive/test/profiling/profiler.py index 91226fc577..732a69eebd 100755 --- a/selfdrive/test/profiling/profiler.py +++ b/selfdrive/test/profiling/profiler.py @@ -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): diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 5c2fbd6825..89b81f06ec 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -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) diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 0045e0766c..61a575f141 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -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}") diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 92f6578dfc..84e055752a 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -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 diff --git a/selfdrive/ui/qt/maps/map_settings.cc b/selfdrive/ui/qt/maps/map_settings.cc index dd2ad04a7d..3205ca517d 100644 --- a/selfdrive/ui/qt/maps/map_settings.cc +++ b/selfdrive/ui/qt/maps/map_settings.cc @@ -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()); diff --git a/selfdrive/ui/qt/offroad/networking.cc b/selfdrive/ui/qt/offroad/networking.cc index 0ed6317c3c..13697adfb5 100644 --- a/selfdrive/ui/qt/offroad/networking.cc +++ b/selfdrive/ui/qt/offroad/networking.cc @@ -8,9 +8,11 @@ #include #include +#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); } diff --git a/selfdrive/ui/qt/offroad/networking.h b/selfdrive/ui/qt/offroad/networking.h index 4fc9a53d93..79cbcc3493 100644 --- a/selfdrive/ui/qt/offroad/networking.h +++ b/selfdrive/ui/qt/offroad/networking.h @@ -40,6 +40,9 @@ public: private: LabelControl* ipLabel; ToggleControl* tetheringToggle; + ToggleControl* roamingToggle; + ButtonControl* editApnButton; + ToggleControl* meteredToggle; WifiManager* wifi = nullptr; Params params; diff --git a/selfdrive/ui/qt/offroad/wifiManager.cc b/selfdrive/ui/qt/offroad/wifiManager.cc index 3a30456c93..62de3041b9 100644 --- a/selfdrive/ui/qt/offroad/wifiManager.cc +++ b/selfdrive/ui/qt/offroad/wifiManager.cc @@ -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) { diff --git a/selfdrive/ui/qt/widgets/prime.cc b/selfdrive/ui/qt/widgets/prime.cc index 04684fc765..da2f4e60d1 100644 --- a/selfdrive/ui/qt/widgets/prime.cc +++ b/selfdrive/ui/qt/widgets/prime.cc @@ -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); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index fcee00d1c8..152c7fbfcd 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -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) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 887b7ee841..f60c26b59a 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -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; diff --git a/selfdrive/updated.py b/selfdrive/updated.py index f96abcb7d5..9568b28ae3 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -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"], diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json index dc0fa6f1c1..8534c8a978 100644 --- a/system/hardware/tici/agnos.json +++ b/system/hardware/tici/agnos.json @@ -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, diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index 340093b604..e2fd20c1be 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -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 = {} diff --git a/system/hardware/tici/test_power_draw.py b/system/hardware/tici/test_power_draw.py index 4b380372b9..4830975917 100755 --- a/system/hardware/tici/test_power_draw.py +++ b/system/hardware/tici/test_power_draw.py @@ -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), ] diff --git a/system/proclogd/proclog.cc b/system/proclogd/proclog.cc index cbe3b53493..09ab4f559e 100644 --- a/system/proclogd/proclog.cc +++ b/system/proclogd/proclog.cc @@ -95,7 +95,7 @@ std::optional 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; diff --git a/system/proclogd/proclog.h b/system/proclogd/proclog.h index 9ed53d1bac..49f97cdd36 100644 --- a/system/proclogd/proclog.h +++ b/system/proclogd/proclog.h @@ -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; }; diff --git a/tinygrad_repo b/tinygrad_repo index 2e9b7637b3..870ea766ee 160000 --- a/tinygrad_repo +++ b/tinygrad_repo @@ -1 +1 @@ -Subproject commit 2e9b7637b3c3c8895fda9f964215db3a35fe3441 +Subproject commit 870ea766eec7a38d7d590c81436f15271ba2667e diff --git a/tools/cabana/.gitignore b/tools/cabana/.gitignore new file mode 100644 index 0000000000..0c21d5530d --- /dev/null +++ b/tools/cabana/.gitignore @@ -0,0 +1,4 @@ +moc_* +*.moc + +_cabana diff --git a/tools/cabana/README.md b/tools/cabana/README.md new file mode 100644 index 0000000000..f64e6b2d2d --- /dev/null +++ b/tools/cabana/README.md @@ -0,0 +1,9 @@ +# Cabana + + + +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) diff --git a/tools/cabana/SConscript b/tools/cabana/SConscript new file mode 100644 index 0000000000..c87e2cdd94 --- /dev/null +++ b/tools/cabana/SConscript @@ -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) diff --git a/tools/cabana/cabana b/tools/cabana/cabana new file mode 100755 index 0000000000..b29dd66e3d --- /dev/null +++ b/tools/cabana/cabana @@ -0,0 +1,4 @@ +#!/bin/sh +cd "$(dirname "$0")" +export LD_LIBRARY_PATH="../../opendbc/can:$LD_LIBRARY_PATH" +exec ./_cabana "$1" diff --git a/tools/cabana/cabana.cc b/tools/cabana/cabana.cc new file mode 100644 index 0000000000..20cd889023 --- /dev/null +++ b/tools/cabana/cabana.cc @@ -0,0 +1,32 @@ +#include +#include + +#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(); +} diff --git a/tools/cabana/chartswidget.cc b/tools/cabana/chartswidget.cc new file mode 100644 index 0000000000..5caa8d5a43 --- /dev/null +++ b/tools/cabana/chartswidget.cc @@ -0,0 +1,271 @@ +#include "tools/cabana/chartswidget.h" + +#include +#include +#include +#include +#include +#include + +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(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()) { + 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(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(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(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}); +} diff --git a/tools/cabana/chartswidget.h b/tools/cabana/chartswidget.h new file mode 100644 index 0000000000..81df2237bc --- /dev/null +++ b/tools/cabana/chartswidget.h @@ -0,0 +1,77 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +#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 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 charts; +}; diff --git a/tools/cabana/detailwidget.cc b/tools/cabana/detailwidget.cc new file mode 100644 index 0000000000..1b6552804e --- /dev/null +++ b/tools/cabana/detailwidget.cc @@ -0,0 +1,264 @@ + +#include "tools/cabana/detailwidget.h" + +#include +#include +#include +#include +#include +#include +#include + +#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(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(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(); +} diff --git a/tools/cabana/detailwidget.h b/tools/cabana/detailwidget.h new file mode 100644 index 0000000000..b2e7cbf3b7 --- /dev/null +++ b/tools/cabana/detailwidget.h @@ -0,0 +1,70 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#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; +}; diff --git a/tools/cabana/mainwin.cc b/tools/cabana/mainwin.cc new file mode 100644 index 0000000000..49e6cd2cca --- /dev/null +++ b/tools/cabana/mainwin.cc @@ -0,0 +1,58 @@ +#include "tools/cabana/mainwin.h" + +#include +#include +#include + +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); +} diff --git a/tools/cabana/mainwin.h b/tools/cabana/mainwin.h new file mode 100644 index 0000000000..bcd15e4d8e --- /dev/null +++ b/tools/cabana/mainwin.h @@ -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; +}; diff --git a/tools/cabana/messageswidget.cc b/tools/cabana/messageswidget.cc new file mode 100644 index 0000000000..7de3507b3d --- /dev/null +++ b/tools/cabana/messageswidget.cc @@ -0,0 +1,93 @@ +#include "tools/cabana/messageswidget.h" + +#include +#include +#include +#include + +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); + } +} diff --git a/tools/cabana/messageswidget.h b/tools/cabana/messageswidget.h new file mode 100644 index 0000000000..1dbb4a1af3 --- /dev/null +++ b/tools/cabana/messageswidget.h @@ -0,0 +1,24 @@ +#pragma once + +#include +#include +#include + +#include "tools/cabana/parser.h" + +class MessagesWidget : public QWidget { + Q_OBJECT + + public: + MessagesWidget(QWidget *parent); + + public slots: + void updateState(); + + signals: + void msgChanged(const CanData *id); + + protected: + QLineEdit *filter; + QTableWidget *table_widget; +}; diff --git a/tools/cabana/parser.cc b/tools/cabana/parser.cc new file mode 100644 index 0000000000..f4bacbb86d --- /dev/null +++ b/tools/cabana/parser.cc @@ -0,0 +1,181 @@ +#include "tools/cabana/parser.h" + +#include + +#include "cereal/messaging/messaging.h" + +Parser *parser = nullptr; + +static bool event_filter(const Event *e, void *opaque) { + Parser *p = (Parser*)opaque; + return p->eventFilter(e); +} + +Parser::Parser(QObject *parent) : QObject(parent) { + parser = this; + + qRegisterMetaType>(); + QObject::connect(this, &Parser::received, this, &Parser::process, Qt::QueuedConnection); +} + +Parser::~Parser() { + replay->stop(); +} + +bool Parser::loadRoute(const QString &route, const QString &data_dir, bool use_qcam) { + replay = new Replay(route, {"can", "roadEncodeIdx"}, {}, nullptr, use_qcam ? REPLAY_FLAG_QCAMERA : 0, data_dir, this); + replay->installEventFilter(event_filter, this); + QObject::connect(replay, &Replay::segmentsMerged, this, &Parser::segmentsMerged); + if (replay->load()) { + replay->start(); + return true; + } + return false; +} + +void Parser::openDBC(const QString &name) { + dbc_name = name; + dbc = const_cast(dbc_lookup(name.toStdString())); + counters.clear(); + msg_map.clear(); + for (auto &msg : dbc->msgs) { + msg_map[msg.address] = &msg; + } +} + +void Parser::process(std::vector msgs) { + static double prev_update_ts = 0; + + for (const auto &can_data : msgs) { + can_msgs[can_data.id] = can_data; + ++counters[can_data.id]; + + if (can_data.id == current_msg_id) { + while (history_log.size() >= LOG_SIZE) { + history_log.pop_back(); + } + history_log.push_front(can_data); + } + } + double now = millis_since_boot(); + if ((now - prev_update_ts) > 1000.0 / FPS) { + prev_update_ts = now; + emit updated(); + } + + if (current_sec < begin_sec || current_sec > end_sec) { + // loop replay in selected range. + seekTo(begin_sec); + } +} + +bool Parser::eventFilter(const Event *event) { + // drop packets when the GUI thread is calling seekTo. to make sure the current_sec is accurate. + if (!seeking && event->which == cereal::Event::Which::CAN) { + current_sec = (event->mono_time - replay->routeStartTime()) / (double)1e9; + + auto can = event->event.getCan(); + msgs_buf.clear(); + msgs_buf.reserve(can.size()); + + for (const auto &c : can) { + CanData &data = msgs_buf.emplace_back(); + data.address = c.getAddress(); + data.bus_time = c.getBusTime(); + data.source = c.getSrc(); + data.dat.append((char *)c.getDat().begin(), c.getDat().size()); + data.id = QString("%1:%2").arg(data.source).arg(data.address, 1, 16); + data.ts = current_sec; + } + emit received(msgs_buf); + } + return true; +} + +void Parser::seekTo(double ts) { + seeking = true; + replay->seekTo(ts, false); + seeking = false; +} + +void Parser::addNewMsg(const Msg &msg) { + dbc->msgs.push_back(msg); + msg_map[dbc->msgs.back().address] = &dbc->msgs.back(); +} + +void Parser::removeSignal(const QString &id, const QString &sig_name) { + Msg *msg = const_cast(getMsg(id)); + if (!msg) return; + + auto it = std::find_if(msg->sigs.begin(), msg->sigs.end(), [=](auto &sig) { return sig_name == sig.name.c_str(); }); + if (it != msg->sigs.end()) { + msg->sigs.erase(it); + emit signalRemoved(id, sig_name); + } +} + +uint32_t Parser::addressFromId(const QString &id) { + return id.mid(id.indexOf(':') + 1).toUInt(nullptr, 16); +} + +const Signal *Parser::getSig(const QString &id, const QString &sig_name) { + if (auto msg = getMsg(id)) { + auto it = std::find_if(msg->sigs.begin(), msg->sigs.end(), [&](auto &s) { return sig_name == s.name.c_str(); }); + if (it != msg->sigs.end()) { + return &(*it); + } + } + return nullptr; +} + +void Parser::setRange(double min, double max) { + if (begin_sec != min || end_sec != max) { + begin_sec = min; + end_sec = max; + is_zoomed = begin_sec != event_begin_sec || end_sec != event_end_sec; + emit rangeChanged(min, max); + } +} + +void Parser::segmentsMerged() { + auto events = replay->events(); + if (!events || events->empty()) return; + + auto it = std::find_if(events->begin(), events->end(), [=](const Event *e) { return e->which == cereal::Event::Which::CAN; }); + event_begin_sec = it == events->end() ? 0 : ((*it)->mono_time - replay->routeStartTime()) / (double)1e9; + event_end_sec = double(events->back()->mono_time - replay->routeStartTime()) / 1e9; + if (!is_zoomed) { + begin_sec = event_begin_sec; + end_sec = event_end_sec; + } + emit eventsMerged(); +} + +void Parser::resetRange() { + setRange(event_begin_sec, event_end_sec); +} + +void Parser::setCurrentMsg(const QString &id) { + current_msg_id = id; + history_log.clear(); +} + +// helper functions + +static QVector BIG_ENDIAN_START_BITS = []() { + QVector ret; + for (int i = 0; i < 64; i++) { + for (int j = 7; j >= 0; j--) { + ret.push_back(j + i * 8); + } + } + return ret; +}(); + +int bigEndianStartBitsIndex(int start_bit) { + return BIG_ENDIAN_START_BITS[start_bit]; +} + +int bigEndianBitIndex(int index) { + return BIG_ENDIAN_START_BITS.indexOf(index); +} diff --git a/tools/cabana/parser.h b/tools/cabana/parser.h new file mode 100644 index 0000000000..1632fcf6a6 --- /dev/null +++ b/tools/cabana/parser.h @@ -0,0 +1,95 @@ +#pragma once + +#include +#include + +#include +#include +#include + +#include "opendbc/can/common.h" +#include "opendbc/can/common_dbc.h" +#include "tools/replay/replay.h" + +const int FPS = 20; +const static int LOG_SIZE = 25; + +struct CanData { + QString id; + double ts; + uint32_t address; + uint16_t bus_time; + uint8_t source; + QByteArray dat; +}; + +class Parser : public QObject { + Q_OBJECT + +public: + Parser(QObject *parent); + ~Parser(); + static uint32_t addressFromId(const QString &id); + bool eventFilter(const Event *event); + bool loadRoute(const QString &route, const QString &data_dir, bool use_qcam); + void openDBC(const QString &name); + void saveDBC(const QString &name) {} + void addNewMsg(const Msg &msg); + void removeSignal(const QString &id, const QString &sig_name); + void seekTo(double ts); + const Signal *getSig(const QString &id, const QString &sig_name); + void setRange(double min, double max); + void resetRange(); + void setCurrentMsg(const QString &id); + inline std::pair range() const { return {begin_sec, end_sec}; } + inline double currentSec() const { return current_sec; } + inline bool isZoomed() const { return is_zoomed; } + inline const Msg *getMsg(const QString &id) { return getMsg(addressFromId(id)); } + inline const Msg *getMsg(uint32_t address) { + auto it = msg_map.find(address); + return it != msg_map.end() ? it->second : nullptr; + } + +signals: + void showPlot(const QString &id, const QString &name); + void hidePlot(const QString &id, const QString &name); + void signalRemoved(const QString &id, const QString &sig_name); + void eventsMerged(); + void rangeChanged(double min, double max); + void received(std::vector can); + void updated(); + +public: + Replay *replay = nullptr; + QHash counters; + std::map can_msgs; + QList history_log; + +protected: + void process(std::vector can); + void segmentsMerged(); + + std::atomic current_sec = 0.; + std::atomic seeking = false; + QString dbc_name; + double begin_sec = 0; + double end_sec = 0; + double event_begin_sec = 0; + double event_end_sec = 0; + bool is_zoomed = false; + DBC *dbc = nullptr; + std::map msg_map; + QString current_msg_id; + std::vector msgs_buf; +}; + +Q_DECLARE_METATYPE(std::vector); + +// TODO: Add helper function in dbc.h +int bigEndianStartBitsIndex(int start_bit); +int bigEndianBitIndex(int index); +inline QString toHex(const QByteArray &dat) { + return dat.toHex(' ').toUpper(); +} + +extern Parser *parser; diff --git a/tools/cabana/signaledit.cc b/tools/cabana/signaledit.cc new file mode 100644 index 0000000000..c214adab09 --- /dev/null +++ b/tools/cabana/signaledit.cc @@ -0,0 +1,164 @@ +#include "tools/cabana/signaledit.h" + +#include +#include +#include +#include +#include +#include + +// SignalForm + +SignalForm::SignalForm(const Signal &sig, QWidget *parent) : QWidget(parent) { + QFormLayout *form_layout = new QFormLayout(this); + + name = new QLineEdit(sig.name.c_str()); + form_layout->addRow(tr("Name"), name); + + size = new QSpinBox(); + size->setValue(sig.size); + form_layout->addRow(tr("Size"), size); + + msb = new QSpinBox(); + msb->setValue(sig.msb); + form_layout->addRow(tr("Most significant bit"), msb); + + endianness = new QComboBox(); + endianness->addItems({"Little", "Big"}); + endianness->setCurrentIndex(sig.is_little_endian ? 0 : 1); + form_layout->addRow(tr("Endianness"), endianness); + + sign = new QComboBox(); + sign->addItems({"Signed", "Unsigned"}); + sign->setCurrentIndex(sig.is_signed ? 0 : 1); + form_layout->addRow(tr("sign"), sign); + + factor = new QSpinBox(); + factor->setValue(sig.factor); + form_layout->addRow(tr("Factor"), factor); + + offset = new QSpinBox(); + offset->setValue(sig.offset); + form_layout->addRow(tr("Offset"), offset); + + // TODO: parse the following parameters in opendbc + unit = new QLineEdit(); + form_layout->addRow(tr("Unit"), unit); + comment = new QLineEdit(); + form_layout->addRow(tr("Comment"), comment); + min_val = new QSpinBox(); + form_layout->addRow(tr("Minimum value"), min_val); + max_val = new QSpinBox(); + form_layout->addRow(tr("Maximum value"), max_val); + val_desc = new QLineEdit(); + form_layout->addRow(tr("Value descriptions"), val_desc); +} + +std::optional SignalForm::getSignal() { + Signal sig = {}; + sig.name = name->text().toStdString(); + sig.size = size->text().toInt(); + sig.offset = offset->text().toDouble(); + sig.factor = factor->text().toDouble(); + sig.msb = msb->text().toInt(); + sig.is_signed = sign->currentIndex() == 0; + sig.is_little_endian = endianness->currentIndex() == 0; + if (sig.is_little_endian) { + sig.lsb = sig.start_bit; + sig.msb = sig.start_bit + sig.size - 1; + } else { + sig.lsb = bigEndianStartBitsIndex(bigEndianBitIndex(sig.start_bit) + sig.size - 1); + sig.msb = sig.start_bit; + } + return (sig.name.empty() || sig.size <= 0) ? std::nullopt : std::optional(sig); +} + +// SignalEdit + +SignalEdit::SignalEdit(const QString &id, const Signal &sig, const QString &color, QWidget *parent) : id(id), name_(sig.name.c_str()), QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(0, 0, 0, 0); + + // title + QHBoxLayout *title_layout = new QHBoxLayout(); + QLabel *icon = new QLabel(">"); + icon->setStyleSheet("font-weight:bold"); + title_layout->addWidget(icon); + title = new ElidedLabel(this); + title->setText(sig.name.c_str()); + title->setStyleSheet(QString("font-weight:bold; color:%1").arg(color)); + title_layout->addWidget(title); + title_layout->addStretch(); + + plot_btn = new QPushButton("📈"); + plot_btn->setToolTip(tr("Show Plot")); + plot_btn->setFixedSize(30, 30); + QObject::connect(plot_btn, &QPushButton::clicked, [=]() { emit parser->showPlot(id, name_); }); + title_layout->addWidget(plot_btn); + main_layout->addLayout(title_layout); + + edit_container = new QWidget(this); + QVBoxLayout *v_layout = new QVBoxLayout(edit_container); + form = new SignalForm(sig, this); + v_layout->addWidget(form); + + QHBoxLayout *h = new QHBoxLayout(); + remove_btn = new QPushButton(tr("Remove Signal")); + h->addWidget(remove_btn); + h->addStretch(); + QPushButton *save_btn = new QPushButton(tr("Save")); + h->addWidget(save_btn); + v_layout->addLayout(h); + + edit_container->setVisible(false); + main_layout->addWidget(edit_container); + + QObject::connect(remove_btn, &QPushButton::clicked, this, &SignalEdit::remove); + QObject::connect(save_btn, &QPushButton::clicked, this, &SignalEdit::save); + QObject::connect(title, &ElidedLabel::clicked, [=]() { + edit_container->isVisible() ? edit_container->hide() : edit_container->show(); + icon->setText(edit_container->isVisible() ? "▼" : ">"); + }); +} + +void SignalEdit::save() { + if (auto sig = const_cast(parser->getSig(id, name_))) { + if (auto s = form->getSignal()) { + *sig = *s; + // TODO: reset the chart for sig + } + } +} + +void SignalEdit::remove() { + QMessageBox msgbox; + msgbox.setText(tr("Remove signal")); + msgbox.setInformativeText(tr("Are you sure you want to remove signal '%1'").arg(name_)); + msgbox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel); + msgbox.setDefaultButton(QMessageBox::Cancel); + if (msgbox.exec()) { + parser->removeSignal(id, name_); + deleteLater(); + } +} + +// AddSignalDialog + +AddSignalDialog::AddSignalDialog(const QString &id, QWidget *parent) : QDialog(parent) { + setWindowTitle(tr("Add signal to %1").arg(parser->getMsg(id)->name.c_str())); + QVBoxLayout *main_layout = new QVBoxLayout(this); + Signal sig = {.name = "untitled"}; + auto form = new SignalForm(sig, this); + main_layout->addWidget(form); + auto buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel); + main_layout->addWidget(buttonBox); + connect(buttonBox, &QDialogButtonBox::rejected, this, &QDialog::reject); + connect(buttonBox, &QDialogButtonBox::accepted, [=]() { + if (auto msg = const_cast(parser->getMsg(id))) { + if (auto signal = form->getSignal()) { + msg->sigs.push_back(*signal); + } + } + QDialog::accept(); + }); +} diff --git a/tools/cabana/signaledit.h b/tools/cabana/signaledit.h new file mode 100644 index 0000000000..b8140cc93b --- /dev/null +++ b/tools/cabana/signaledit.h @@ -0,0 +1,50 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include "selfdrive/ui/qt/widgets/controls.h" +#include "tools/cabana/parser.h" + +class SignalForm : public QWidget { + Q_OBJECT + +public: + SignalForm(const Signal &sig, QWidget *parent); + std::optional getSignal(); + + QLineEdit *name, *unit, *comment, *val_desc; + QSpinBox *size, *msb, *lsb, *factor, *offset, *min_val, *max_val; + QComboBox *sign, *endianness; +}; + +class SignalEdit : public QWidget { + Q_OBJECT + +public: + SignalEdit(const QString &id, const Signal &sig, const QString &color, QWidget *parent = nullptr); + void save(); + +protected: + void remove(); + + QString id; + QString name_; + QPushButton *plot_btn; + ElidedLabel *title; + SignalForm *form; + QWidget *edit_container; + QPushButton *remove_btn; +}; + +class AddSignalDialog : public QDialog { + Q_OBJECT + +public: + AddSignalDialog(const QString &id, QWidget *parent); +}; diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc new file mode 100644 index 0000000000..957747584c --- /dev/null +++ b/tools/cabana/videowidget.cc @@ -0,0 +1,172 @@ +#include "tools/cabana/videowidget.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "tools/cabana/parser.h" + +inline QString formatTime(int seconds) { + return QDateTime::fromTime_t(seconds).toString(seconds > 60 * 60 ? "hh::mm::ss" : "mm::ss"); +} + +VideoWidget::VideoWidget(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + + // TODO: figure out why the CameraViewWidget crashed occasionally. + cam_widget = new CameraViewWidget("camerad", VISION_STREAM_ROAD, false, this); + cam_widget->setFixedSize(parent->width(), parent->width() / 1.596); + main_layout->addWidget(cam_widget); + + // slider controls + QHBoxLayout *slider_layout = new QHBoxLayout(); + time_label = new QLabel("00:00"); + slider_layout->addWidget(time_label); + + slider = new Slider(this); + slider->setSingleStep(0); + slider->setMinimum(0); + slider->setMaximum(parser->replay->totalSeconds() * 1000); + slider_layout->addWidget(slider); + + total_time_label = new QLabel(formatTime(parser->replay->totalSeconds())); + slider_layout->addWidget(total_time_label); + + main_layout->addLayout(slider_layout); + + // btn controls + QHBoxLayout *control_layout = new QHBoxLayout(); + QPushButton *play = new QPushButton("⏸"); + play->setStyleSheet("font-weight:bold"); + control_layout->addWidget(play); + + QButtonGroup *group = new QButtonGroup(this); + group->setExclusive(true); + for (float speed : {0.1, 0.5, 1., 2.}) { + QPushButton *btn = new QPushButton(QString("%1x").arg(speed), this); + btn->setCheckable(true); + QObject::connect(btn, &QPushButton::clicked, [=]() { parser->replay->setSpeed(speed); }); + control_layout->addWidget(btn); + group->addButton(btn); + if (speed == 1.0) btn->setChecked(true); + } + + main_layout->addLayout(control_layout); + setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + + QObject::connect(parser, &Parser::rangeChanged, this, &VideoWidget::rangeChanged); + QObject::connect(parser, &Parser::updated, this, &VideoWidget::updateState); + QObject::connect(slider, &QSlider::sliderMoved, [=]() { time_label->setText(formatTime(slider->value() / 1000)); }); + QObject::connect(slider, &QSlider::sliderReleased, [this]() { setPosition(slider->value()); }); + QObject::connect(slider, &Slider::setPosition, this, &VideoWidget::setPosition); + + QObject::connect(play, &QPushButton::clicked, [=]() { + bool is_paused = parser->replay->isPaused(); + play->setText(is_paused ? "⏸" : "▶"); + parser->replay->pause(!is_paused); + }); +} + +void VideoWidget::setPosition(int value) { + time_label->setText(formatTime(value / 1000.0)); + parser->seekTo(value / 1000.0); +} + +void VideoWidget::rangeChanged(double min, double max) { + if (!parser->isZoomed()) { + min = 0; + max = parser->replay->totalSeconds(); + } + time_label->setText(formatTime(min)); + total_time_label->setText(formatTime(max)); + slider->setMinimum(min * 1000); + slider->setMaximum(max * 1000); + slider->setValue(parser->currentSec() * 1000); +} + +void VideoWidget::updateState() { + if (!slider->isSliderDown()) { + double current_sec = parser->currentSec(); + time_label->setText(formatTime(current_sec)); + slider->setValue(current_sec * 1000); + } +} + +// Slider +Slider::Slider(QWidget *parent) : QSlider(Qt::Horizontal, parent) { + QTimer *timer = new QTimer(this); + timer->setInterval(2000); + timer->callOnTimeout([this]() { + timeline = parser->replay->getTimeline(); + update(); + }); + timer->start(); +} + +void Slider::sliderChange(QAbstractSlider::SliderChange change) { + if (change == QAbstractSlider::SliderValueChange) { + qreal x = width() * ((value() - minimum()) / double(maximum() - minimum())); + if (x != slider_x) { + slider_x = x; + update(); + } + } else { + QAbstractSlider::sliderChange(change); + } +} + +void Slider::paintEvent(QPaintEvent *ev) { + auto getPaintRange = [this](double begin, double end) -> std::pair { + double total_sec = maximum() - minimum(); + int start_pos = ((std::max((double)minimum(), (double)begin) - minimum()) / total_sec) * width(); + int end_pos = ((std::min((double)maximum(), (double)end) - minimum()) / total_sec) * width(); + return {start_pos, end_pos}; + }; + + QPainter p(this); + const int v_margin = 2; + p.fillRect(rect().adjusted(0, v_margin, 0, -v_margin), QColor(111, 143, 175)); + + for (auto [begin, end, type] : timeline) { + begin *= 1000; + end *= 1000; + if (begin > maximum() || end < minimum()) continue; + + if (type == TimelineType::Engaged) { + auto [start_pos, end_pos] = getPaintRange(begin, end); + p.fillRect(QRect(start_pos, v_margin, end_pos - start_pos, height() - v_margin * 2), QColor(0, 163, 108)); + } + } + for (auto [begin, end, type] : timeline) { + begin *= 1000; + end *= 1000; + if (type == TimelineType::Engaged || begin > maximum() || end < minimum()) continue; + + auto [start_pos, end_pos] = getPaintRange(begin, end); + if (type == TimelineType::UserFlag) { + p.fillRect(QRect(start_pos, height() - v_margin - 3, end_pos - start_pos, 3), Qt::white); + } else { + QColor color(Qt::green); + if (type != TimelineType::AlertInfo) + color = type == TimelineType::AlertWarning ? QColor(255, 195, 0) : QColor(199, 0, 57); + + p.fillRect(QRect(start_pos, height() - v_margin - 3, end_pos - start_pos, 3), color); + } + } + p.setPen(QPen(QColor(88, 24, 69), 3)); + p.drawLine(QPoint{slider_x, 0}, QPoint{slider_x, height()}); +} + +void Slider::mousePressEvent(QMouseEvent *e) { + QSlider::mousePressEvent(e); + if (e->button() == Qt::LeftButton && !isSliderDown()) { + int value = minimum() + ((maximum() - minimum()) * e->x()) / width(); + setValue(value); + emit setPosition(value); + } +} diff --git a/tools/cabana/videowidget.h b/tools/cabana/videowidget.h new file mode 100644 index 0000000000..060565d322 --- /dev/null +++ b/tools/cabana/videowidget.h @@ -0,0 +1,42 @@ +#pragma once + +#include +#include +#include + +#include "selfdrive/ui/qt/widgets/cameraview.h" +#include "tools/replay/replay.h" + +class Slider : public QSlider { + Q_OBJECT + +public: + Slider(QWidget *parent); + void mousePressEvent(QMouseEvent* e) override; + void sliderChange(QAbstractSlider::SliderChange change) override; + +signals: + void setPosition(int value); + +private: + void paintEvent(QPaintEvent *ev) override; + std::vector> timeline; + + int slider_x = -1; +}; + +class VideoWidget : public QWidget { + Q_OBJECT + +public: + VideoWidget(QWidget *parnet = nullptr); + +protected: + void rangeChanged(double min, double max); + void updateState(); + void setPosition(int value); + + CameraViewWidget *cam_widget; + QLabel *time_label, *total_time_label; + Slider *slider; +}; diff --git a/tools/lib/bootlog.py b/tools/lib/bootlog.py index 3515370823..1e474e5dde 100644 --- a/tools/lib/bootlog.py +++ b/tools/lib/bootlog.py @@ -1,6 +1,7 @@ import datetime import functools import re +from typing import List, Optional from tools.lib.auth_config import get_token from tools.lib.api import CommaApi @@ -48,8 +49,15 @@ class Bootlog: return False return self.datetime < b.datetime +def get_bootlog_from_id(bootlog_id: str) -> Optional[Bootlog]: + # TODO: implement an API endpoint for this + bl = Bootlog(bootlog_id) + for b in get_bootlogs(bl.dongle_id): + if b == bl: + return b + return None -def get_bootlogs(dongle_id: str): +def get_bootlogs(dongle_id: str) -> List[Bootlog]: api = CommaApi(get_token()) r = api.get(f'v1/devices/{dongle_id}/bootlogs') return [Bootlog(b) for b in r] diff --git a/tools/replay/SConscript b/tools/replay/SConscript index d3967708fa..9985375688 100644 --- a/tools/replay/SConscript +++ b/tools/replay/SConscript @@ -18,6 +18,7 @@ if arch in ['x86_64', 'Darwin'] or GetOption('extras'): replay_lib_src = ["replay.cc", "consoleui.cc", "camera.cc", "filereader.cc", "logreader.cc", "framereader.cc", "route.cc", "util.cc"] replay_lib = qt_env.Library("qt_replay", replay_lib_src, LIBS=qt_libs, FRAMEWORKS=base_frameworks) + Export('replay_lib') replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv', 'ncurses'] + qt_libs qt_env.Program("replay", ["main.cc"], LIBS=replay_libs, FRAMEWORKS=base_frameworks) diff --git a/tools/replay/logreader.cc b/tools/replay/logreader.cc index 9b7a07a83f..e3d5071412 100644 --- a/tools/replay/logreader.cc +++ b/tools/replay/logreader.cc @@ -46,7 +46,9 @@ LogReader::~LogReader() { #endif } -bool LogReader::load(const std::string &url, std::atomic *abort, bool local_cache, int chunk_size, int retries) { +bool LogReader::load(const std::string &url, std::atomic *abort, + const std::set &allow, + bool local_cache, int chunk_size, int retries) { raw_ = FileReader(local_cache, chunk_size, retries).read(url, abort); if (raw_.empty()) return false; @@ -54,18 +56,26 @@ bool LogReader::load(const std::string &url, std::atomic *abort, bool loca raw_ = decompressBZ2(raw_, abort); if (raw_.empty()) return false; } - return parse(abort); + return parse(allow, abort); } bool LogReader::load(const std::byte *data, size_t size, std::atomic *abort) { raw_.assign((const char *)data, size); - return parse(abort); + return parse({}, abort); } -bool LogReader::parse(std::atomic *abort) { +bool LogReader::parse(const std::set &allow, std::atomic *abort) { try { kj::ArrayPtr words((const capnp::word *)raw_.data(), raw_.size() / sizeof(capnp::word)); while (words.size() > 0 && !(abort && *abort)) { + if (!allow.empty()) { + capnp::FlatArrayMessageReader reader(words); + auto which = reader.getRoot().which(); + if (allow.find(which) == allow.end()) { + words = kj::arrayPtr(reader.getEnd(), words.end()); + continue; + } + } #ifdef HAS_MEMORY_RESOURCE Event *evt = new (mbr_) Event(words); diff --git a/tools/replay/logreader.h b/tools/replay/logreader.h index bd666d0a74..010839af22 100644 --- a/tools/replay/logreader.h +++ b/tools/replay/logreader.h @@ -5,6 +5,8 @@ #include #endif +#include + #include "cereal/gen/cpp/log.capnp.h" #include "system/camerad/cameras/camera_common.h" #include "tools/replay/filereader.h" @@ -50,12 +52,13 @@ class LogReader { public: LogReader(size_t memory_pool_block_size = DEFAULT_EVENT_MEMORY_POOL_BLOCK_SIZE); ~LogReader(); - bool load(const std::string &url, std::atomic *abort = nullptr, bool local_cache = false, int chunk_size = -1, int retries = 0); + bool load(const std::string &url, std::atomic *abort = nullptr, const std::set &allow = {}, + bool local_cache = false, int chunk_size = -1, int retries = 0); bool load(const std::byte *data, size_t size, std::atomic *abort = nullptr); std::vector events; private: - bool parse(std::atomic *abort); + bool parse(const std::set &allow, std::atomic *abort); std::string raw_; #ifdef HAS_MEMORY_RESOURCE std::pmr::monotonic_buffer_resource *mbr_ = nullptr; diff --git a/tools/replay/replay.cc b/tools/replay/replay.cc index c6c78f47ae..80e58b47a3 100644 --- a/tools/replay/replay.cc +++ b/tools/replay/replay.cc @@ -19,6 +19,9 @@ Replay::Replay(QString route, QStringList allow, QStringList block, SubMaster *s if ((allow.empty() || allow.contains(it.name)) && !block.contains(it.name)) { uint16_t which = event_struct.getFieldByName(it.name).getProto().getDiscriminantValue(); sockets_[which] = it.name; + if (!allow.empty() || !block.empty()) { + allow_list.insert((cereal::Event::Which)which); + } s.push_back(it.name); } } @@ -91,17 +94,17 @@ void Replay::updateEvents(const std::function &lambda) { stream_cv_.notify_one(); } -void Replay::seekTo(int seconds, bool relative) { +void Replay::seekTo(double seconds, bool relative) { seconds = relative ? seconds + currentSeconds() : seconds; updateEvents([&]() { - seconds = std::max(0, seconds); - int seg = seconds / 60; + seconds = std::max(double(0.0), seconds); + int seg = (int)seconds / 60; if (segments_.find(seg) == segments_.end()) { rWarning("can't seek to %d s segment %d is invalid", seconds, seg); return true; } - rInfo("seeking to %d s, segment %d", seconds, seg); + rInfo("seeking to %d s, segment %d", (int)seconds, seg); current_segment_ = seg; cur_mono_time_ = route_start_ts_ + seconds * 1e9; return isSegmentMerged(seg); @@ -122,7 +125,9 @@ void Replay::buildTimeline() { for (int i = 0; i < segments_.size() && !exit_; ++i) { LogReader log; - if (!log.load(route_->at(i).qlog.toStdString(), &exit_, !hasFlag(REPLAY_FLAG_NO_FILE_CACHE), 0, 3)) continue; + if (!log.load(route_->at(i).qlog.toStdString(), &exit_, + {cereal::Event::Which::CONTROLS_STATE, cereal::Event::Which::USER_FLAG}, + !hasFlag(REPLAY_FLAG_NO_FILE_CACHE), 0, 3)) continue; for (const Event *e : log.events) { if (e->which == cereal::Event::Which::CONTROLS_STATE) { @@ -215,7 +220,7 @@ void Replay::queueSegment() { if ((seg && !seg->isLoaded()) || !seg) { if (!seg) { rDebug("loading segment %d...", n); - seg = std::make_unique(n, route_->at(n), flags_); + seg = std::make_unique(n, route_->at(n), flags_, allow_list); QObject::connect(seg.get(), &Segment::loadFinished, this, &Replay::segmentLoadFinished); } break; @@ -270,6 +275,9 @@ void Replay::mergeSegments(const SegmentMap::iterator &begin, const SegmentMap:: segments_merged_ = segments_need_merge; return true; }); + if (stream_thread_) { + emit segmentsMerged(); + } } } @@ -306,6 +314,7 @@ void Replay::startStream(const Segment *cur_segment) { camera_server_ = std::make_unique(camera_size); } + emit segmentsMerged(); // start stream thread stream_thread_ = new QThread(); QObject::connect(stream_thread_, &QThread::started, [=]() { stream(); }); @@ -316,6 +325,8 @@ void Replay::startStream(const Segment *cur_segment) { } void Replay::publishMessage(const Event *e) { + if (event_filter && event_filter(e, filter_opaque)) return; + if (sm == nullptr) { auto bytes = e->bytes(); int ret = pm->send(sockets_[e->which], (capnp::byte *)bytes.begin(), bytes.size()); @@ -382,7 +393,7 @@ void Replay::stream() { if (cur_which < sockets_.size() && sockets_[cur_which] != nullptr) { // keep time - long etime = cur_mono_time_ - evt_start_ts; + long etime = (cur_mono_time_ - evt_start_ts) / speed_; long rtime = nanos_since_boot() - loop_start_ts; long behind_ns = etime - rtime; // if behind_ns is greater than 1 second, it means that an invalid segemnt is skipped by seeking/replaying diff --git a/tools/replay/replay.h b/tools/replay/replay.h index e86c453f7e..725bd1a27e 100644 --- a/tools/replay/replay.h +++ b/tools/replay/replay.h @@ -32,6 +32,7 @@ enum class FindFlag { }; enum class TimelineType { None, Engaged, AlertInfo, AlertWarning, AlertCritical, UserFlag }; +typedef bool (*replayEventFilter)(const Event *, void *); class Replay : public QObject { Q_OBJECT @@ -45,15 +46,26 @@ public: void stop(); void pause(bool pause); void seekToFlag(FindFlag flag); - void seekTo(int seconds, bool relative); + void seekTo(double seconds, bool relative); inline bool isPaused() const { return paused_; } + // the filter is called in streaming thread.try to return quickly from it to avoid blocking streaming. + // the filter function must return true if the event should be filtered. + // otherwise it must return false. + inline void installEventFilter(replayEventFilter filter, void *opaque) { + filter_opaque = opaque; + event_filter = filter; + } inline bool hasFlag(REPLAY_FLAGS flag) const { return flags_ & flag; } inline void addFlag(REPLAY_FLAGS flag) { flags_ |= flag; } inline void removeFlag(REPLAY_FLAGS flag) { flags_ &= ~flag; } inline const Route* route() const { return route_.get(); } - inline int currentSeconds() const { return (cur_mono_time_ - route_start_ts_) / 1e9; } + inline double currentSeconds() const { return double(cur_mono_time_ - route_start_ts_) / 1e9; } + inline uint64_t routeStartTime() const { return route_start_ts_; } inline int toSeconds(uint64_t mono_time) const { return (mono_time - route_start_ts_) / 1e9; } inline int totalSeconds() const { return segments_.size() * 60; } + inline void setSpeed(float speed) { speed_ = speed; } + inline float getSpeed() const { return speed_; } + inline const std::vector *events() const { return events_.get(); } inline const std::string &carFingerprint() const { return car_fingerprint_; } inline const std::vector> getTimeline() { std::lock_guard lk(timeline_lock); @@ -62,6 +74,7 @@ public: signals: void streamStarted(); + void segmentsMerged(); protected slots: void segmentLoadFinished(bool success); @@ -95,7 +108,7 @@ protected: bool paused_ = false; bool events_updated_ = false; uint64_t route_start_ts_ = 0; - uint64_t cur_mono_time_ = 0; + std::atomic cur_mono_time_ = 0; std::unique_ptr> events_; std::unique_ptr> new_events_; std::vector segments_merged_; @@ -111,5 +124,9 @@ protected: std::mutex timeline_lock; QFuture timeline_future; std::vector> timeline; + std::set allow_list; std::string car_fingerprint_; + float speed_ = 1.0; + replayEventFilter event_filter = nullptr; + void *filter_opaque = nullptr; }; diff --git a/tools/replay/route.cc b/tools/replay/route.cc index c91b27ae81..f0d6ec5a12 100644 --- a/tools/replay/route.cc +++ b/tools/replay/route.cc @@ -99,7 +99,9 @@ void Route::addFileToSegment(int n, const QString &file) { // class Segment -Segment::Segment(int n, const SegmentFile &files, uint32_t flags) : seg_num(n), flags(flags) { +Segment::Segment(int n, const SegmentFile &files, uint32_t flags, + const std::set &allow) + : seg_num(n), flags(flags), allow(allow) { // [RoadCam, DriverCam, WideRoadCam, log]. fallback to qcamera/qlog const std::array file_list = { (flags & REPLAY_FLAG_QCAMERA) || files.road_cam.isEmpty() ? files.qcamera : files.road_cam, @@ -130,7 +132,7 @@ void Segment::loadFile(int id, const std::string file) { success = frames[id]->load(file, flags & REPLAY_FLAG_NO_HW_DECODER, &abort_, local_cache, 20 * 1024 * 1024, 3); } else { log = std::make_unique(); - success = log->load(file, &abort_, local_cache, 0, 3); + success = log->load(file, &abort_, allow, local_cache, 0, 3); } if (!success) { diff --git a/tools/replay/route.h b/tools/replay/route.h index 6ca9c3b883..6b78ebad87 100644 --- a/tools/replay/route.h +++ b/tools/replay/route.h @@ -47,7 +47,7 @@ class Segment : public QObject { Q_OBJECT public: - Segment(int n, const SegmentFile &files, uint32_t flags); + Segment(int n, const SegmentFile &files, uint32_t flags, const std::set &allow = {}); ~Segment(); inline bool isLoaded() const { return !loading_ && !abort_; } @@ -65,4 +65,5 @@ protected: std::atomic loading_ = 0; QFutureSynchronizer synchronizer_; uint32_t flags; + std::set allow; }; diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index c400eb93f5..e436e92292 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -244,11 +244,13 @@ class CarlaBridge: def __init__(self, arguments): set_params_enabled() + self.params = Params() + msg = messaging.new_message('liveCalibration') msg.liveCalibration.validBlocks = 20 msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] - Params().put("CalibrationParams", msg.to_bytes()) - Params().put_bool("WideCameraOnly", not arguments.dual_camera) + self.params.put("CalibrationParams", msg.to_bytes()) + self.params.put_bool("WideCameraOnly", not arguments.dual_camera) self._args = arguments self._carla_objects = [] @@ -363,6 +365,7 @@ class CarlaBridge: gps_bp = blueprint_library.find('sensor.other.gnss') gps = world.spawn_actor(gps_bp, transform, attach_to=vehicle) gps.listen(lambda gps: gps_callback(gps, vehicle_state)) + self.params.put_bool("UbloxAvailable", True) self._carla_objects.extend([imu, gps]) # launch fake car threads diff --git a/tools/sim/launch_openpilot.sh b/tools/sim/launch_openpilot.sh index 15f45b4cc2..adabc40c2e 100755 --- a/tools/sim/launch_openpilot.sh +++ b/tools/sim/launch_openpilot.sh @@ -3,6 +3,7 @@ export PASSIVE="0" export NOBOARD="1" export SIMULATION="1" +export SKIP_FW_QUERY="1" export FINGERPRINT="HONDA CIVIC 2016" export BLOCK="camerad,loggerd,encoderd" diff --git a/tools/ubuntu_setup.sh b/tools/ubuntu_setup.sh index 863b853718..7e021bcc23 100755 --- a/tools/ubuntu_setup.sh +++ b/tools/ubuntu_setup.sh @@ -63,6 +63,7 @@ function install_ubuntu_common_requirements() { qttools5-dev-tools \ libqt5sql5-sqlite \ libqt5svg5-dev \ + libqt5charts5-dev \ libqt5x11extras5-dev \ libreadline-dev \ libdw1 \