From 6d2fed1745fb901f8b77d1086e27be8945e6c97d Mon Sep 17 00:00:00 2001 From: deanlee Date: Sun, 4 Aug 2024 18:38:02 +0800 Subject: [PATCH] new PandaSafety class --- selfdrive/pandad/SConscript | 2 +- selfdrive/pandad/panda_safety.cc | 79 ++++++++++++++++++++ selfdrive/pandad/pandad.cc | 120 ++----------------------------- selfdrive/pandad/pandad.h | 19 ++++- 4 files changed, 102 insertions(+), 118 deletions(-) create mode 100644 selfdrive/pandad/panda_safety.cc diff --git a/selfdrive/pandad/SConscript b/selfdrive/pandad/SConscript index dcc1f9811ee0bd..58777cafe962eb 100644 --- a/selfdrive/pandad/SConscript +++ b/selfdrive/pandad/SConscript @@ -3,7 +3,7 @@ Import('env', 'envCython', 'common', 'messaging') libs = ['usb-1.0', common, messaging, 'pthread'] panda = env.Library('panda', ['panda.cc', 'panda_comms.cc', 'spi.cc']) -env.Program('pandad', ['main.cc', 'pandad.cc'], LIBS=[panda] + libs) +env.Program('pandad', ['main.cc', 'pandad.cc', 'panda_safety.cc'], LIBS=[panda] + libs) env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc']) pandad_python = envCython.Program('pandad_api_impl.so', 'pandad_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"]) diff --git a/selfdrive/pandad/panda_safety.cc b/selfdrive/pandad/panda_safety.cc new file mode 100644 index 00000000000000..1d0f72f223444e --- /dev/null +++ b/selfdrive/pandad/panda_safety.cc @@ -0,0 +1,79 @@ +#include "selfdrive/pandad/pandad.h" +#include "cereal/messaging/messaging.h" +#include "common/swaglog.h" + +void PandaSafety::configureSafetyMode() { + bool is_onroad = params_.getBool("IsOnroad"); + + if (is_onroad && !safety_configured_) { + updateMultiplexingMode(); + + auto car_params = fetchCarParams(); + if (!car_params.empty()) { + LOGW("got %lu bytes CarParams", car_params.size()); + setSafetyMode(car_params); + safety_configured_ = true; + } + } else if (!is_onroad) { + initialized_ = false; + safety_configured_ = false; + } +} + +void PandaSafety::updateMultiplexingMode() { + // Initialize to ELM327 without OBD multiplexing for initial fingerprinting + if (!initialized_) { + prev_obd_multiplexing_ = false; + for (int i = 0; i < pandas_.size(); ++i) { + pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U); + } + initialized_ = true; + } + + // Switch between multiplexing modes based on the OBD multiplexing request + bool obd_multiplexing_requested = params_.getBool("ObdMultiplexingEnabled"); + if (obd_multiplexing_requested != prev_obd_multiplexing_) { + for (int i = 0; i < pandas_.size(); ++i) { + const uint16_t safety_param = (i > 0 || !obd_multiplexing_requested) ? 1U : 0U; + pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param); + } + prev_obd_multiplexing_ = obd_multiplexing_requested; + params_.putBool("ObdMultiplexingChanged", true); + } +} + +std::string PandaSafety::fetchCarParams() { + if (!params_.getBool("FirmwareQueryDone")) { + return {}; + } + LOGW("Finished FW query"); + + LOGW("Waiting for params to set safety model"); + if (!params_.getBool("ControlsReady")) { + return {}; + } + return params_.get("CarParams"); +} + +void PandaSafety::setSafetyMode(const std::string ¶ms_string) { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params_string.data(), params_string.size())); + cereal::CarParams::Reader car_params = cmsg.getRoot(); + + auto safety_configs = car_params.getSafetyConfigs(); + uint16_t alternative_experience = car_params.getAlternativeExperience(); + + for (int i = 0; i < pandas_.size(); ++i) { + // Default to SILENT safety model if not specified + cereal::CarParams::SafetyModel safety_model = cereal::CarParams::SafetyModel::SILENT; + uint16_t safety_param = 0U; + if (i < safety_configs.size()) { + safety_model = safety_configs[i].getSafetyModel(); + 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); + pandas_[i]->set_safety_model(safety_model, safety_param); + } +} diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index 07dd048f21c52a..239b79fd1b3a25 100644 --- a/selfdrive/pandad/pandad.cc +++ b/selfdrive/pandad/pandad.cc @@ -2,19 +2,15 @@ #include #include -#include #include #include #include -#include -#include #include #include #include #include "cereal/gen/cpp/car.capnp.h" #include "cereal/messaging/messaging.h" -#include "common/params.h" #include "common/ratekeeper.h" #include "common/swaglog.h" #include "common/timing.h" @@ -44,10 +40,6 @@ #define MIN_IR_POWER 0.0f #define CUTOFF_IL 400 #define SATURATE_IL 1000 -using namespace std::chrono_literals; - -std::atomic g_ignition(false); -std::future g_safety_future; ExitHandler do_exit; @@ -61,88 +53,6 @@ bool check_all_connected(const std::vector &pandas) { return true; } -bool safety_setter_thread(std::vector pandas) { - LOGD("Starting safety setter thread"); - - Params p; - - // there should be at least one panda connected - if (pandas.size() == 0) { - return false; - } - - // initialize to ELM327 without OBD multiplexing for fingerprinting - bool obd_multiplexing_enabled = false; - for (int i = 0; i < pandas.size(); i++) { - pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U); - } - - // openpilot can switch between multiplexing modes for different FW queries - while (true) { - if (do_exit || !check_all_connected(pandas) || !g_ignition) { - return false; - } - - bool obd_multiplexing_requested = p.getBool("ObdMultiplexingEnabled"); - if (obd_multiplexing_requested != obd_multiplexing_enabled) { - for (int i = 0; i < pandas.size(); i++) { - const uint16_t safety_param = (i > 0 || !obd_multiplexing_requested) ? 1U : 0U; - pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param); - } - obd_multiplexing_enabled = obd_multiplexing_requested; - p.putBool("ObdMultiplexingChanged", true); - } - - if (p.getBool("FirmwareQueryDone")) { - LOGW("finished FW query"); - break; - } - util::sleep_for(20); - } - - std::string params; - LOGW("waiting for params to set safety model"); - while (true) { - if (do_exit || !check_all_connected(pandas) || !g_ignition) { - return false; - } - - if (p.getBool("ControlsReady")) { - params = p.get("CarParams"); - if (params.size() > 0) break; - } - util::sleep_for(100); - } - LOGW("got %lu bytes CarParams", params.size()); - - AlignedBuffer aligned_buf; - capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params.data(), params.size())); - cereal::CarParams::Reader car_params = cmsg.getRoot(); - cereal::CarParams::SafetyModel safety_model; - uint16_t safety_param; - - auto safety_configs = car_params.getSafetyConfigs(); - uint16_t alternative_experience = car_params.getAlternativeExperience(); - for (uint32_t i = 0; i < pandas.size(); i++) { - auto panda = pandas[i]; - - if (safety_configs.size() > i) { - safety_model = safety_configs[i].getSafetyModel(); - safety_param = safety_configs[i].getSafetyParam(); - } else { - // If no safety mode is specified, default to silent - safety_model = cereal::CarParams::SafetyModel::SILENT; - safety_param = 0U; - } - - LOGW("panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience); - panda->set_alternative_experience(alternative_experience); - panda->set_safety_model(safety_model, safety_param); - } - - return true; -} - Panda *connect(std::string serial="", uint32_t index=0) { std::unique_ptr panda; try { @@ -397,9 +307,7 @@ void send_peripheral_state(Panda *panda, PubMaster *pm) { } void process_panda_state(std::vector &pandas, PubMaster *pm, bool spoofing_started) { - static Params params; static SubMaster sm({"controlsState"}); - static bool is_onroad_last = false; std::vector connected_serials; for (Panda *p : pandas) { @@ -412,10 +320,9 @@ void process_panda_state(std::vector &pandas, PubMaster *pm, bool spoof LOGE("Failed to get ignition_opt"); return; } - g_ignition = *ignition_opt; // check if we should have pandad reconnect - if (!g_ignition) { + if (!ignition_opt.value()) { bool comms_healthy = true; for (const auto &panda : pandas) { comms_healthy &= panda->comms_healthy(); @@ -437,30 +344,14 @@ void process_panda_state(std::vector &pandas, PubMaster *pm, bool spoof } } - bool is_onroad = params.getBool("IsOnroad"); - - // set new safety on onroad transition, after params are cleared - if (is_onroad && !is_onroad_last) { - if (!g_safety_future.valid() || g_safety_future.wait_for(0ms) == std::future_status::ready) { - g_safety_future = std::async(std::launch::async, safety_setter_thread, pandas); - } else { - LOGW("Safety setter thread already running"); - } - } - - is_onroad_last = is_onroad; - sm.update(0); const bool engaged = sm.allAliveAndValid({"controlsState"}) && sm["controlsState"].getControlsState().getEnabled(); - for (const auto &panda : pandas) { panda->send_heartbeat(engaged); } } } - -// run at 20 HZ void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control) { static SubMaster sm({"deviceState", "driverCameraState"}); @@ -518,8 +409,9 @@ void pandad_run(std::vector &pandas) { // Start the CAN send thread std::thread send_thread(can_send_thread, pandas, fake_send); + RateKeeper rk("pandad", 100); PubMaster pm({"can", "pandaStates", "peripheralState"}); - RateKeeper rk("pandad", 100); // 100 hz + PandaSafety panda_safety(pandas); Panda *peripheral_panda = pandas[0]; // Main loop: receive CAN data and process states @@ -534,6 +426,7 @@ void pandad_run(std::vector &pandas) { // Process panda state at 10 Hz if (rk.frame() % 10 == 0) { process_panda_state(pandas, &pm, spoofing_started); + panda_safety.configureSafetyMode(); } // Send out peripheralState at 2Hz @@ -544,11 +437,6 @@ void pandad_run(std::vector &pandas) { rk.keepTime(); } - // Wait for safety future to finish if valid - if (g_safety_future.valid()) { - g_safety_future.wait(); - } - send_thread.join(); } diff --git a/selfdrive/pandad/pandad.h b/selfdrive/pandad/pandad.h index 9d35949a8fd1f4..14d97551f4ee88 100644 --- a/selfdrive/pandad/pandad.h +++ b/selfdrive/pandad/pandad.h @@ -3,7 +3,24 @@ #include #include +#include "common/params.h" #include "selfdrive/pandad/panda.h" -bool safety_setter_thread(std::vector pandas); void pandad_main_thread(std::vector serials); + +class PandaSafety { +public: + PandaSafety(const std::vector &pandas) : pandas_(pandas) {} + void configureSafetyMode(); + +private: + void updateMultiplexingMode(); + std::string fetchCarParams(); + void setSafetyMode(const std::string ¶ms_string); + + bool initialized_ = false; + bool safety_configured_ = false; + bool prev_obd_multiplexing_ = false; + std::vector pandas_; + Params params_; +};