diff --git a/opendbc_repo b/opendbc_repo index dfcfacb66..bf0aba991 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit dfcfacb667c6ff131d0e620c36e118f14a84dc4a +Subproject commit bf0aba991acbe14dd164ddfc8cc272cebf794544 diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 34f0283ce..4c44e4328 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -124,7 +124,7 @@ class Car: self.CP.alternativeExperience = 0 # mads - set_alternative_experience(self.CP, self.params) + set_alternative_experience(self.CP, self.CP_SP, self.params) set_car_specific_params(self.CP, self.CP_SP, self.params) # Dynamic Experimental Control diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc index 72e7a5c8c..749bc9e3f 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.cc @@ -52,12 +52,16 @@ void MadsSettings::updateToggles(bool _offroad) { ); auto cp_bytes = params.get("CarParamsPersistent"); - if (!cp_bytes.empty()) { + auto cp_sp_bytes = params.get("CarParamsSPPersistent"); + if (!cp_bytes.empty() && !cp_sp_bytes.empty()) { AlignedBuffer aligned_buf; + AlignedBuffer aligned_buf_sp; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); + capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size())); cereal::CarParams::Reader CP = cmsg.getRoot(); + cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot(); - if (isBrandInList(CP.getBrand(), mads_limited_settings_brands)) { + if (madsLimitedSettings(CP, CP_SP)) { params.remove("MadsMainCruiseAllowed"); params.putBool("MadsUnifiedEngagementMode", true); params.put("MadsSteeringMode", std::to_string(static_cast(MadsSteeringMode::DISENGAGE))); diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h index af624865c..9cf62a0ff 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h @@ -12,7 +12,16 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" -const std::vector mads_limited_settings_brands = {"rivian", "tesla"}; +inline bool madsLimitedSettings(const cereal::CarParams::Reader &CP, const cereal::CarParamsSP::Reader &CP_SP) { + if (CP.getBrand() == "rivian") { + return true; + } + if (CP.getBrand() == "tesla") { + return !(CP_SP.getFlags() & 1); // 1 == TeslaFlagsSP.HAS_VEHICLE_BUS + } + + return false; +} enum class MadsSteeringMode { REMAIN_ACTIVE = 0, diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc index ae954d753..59636ca5a 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc @@ -134,12 +134,16 @@ void LateralPanel::updateToggles(bool _offroad) { } auto cp_bytes = params.get("CarParamsPersistent"); - if (!cp_bytes.empty()) { + auto cp_sp_bytes = params.get("CarParamsSPPersistent"); + if (!cp_bytes.empty() && !cp_sp_bytes.empty()) { AlignedBuffer aligned_buf; + AlignedBuffer aligned_buf_sp; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); + capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size())); cereal::CarParams::Reader CP = cmsg.getRoot(); + cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot(); - if (isBrandInList(CP.getBrand(), mads_limited_settings_brands)) { + if (madsLimitedSettings(CP, CP_SP)) { madsToggle->setDescription(descriptionBuilder(STATUS_MADS_SETTINGS_LIMITED_COMPATIBILITY, MADS_BASE_DESC)); } else { madsToggle->setDescription(descriptionBuilder(STATUS_MADS_SETTINGS_FULL_COMPATIBILITY, MADS_BASE_DESC)); diff --git a/sunnypilot/mads/helpers.py b/sunnypilot/mads/helpers.py index 70b590a8b..f9c4057ba 100644 --- a/sunnypilot/mads/helpers.py +++ b/sunnypilot/mads/helpers.py @@ -9,6 +9,10 @@ from openpilot.common.params import Params from opendbc.car import structs from opendbc.safety import ALTERNATIVE_EXPERIENCE from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP, HyundaiSafetyFlagsSP +from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP + + +MADS_NO_ACC_MAIN_BUTTON = ("rivian", "tesla") class MadsSteeringModeOnBrake: @@ -17,20 +21,25 @@ class MadsSteeringModeOnBrake: DISENGAGE = 2 -def get_mads_limited_brands(CP: structs.CarParams) -> bool: - return CP.brand in ("rivian", "tesla") +def get_mads_limited_brands(CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> bool: + if CP.brand == 'rivian': + return True + if CP.brand == 'tesla': + return not CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS + + return False -def read_steering_mode_param(CP: structs.CarParams, params: Params): - if get_mads_limited_brands(CP): +def read_steering_mode_param(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params): + if get_mads_limited_brands(CP, CP_SP): return MadsSteeringModeOnBrake.DISENGAGE return params.get("MadsSteeringMode", return_default=True) -def set_alternative_experience(CP: structs.CarParams, params: Params): +def set_alternative_experience(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params): enabled = params.get_bool("Mads") - steering_mode = read_steering_mode_param(CP, params) + steering_mode = read_steering_mode_param(CP, CP_SP, params) if enabled: CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS @@ -53,9 +62,12 @@ def set_car_specific_params(CP: structs.CarParams, CP_SP: structs.CarParamsSP, p # MADS Partial Support # MADS is currently partially supported for these platforms due to lack of consistent states to engage controls # Only MadsSteeringModeOnBrake.DISENGAGE is supported for these platforms - # TODO-SP: To enable MADS full support for Rivian/Tesla, identify consistent signals for MADS toggling - mads_partial_support = get_mads_limited_brands(CP) + # TODO-SP: To enable MADS full support for Rivian and most Tesla, identify consistent signals for MADS toggling + mads_partial_support = get_mads_limited_brands(CP, CP_SP) if mads_partial_support: params.put("MadsSteeringMode", 2) params.put_bool("MadsUnifiedEngagementMode", True) + + # no ACC MAIN button for these brands + if CP.brand in MADS_NO_ACC_MAIN_BUTTON: params.remove("MadsMainCruiseAllowed") diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index b75bd8ccd..21e7f7d3e 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -10,7 +10,7 @@ from cereal import log, custom from opendbc.car import structs from opendbc.car.hyundai.values import HyundaiFlags from openpilot.common.params import Params -from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param, get_mads_limited_brands +from openpilot.sunnypilot.mads.helpers import MadsSteeringModeOnBrake, read_steering_mode_param, MADS_NO_ACC_MAIN_BUTTON from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState @@ -27,6 +27,7 @@ IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) class ModularAssistiveDrivingSystem: def __init__(self, selfdrive): self.CP = selfdrive.CP + self.CP_SP = selfdrive.CP_SP self.params = selfdrive.params self.enabled = False @@ -43,14 +44,16 @@ class ModularAssistiveDrivingSystem: if self.CP.brand == "hyundai": if self.CP.flags & (HyundaiFlags.HAS_LDA_BUTTON | HyundaiFlags.CANFD): self.allow_always = True + if self.CP.brand == "tesla": + self.allow_always = True - if get_mads_limited_brands(self.CP): + if self.CP.brand in MADS_NO_ACC_MAIN_BUTTON: self.no_main_cruise = True # read params on init self.enabled_toggle = self.params.get_bool("Mads") self.main_enabled_toggle = self.params.get_bool("MadsMainCruiseAllowed") - self.steering_mode_on_brake = read_steering_mode_param(self.CP, self.params) + self.steering_mode_on_brake = read_steering_mode_param(self.CP, self.CP_SP, self.params) self.unified_engagement_mode = self.params.get_bool("MadsUnifiedEngagementMode") def read_params(self):