From db0ce99c016cbd7a8668d837702999e57f183c47 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 15 Mar 2025 17:55:41 -0400 Subject: [PATCH] Car interface: custom safety params (#659) * init * do it for hyundai * Bump submodules * no complaints * use latest opendbc pr ref * don't need this for leaf for now * bump opendbc * gotta set the sp safety flags in test models and safety replay! * actually fetch CP_SP * set safety param for lda button availability (lol forgot) * use GET_FLAG * Add TODO-SP * bump submodules --- cereal/custom.capnp | 3 ++- panda | 2 +- selfdrive/car/tests/test_models.py | 3 +++ selfdrive/pandad/panda.cc | 4 ++-- selfdrive/pandad/panda.h | 2 +- selfdrive/pandad/panda_safety.cc | 21 ++++++++++++++------- selfdrive/pandad/pandad.h | 4 ++-- sunnypilot/mads/helpers.py | 5 ++--- 8 files changed, 27 insertions(+), 17 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 36cc2c9498..524ecdd657 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -136,7 +136,8 @@ struct OnroadEventSP @0xda96579883444c35 { } struct CarParamsSP @0x80ae746ee2596b11 { - flags @0 :UInt32; # flags for car specific quirks in sunnypilot + flags @0 :UInt32; # flags for car specific quirks in sunnypilot + safetyParam @1 : Int16; # flags for sunnypilot's custom safety flags } struct CarControlSP @0xa5cd762cd951a455 { diff --git a/panda b/panda index 5479e1572a..ccb9581b1c 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 5479e1572ad1d2733c54e0acdd1a8fa6a960fab9 +Subproject commit ccb9581b1ce08ff899b50d986c5e2b155ce8c339 diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 7698c12c07..573d818365 100644 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -172,6 +172,9 @@ class TestCarModelBase(unittest.TestCase): # TODO: check safetyModel is in release panda build self.safety = libsafety_py.libsafety + safety_param_sp = self.CP_SP.safetyParam + self.safety.set_current_safety_param_sp(safety_param_sp) + cfg = self.CP.safetyConfigs[-1] set_status = self.safety.set_safety_hooks(cfg.safetyModel.raw, cfg.safetyParam) self.assertEqual(0, set_status, f"failed to set safetyModel {cfg}") diff --git a/selfdrive/pandad/panda.cc b/selfdrive/pandad/panda.cc index 45d835e5a4..8d8984c2d2 100644 --- a/selfdrive/pandad/panda.cc +++ b/selfdrive/pandad/panda.cc @@ -62,8 +62,8 @@ void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16 handle->control_write(0xdc, (uint16_t)safety_model, safety_param); } -void Panda::set_alternative_experience(uint16_t alternative_experience) { - handle->control_write(0xdf, alternative_experience, 0); +void Panda::set_alternative_experience(uint16_t alternative_experience, uint16_t safety_param_sp) { + handle->control_write(0xdf, alternative_experience, safety_param_sp); } cereal::PandaState::PandaType Panda::get_hw_type() { diff --git a/selfdrive/pandad/panda.h b/selfdrive/pandad/panda.h index 3e0f4b8507..5ed87104df 100644 --- a/selfdrive/pandad/panda.h +++ b/selfdrive/pandad/panda.h @@ -63,7 +63,7 @@ public: // Panda functionality cereal::PandaState::PandaType get_hw_type(); void set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param=0U); - void set_alternative_experience(uint16_t alternative_experience); + void set_alternative_experience(uint16_t alternative_experience, uint16_t safety_param_sp=0U); void set_fan_speed(uint16_t fan_speed); uint16_t get_fan_speed(); void set_ir_pwr(uint16_t ir_pwr); diff --git a/selfdrive/pandad/panda_safety.cc b/selfdrive/pandad/panda_safety.cc index 6d5e5ce4f3..ef9b459e5b 100644 --- a/selfdrive/pandad/panda_safety.cc +++ b/selfdrive/pandad/panda_safety.cc @@ -10,7 +10,8 @@ void PandaSafety::configureSafetyMode() { auto car_params = fetchCarParams(); if (!car_params.empty()) { - LOGW("got %lu bytes CarParams", car_params.size()); + LOGW("got %lu bytes CarParams", car_params[0].size()); + LOGW("got %lu bytes CarParamsSP", car_params[1].size()); setSafetyMode(car_params); safety_configured_ = true; } @@ -43,7 +44,8 @@ void PandaSafety::updateMultiplexingMode() { } } -std::string PandaSafety::fetchCarParams() { +// TODO-SP: Use structs instead of vector +std::vector PandaSafety::fetchCarParams() { if (!params_.getBool("FirmwareQueryDone")) { return {}; } @@ -56,16 +58,21 @@ std::string PandaSafety::fetchCarParams() { if (!params_.getBool("ControlsReady")) { return {}; } - return params_.get("CarParams"); + return {params_.get("CarParams"), params_.get("CarParamsSP")}; } -void PandaSafety::setSafetyMode(const std::string ¶ms_string) { +// TODO-SP: Use structs instead of vector +void PandaSafety::setSafetyMode(const std::vector ¶ms_string) { AlignedBuffer aligned_buf; - capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params_string.data(), params_string.size())); + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params_string[0].data(), params_string[0].size())); cereal::CarParams::Reader car_params = cmsg.getRoot(); + capnp::FlatArrayMessageReader cmsg_sp(aligned_buf.align(params_string[1].data(), params_string[1].size())); + cereal::CarParamsSP::Reader car_params_sp = cmsg_sp.getRoot(); + auto safety_configs = car_params.getSafetyConfigs(); uint16_t alternative_experience = car_params.getAlternativeExperience(); + uint16_t safety_param_sp = car_params_sp.getSafetyParam(); for (int i = 0; i < pandas_.size(); ++i) { // Default to SILENT safety model if not specified @@ -76,8 +83,8 @@ void PandaSafety::setSafetyMode(const std::string ¶ms_string) { safety_param = safety_configs[i].getSafetyParam(); } - LOGW("Panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience); - pandas_[i]->set_alternative_experience(alternative_experience); + LOGW("Panda %d: setting safety model: %d, param: %d, alternative experience: %d, param_sp: %d", i, (int)safety_model, safety_param, alternative_experience, safety_param_sp); + pandas_[i]->set_alternative_experience(alternative_experience, safety_param_sp); pandas_[i]->set_safety_model(safety_model, safety_param); } } diff --git a/selfdrive/pandad/pandad.h b/selfdrive/pandad/pandad.h index b338b70378..7b37fbe59a 100644 --- a/selfdrive/pandad/pandad.h +++ b/selfdrive/pandad/pandad.h @@ -16,8 +16,8 @@ public: private: void updateMultiplexingMode(); - std::string fetchCarParams(); - void setSafetyMode(const std::string ¶ms_string); + std::vector fetchCarParams(); + void setSafetyMode(const std::vector ¶ms_string); bool initialized_ = false; bool log_once_ = false; diff --git a/sunnypilot/mads/helpers.py b/sunnypilot/mads/helpers.py index 9dc47b26a8..16a5a74dac 100644 --- a/sunnypilot/mads/helpers.py +++ b/sunnypilot/mads/helpers.py @@ -27,8 +27,7 @@ Last updated: July 29, 2024 from openpilot.common.params import Params from opendbc.safety import ALTERNATIVE_EXPERIENCE -from opendbc.car.hyundai.values import HyundaiSafetyFlags -from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP +from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP, HyundaiSafetyFlagsSP class MadsParams: @@ -55,7 +54,7 @@ class MadsParams: hyundai_cruise_main_toggleable = True if hyundai_cruise_main_toggleable: CP_SP.flags |= HyundaiFlagsSP.LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE.value - CP.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.FLAG_HYUNDAI_LONG_MAIN_CRUISE_TOGGLEABLE.value + CP_SP.safetyParam |= HyundaiSafetyFlagsSP.LONG_MAIN_CRUISE_TOGGLEABLE # MADS is currently not supported in Tesla due to lack of consistent states to engage controls # TODO-SP: To enable MADS for Tesla, identify consistent signals for MADS toggling