From c44f915df88acd08ec3f31b704ff03676bf6fbd8 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 1 Jul 2024 14:39:16 -0400 Subject: [PATCH 1/3] Bump submodules --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 757a111a9f..2b1869bb1b 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 757a111a9fefb7d274c057b9b75964bcefd82aca +Subproject commit 2b1869bb1b2d2cc3c38ce0ac297dd114649a5e01 From 9097997b7672746c534ce4149375f9e01b1a292d Mon Sep 17 00:00:00 2001 From: rav4 kumar Date: Mon, 1 Jul 2024 18:50:12 +0000 Subject: [PATCH 2/3] Toyota: Enhance BSM support --- CHANGELOGS.md | 9 +++ common/params.cc | 1 + selfdrive/car/toyota/carcontroller.py | 54 ++++++++++++++- selfdrive/car/toyota/carstate.py | 56 +++++++++++++++ selfdrive/car/toyota/interface.py | 6 ++ selfdrive/car/toyota/toyotacan.py | 12 ++++ selfdrive/car/toyota/values.py | 2 + .../qt/offroad/sunnypilot/vehicle_settings.cc | 69 +++++++++++++++++++ .../qt/offroad/sunnypilot/vehicle_settings.h | 16 +++++ system/manager/manager.py | 1 + 10 files changed, 225 insertions(+), 1 deletion(-) diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 005afc9e8d..b9a95ff6f4 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -4,6 +4,15 @@ sunnypilot - 0.9.8.0 (2024-xx-xx) ************************ * UPDATED: Synced with commaai's openpilot * master commit b45caf4 (June 14, 2024) +* NEW❗: Toyota - Enhanced Blind Spot Monitor (BSM) thanks to arne182, rav4kumar, and eFiniLan! + * Enables Blind Spot Monitor (BSM) signals parsing in sunnypilot using the factory Blind Spot Monitor (BSM) + * sunnypilot will use debugging CAN messages to receive unfiltered BSM signals, allowing detection of more objects + * Supported platforms + * RAV4 TSS1, equipped with factory Blind Spot Monitoring (BSM) + * Lexus LSS1, equipped with factory Blind Spot Monitoring (BSM) + * Toyota TSS1/1.5, equipped with factory Blind Spot Monitoring (BSM) + * Prius TSS2, equipped with factory Blind Spot Monitoring (BSM) + * NOTE: Only enable this feature if your Toyota/Lexus vehicle has factory Blind Spot Monitor equipped, and mentioned in the supported platforms list * Kia Ceed Plug-in Hybrid Non-SCC 2022 support thanks to TerminatorNL! sunnypilot - 0.9.7.1 (2024-06-13) diff --git a/common/params.cc b/common/params.cc index 73906d35f4..c09c75aa4a 100644 --- a/common/params.cc +++ b/common/params.cc @@ -317,6 +317,7 @@ std::unordered_map keys = { {"TorqueFriction", PERSISTENT | BACKUP}, {"TorqueMaxLatAccel", PERSISTENT | BACKUP}, {"TorquedOverride", PERSISTENT | BACKUP}, + {"ToyotaEnhancedBsm", PERSISTENT | BACKUP}, {"ToyotaSnG", PERSISTENT | BACKUP}, {"ToyotaTSS2Long", PERSISTENT | BACKUP}, {"TrueVEgoUi", PERSISTENT | BACKUP}, diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 71179dd6af..b83ef9b3c2 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -6,7 +6,7 @@ from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_st from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.toyota import toyotacan from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ - MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \ + MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, ToyotaFlagsSP, \ UNSUPPORTED_DSU_CAR from opendbc.can.packer import CANPacker @@ -26,6 +26,9 @@ MAX_USER_TORQUE = 500 MAX_LTA_ANGLE = 94.9461 # deg MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows some resistance when changing lanes +LEFT_BLINDSPOT = b"\x41" +RIGHT_BLINDSPOT = b"\x42" + class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): @@ -48,6 +51,10 @@ class CarController(CarControllerBase): self._reverse_acc_change = self.param_s.get_bool("ReverseAccChange") self._sng_hack = self.param_s.get_bool("ToyotaSnG") + self.left_blindspot_debug_enabled = False + self.right_blindspot_debug_enabled = False + self.last_blindspot_frame = 0 + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl @@ -205,6 +212,9 @@ class CarController(CarControllerBase): if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: can_sends.append([0x750, 0, b"\x0F\x02\x3E\x00\x00\x00\x00\x00", 0]) + if self.CP.spFlags & ToyotaFlagsSP.SP_ENHANCED_BSM and self.frame > 200: + can_sends.extend(self.create_enhanced_bsm_messages(CS, 20, True)) + new_actuators = actuators.as_builder() new_actuators.steer = apply_steer / self.params.STEER_MAX new_actuators.steerOutputCan = apply_steer @@ -214,3 +224,45 @@ class CarController(CarControllerBase): self.frame += 1 return new_actuators, can_sends + + # Enhanced BSM (@arne182, @rav4kumar) + def create_enhanced_bsm_messages(self, CS: car.CarState, e_bsm_rate: int = 20, always_on: bool = True): + # let's keep all the commented out code for easy debug purpose for future. + can_sends = [] + + # left bsm + if not self.left_blindspot_debug_enabled: + if always_on or CS.out.vEgo > 6: # eagle eye camera will stop working if left bsm is switched on under 6m/s + can_sends.append(toyotacan.create_set_bsm_debug_mode(LEFT_BLINDSPOT, True)) + self.left_blindspot_debug_enabled = True + # print("bsm debug left, on") + else: + if not always_on and self.frame - self.last_blindspot_frame > 50: + can_sends.append(toyotacan.create_set_bsm_debug_mode(LEFT_BLINDSPOT, False)) + self.left_blindspot_debug_enabled = False + # print("bsm debug left, off") + if self.frame % e_bsm_rate == 0: + can_sends.append(toyotacan.create_bsm_polling_status(LEFT_BLINDSPOT)) + # if CS.out.leftBlinker: + self.last_blindspot_frame = self.frame + # print(self.last_blindspot_frame) + # print("bsm poll left") + # right bsm + if not self.right_blindspot_debug_enabled: + if always_on or CS.out.vEgo > 6: # eagle eye camera will stop working if right bsm is switched on under 6m/s + can_sends.append(toyotacan.create_set_bsm_debug_mode(RIGHT_BLINDSPOT, True)) + self.right_blindspot_debug_enabled = True + # print("bsm debug right, on") + else: + if not always_on and self.frame - self.last_blindspot_frame > 50: + can_sends.append(toyotacan.create_set_bsm_debug_mode(RIGHT_BLINDSPOT, False)) + self.right_blindspot_debug_enabled = False + # print("bsm debug right, off") + if self.frame % e_bsm_rate == e_bsm_rate / 2: + can_sends.append(toyotacan.create_bsm_polling_status(RIGHT_BLINDSPOT)) + # if CS.out.rightBlinker: + self.last_blindspot_frame = self.frame + # print(self.last_blindspot_frame) + # print("bsm poll right") + + return can_sends diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 188964d667..fded06c1dc 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -69,6 +69,18 @@ class CarState(CarStateBase): self.zss_angle_offset = 0. self.zss_threshold_count = 0 + self._left_blindspot = False + self._left_blindspot_d1 = 0 + self._left_blindspot_d2 = 0 + self._left_blindspot_counter = 0 + + self._right_blindspot = False + self._right_blindspot_d1 = 0 + self._right_blindspot_d2 = 0 + self._right_blindspot_counter = 0 + + self.frame = 0 + def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -245,8 +257,12 @@ class CarState(CarStateBase): else: self.distance_button = cp.vl["SDSU"]["FD_BUTTON"] + if self.CP.spFlags & ToyotaFlagsSP.SP_ENHANCED_BSM and self.frame > 199: + ret.leftBlindspot, ret.rightBlindspot = self.sp_get_enhanced_bsm(cp) + self._update_traffic_signals(cp_cam) ret.cruiseState.speedLimit = self._calculate_speed_limit() + self.frame += 1 return ret @@ -328,6 +344,43 @@ class CarState(CarStateBase): return self._spdval1 * CV.MPH_TO_MS return 0 + # Enhanced BSM (@arne182, @rav4kumar) + def sp_get_enhanced_bsm(self, cp): + # Let's keep all the commented out code for easy debug purposes in the future. + distance_1 = cp.vl["DEBUG"].get('BLINDSPOTD1') + distance_2 = cp.vl["DEBUG"].get('BLINDSPOTD2') + side = cp.vl["DEBUG"].get('BLINDSPOTSIDE') + + if all(val is not None for val in [distance_1, distance_2, side]): + if side == 65: # left blind spot + if distance_1 != self._left_blindspot_d1 or distance_2 != self._left_blindspot_d2: + self._left_blindspot_d1 = distance_1 + self._left_blindspot_d2 = distance_2 + self._left_blindspot_counter = 100 + self._left_blindspot = distance_1 > 10 or distance_2 > 10 + + elif side == 66: # right blind spot + if distance_1 != self._right_blindspot_d1 or distance_2 != self._right_blindspot_d2: + self._right_blindspot_d1 = distance_1 + self._right_blindspot_d2 = distance_2 + self._right_blindspot_counter = 100 + self._right_blindspot = distance_1 > 10 or distance_2 > 10 + + # update counters + self._left_blindspot_counter = max(0, self._left_blindspot_counter - 1) + self._right_blindspot_counter = max(0, self._right_blindspot_counter - 1) + + # reset blind spot status if counter reaches 0 + if self._left_blindspot_counter == 0: + self._left_blindspot = False + self._left_blindspot_d1 = self._left_blindspot_d2 = 0 + + if self._right_blindspot_counter == 0: + self._right_blindspot = False + self._right_blindspot_d1 = self._right_blindspot_d2 = 0 + + return self._left_blindspot, self._right_blindspot + @staticmethod def get_can_parser(CP): messages = [ @@ -384,6 +437,9 @@ class CarState(CarStateBase): if CP.spFlags & ToyotaFlagsSP.SP_ZSS: messages.append(("SECONDARY_STEER_ANGLE", 0)) + if CP.spFlags & ToyotaFlagsSP.SP_ENHANCED_BSM: + messages.append(("DEBUG", 65)) + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0) @staticmethod diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 15bc8de22a..5b7cadd2ea 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -178,6 +178,12 @@ class CarInterface(CarInterfaceBase): tune.kiBP = [0., 5., 35.] tune.kiV = [3.6, 2.4, 1.5] + if Params().get_bool("ToyotaEnhancedBsm"): + ret.spFlags |= ToyotaFlagsSP.SP_ENHANCED_BSM.value + + if candidate == CAR.TOYOTA_PRIUS_TSS2: + ret.spFlags |= ToyotaFlagsSP.SP_NEED_DEBUG_BSM.value + return ret @staticmethod diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index d42f807357..abde32bc07 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -1,4 +1,5 @@ from cereal import car +from openpilot.selfdrive.car import make_can_msg SteerControlType = car.CarParams.SteerControlType @@ -118,3 +119,14 @@ def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_dep ]}) return packer.make_can_msg("LKAS_HUD", 0, values) + + +def create_set_bsm_debug_mode(lr_blindspot, enabled): + dat = b"\x02\x10\x60\x00\x00\x00\x00" if enabled else b"\x02\x10\x01\x00\x00\x00\x00" + dat = lr_blindspot + dat + + return make_can_msg(0x750, dat, 0) + + +def create_bsm_polling_status(lr_blindspot): + return make_can_msg(0x750, lr_blindspot + b"\x02\x21\x69\x00\x00\x00\x00", 0) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 068a52a275..6952bb41bd 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -62,6 +62,8 @@ class ToyotaFlags(IntFlag): class ToyotaFlagsSP(IntFlag): SP_ZSS = 1 + SP_ENHANCED_BSM = 2 + SP_NEED_DEBUG_BSM = 4 class Footnote(Enum): diff --git a/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc index a0bea9d52b..4d75ad818b 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc @@ -124,6 +124,15 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge toyotaTss2LongTune->setConfirmation(true, false); addItem(toyotaTss2LongTune); + toyotaEnhancedBsm = new ParamControl( + "ToyotaEnhancedBsm", + tr("Enable Enhanced Blind Spot Monitor"), + "", + "../assets/offroad/icon_blank.png" + ); + toyotaEnhancedBsm->setConfirmation(true, false); + addItem(toyotaEnhancedBsm); + auto toyotaSngHack = new ParamControl( "ToyotaSnG", tr("Enable Toyota Stop and Go Hack"), @@ -146,9 +155,69 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge // trigger offroadTransition when going onroad/offroad connect(uiState(), &UIState::offroadTransition, [=](bool offroad) { + is_onroad = !offroad; hkgSmoothStop->setEnabled(offroad); toyotaTss2LongTune->setEnabled(offroad); + toyotaEnhancedBsm->setEnabled(offroad); toyotaSngHack->setEnabled(offroad); volkswagenCCOnly->setEnabled(offroad); + updateToggles(); }); } + +void SPVehiclesTogglesPanel::showEvent(QShowEvent *event) { + updateToggles(); +} + +void SPVehiclesTogglesPanel::updateToggles() { + if (!isVisible()) { + return; + } + + // Toyota: Enhanced Blind Spot Monitor + QString ebsm_init = "⚠️ " + tr("Start the car to check car compatibility") + ""; + QString ebsm_not_required = "✅ " + tr("This platform is already supported, therefore no need to enable this toggle") + ""; + QString ebsm_not_supported = "⚠️ " + tr("This platform is not supported") + ""; + QString ebsm_supported = "✅ " + tr("This platform can be supported") + ""; + + auto cp_bytes = params.get("CarParamsPersistent"); + if (!cp_bytes.empty()) { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); + cereal::CarParams::Reader CP = cmsg.getRoot(); + + // Toyota: Enhanced Blind Spot Monitor + { + if (CP.getCarName() == "toyota") { + if (CP.getEnableBsm() && !(CP.getSpFlags() & 4)) { // 4 = ToyotaFlagsSP.SP_NEED_DEBUG_BSM + toyotaEnhancedBsm->setDescription(toyotaEnhancedBsmDesciptionBuilder(ebsm_not_required)); + toyotaEnhancedBsm->showDescription(); + toyotaEnhancedBsm->setEnabled(false); + params.remove("ToyotaEnhancedBsm"); + } else { + toyotaEnhancedBsm->setDescription(toyotaEnhancedBsmDesciptionBuilder(ebsm_supported)); + toyotaEnhancedBsm->showDescription(); + toyotaEnhancedBsm->setEnabled(true); + } + } else { + toyotaEnhancedBsm->setDescription(toyotaEnhancedBsmDesciptionBuilder(ebsm_not_supported)); + toyotaEnhancedBsm->showDescription(); + toyotaEnhancedBsm->setEnabled(false); + params.remove("ToyotaEnhancedBsm"); + } + } + + toyotaEnhancedBsm->refresh(); + } else { + toyotaEnhancedBsm->setEnabled(false); + params.remove("ToyotaEnhancedBsm"); + + toyotaEnhancedBsm->setDescription(toyotaEnhancedBsmDesciptionBuilder(ebsm_init)); + toyotaEnhancedBsm->showDescription(); + } + + // override toggle state when onroad/offroad + if (is_onroad) { + toyotaEnhancedBsm->setEnabled(false); + } +} diff --git a/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.h b/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.h index 26a7eba4f5..b210225f49 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.h +++ b/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.h @@ -35,9 +35,25 @@ class SPVehiclesTogglesPanel : public ListWidget { Q_OBJECT public: explicit SPVehiclesTogglesPanel(VehiclePanel *parent); + void showEvent(QShowEvent *event) override; + +public slots: + void updateToggles(); private: Params params; + bool is_onroad = false; ParamControl *stockLongToyota; + ParamControl *toyotaEnhancedBsm; + + const QString toyotaEnhancedBsmDescription = QString("%1

" + "%2") + .arg(tr("sunnypilot will use debugging CAN messages to receive unfiltered BSM signals, allowing detection of more objects.")) + .arg(tr("Tested on RAV4 TSS1, Lexus LSS1, Toyota TSS1/1.5, and Prius TSS2.")); + + QString toyotaEnhancedBsmDesciptionBuilder(const QString &custom_description) { + QString description = "" + custom_description + "

" + toyotaEnhancedBsmDescription; + return description; + } }; diff --git a/system/manager/manager.py b/system/manager/manager.py index f3a5b1b366..20e5c33708 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -97,6 +97,7 @@ def manager_init() -> None: ("TorqueDeadzoneDeg", "0"), ("TorqueFriction", "1"), ("TorqueMaxLatAccel", "250"), + ("ToyotaEnhancedBsm", "0"), ("TrueVEgoUi", "0"), ("TurnSpeedControl", "0"), ("TurnVisionControl", "0"), From e5665613cb4e355a569247fe76928ff70d982efd Mon Sep 17 00:00:00 2001 From: rav4 kumar Date: Mon, 1 Jul 2024 18:54:03 +0000 Subject: [PATCH 3/3] Toyota Longitudinal: New TSS2 Tuning --- CHANGELOGS.md | 2 ++ selfdrive/car/toyota/interface.py | 29 ++++++++++++++++++++--------- 2 files changed, 22 insertions(+), 9 deletions(-) diff --git a/CHANGELOGS.md b/CHANGELOGS.md index b9a95ff6f4..0c88c77ffa 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -13,6 +13,8 @@ sunnypilot - 0.9.8.0 (2024-xx-xx) * Toyota TSS1/1.5, equipped with factory Blind Spot Monitoring (BSM) * Prius TSS2, equipped with factory Blind Spot Monitoring (BSM) * NOTE: Only enable this feature if your Toyota/Lexus vehicle has factory Blind Spot Monitor equipped, and mentioned in the supported platforms list +* UPDATED: Toyota: TSS2 longitudinal: Custom Tuning + * Re-tuned and tested by the community (July 1, 2024) * Kia Ceed Plug-in Hybrid Non-SCC 2022 support thanks to TerminatorNL! sunnypilot - 0.9.7.1 (2024-06-13) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 5b7cadd2ea..056d73bd69 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -160,23 +160,34 @@ class CarInterface(CarInterfaceBase): sp_tss2_long_tune = Params().get_bool("ToyotaTSS2Long") + # hand tuned (July 1, 2024) + def custom_tss2_longitudinal_tuning(): + ret.vEgoStopping = 0.01 + ret.vEgoStarting = 0.01 + ret.stoppingDecelRate = 0.35 + + def default_tss2_longitudinal_tuning(): + ret.vEgoStopping = 0.25 + ret.vEgoStarting = 0.25 + ret.stoppingDecelRate = 0.3 # reach stopping target smoothly + + def default_longitudinal_tuning(): + tune.kiBP = [0., 5., 35.] + tune.kiV = [3.6, 2.4, 1.5] + tune = ret.longitudinalTuning if candidate in TSS2_CAR or ret.enableGasInterceptorDEPRECATED: if sp_tss2_long_tune: - tune.kpBP = [0., 5., 20., 30.] - tune.kpV = [1.3, 1.0, 0.7, 0.1] - tune.kiBP = [0., 1., 2., 3., 4., 5., 12., 20., 27., 40.] - tune.kiV = [.348, .3361, .3168, .2831, .2571, .226, .198, .17, .10, .01] + tune.kiBP = [0., 5., 12., 20., 27., 36., 50] + tune.kiV = [0.35, 0.23, 0.20, 0.17, 0.10, 0.07, 0.01] + custom_tss2_longitudinal_tuning() else: tune.kpV = [0.0] tune.kiV = [0.5] if candidate in TSS2_CAR: - ret.vEgoStopping = 0.25 - ret.vEgoStarting = 0.25 - ret.stoppingDecelRate = 0.3 # reach stopping target smoothly + default_tss2_longitudinal_tuning() else: - tune.kiBP = [0., 5., 35.] - tune.kiV = [3.6, 2.4, 1.5] + default_longitudinal_tuning() if Params().get_bool("ToyotaEnhancedBsm"): ret.spFlags |= ToyotaFlagsSP.SP_ENHANCED_BSM.value