From 04b0871c45fe3010b6cee6472db1ca765115ed4a Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Wed, 7 Aug 2024 11:04:17 +0800 Subject: [PATCH] pandad: refactor to consolidate threads, keep only one can_send thread (#32680) * single thread improve comment * Keep can_send() running in a separate thread * send send_peripheral_state in pandad_run * new PandaSafety class old-commit-hash: a4de8739e97b4d5d4b1f2fe2f6739ed90338dead --- selfdrive/pandad/SConscript | 2 +- selfdrive/pandad/panda_safety.cc | 79 ++++++++++ selfdrive/pandad/pandad.cc | 240 ++++++++----------------------- selfdrive/pandad/pandad.h | 19 ++- 4 files changed, 160 insertions(+), 180 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 d627d8e24e7d23..71bbd20dad2ff7 100644 --- a/selfdrive/pandad/pandad.cc +++ b/selfdrive/pandad/pandad.cc @@ -2,18 +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" @@ -43,9 +40,6 @@ #define MIN_IR_POWER 0.0f #define CUTOFF_IL 400 #define SATURATE_IL 1000 -using namespace std::chrono_literals; - -std::atomic ignition(false); ExitHandler do_exit; @@ -59,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) || !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) || !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 { @@ -197,16 +109,9 @@ void can_send_thread(std::vector pandas, bool fake_send) { } } -void can_recv_thread(std::vector pandas) { - util::set_thread_name("pandad_can_recv"); - - PubMaster pm({"can"}); - - // run at 100Hz - RateKeeper rk("pandad_can_recv", 100); - std::vector raw_can_data; - - while (!do_exit && check_all_connected(pandas)) { +void can_recv(std::vector &pandas, PubMaster *pm) { + static std::vector raw_can_data; + { bool comms_healthy = true; raw_can_data.clear(); for (const auto& panda : pandas) { @@ -217,14 +122,12 @@ void can_recv_thread(std::vector pandas) { auto evt = msg.initEvent(); evt.setValid(comms_healthy); auto canData = evt.initCan(raw_can_data.size()); - for (uint i = 0; isend("can", msg); } } @@ -386,7 +289,7 @@ std::optional send_panda_states(PubMaster *pm, const std::vector return ignition_local; } -void send_peripheral_state(PubMaster *pm, Panda *panda) { +void send_peripheral_state(Panda *panda, PubMaster *pm) { // build msg MessageBuilder msg; auto evt = msg.initEvent(); @@ -409,46 +312,23 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) { pm->send("peripheralState", msg); } -void panda_state_thread(std::vector pandas, bool spoofing_started) { - util::set_thread_name("pandad_panda_state"); - - Params params; - SubMaster sm({"controlsState"}); - PubMaster pm({"pandaStates", "peripheralState"}); - - Panda *peripheral_panda = pandas[0]; - bool is_onroad = false; - bool is_onroad_last = false; - std::future safety_future; +void process_panda_state(std::vector &pandas, PubMaster *pm, bool spoofing_started) { + static SubMaster sm({"controlsState"}); std::vector connected_serials; for (Panda *p : pandas) { connected_serials.push_back(p->hw_serial()); } - LOGD("start panda state thread"); - - // run at 10hz - RateKeeper rk("panda_state_thread", 10); - - while (!do_exit && check_all_connected(pandas)) { - // send out peripheralState at 2Hz - if (sm.frame % 5 == 0) { - send_peripheral_state(&pm, peripheral_panda); - } - - auto ignition_opt = send_panda_states(&pm, pandas, spoofing_started); - + { + auto ignition_opt = send_panda_states(pm, pandas, spoofing_started); if (!ignition_opt) { LOGE("Failed to get ignition_opt"); - rk.keepTime(); - continue; + return; } - ignition = *ignition_opt; - // check if we should have pandad reconnect - if (!ignition) { + if (!ignition_opt.value()) { bool comms_healthy = true; for (const auto &panda : pandas) { comms_healthy &= panda->comms_healthy(); @@ -468,52 +348,28 @@ void panda_state_thread(std::vector pandas, bool spoofing_started) { } } } - - if (do_exit) { - break; - } - } - - is_onroad = params.getBool("IsOnroad"); - - // set new safety on onroad transition, after params are cleared - if (is_onroad && !is_onroad_last) { - if (!safety_future.valid() || safety_future.wait_for(0ms) == std::future_status::ready) { - 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); } - - rk.keepTime(); } } +void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control) { + static SubMaster sm({"deviceState", "driverCameraState"}); -void peripheral_control_thread(Panda *panda, bool no_fan_control) { - util::set_thread_name("pandad_peripheral_control"); - - SubMaster sm({"deviceState", "driverCameraState"}); - - uint64_t last_driver_camera_t = 0; - uint16_t prev_fan_speed = 999; - uint16_t ir_pwr = 0; - uint16_t prev_ir_pwr = 999; + static uint64_t last_driver_camera_t = 0; + static uint16_t prev_fan_speed = 999; + static uint16_t ir_pwr = 0; + static uint16_t prev_ir_pwr = 999; - FirstOrderFilter integ_lines_filter(0, 30.0, 0.05); - - while (!do_exit && panda->connected()) { - sm.update(1000); + static FirstOrderFilter integ_lines_filter(0, 30.0, 0.05); + { + sm.update(0); if (sm.updated("deviceState") && !no_fan_control) { // Fan speed uint16_t fan_speed = sm["deviceState"].getDeviceState().getFanSpeedPercentDesired(); @@ -551,9 +407,46 @@ void peripheral_control_thread(Panda *panda, bool no_fan_control) { } } -void pandad_main_thread(std::vector serials) { - LOGW("launching pandad"); +void pandad_run(std::vector &pandas) { + const bool no_fan_control = getenv("NO_FAN_CONTROL") != nullptr; + const bool spoofing_started = getenv("STARTED") != nullptr; + const bool fake_send = getenv("FAKESEND") != nullptr; + + // Start the CAN send thread + std::thread send_thread(can_send_thread, pandas, fake_send); + + RateKeeper rk("pandad", 100); + PubMaster pm({"can", "pandaStates", "peripheralState"}); + PandaSafety panda_safety(pandas); + Panda *peripheral_panda = pandas[0]; + + // Main loop: receive CAN data and process states + while (!do_exit && check_all_connected(pandas)) { + can_recv(pandas, &pm); + + // Process peripheral state at 20 Hz + if (rk.frame() % 5 == 0) { + process_peripheral_state(peripheral_panda, &pm, no_fan_control); + } + + // 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 + if (rk.frame() % 50 == 0) { + send_peripheral_state(peripheral_panda, &pm); + } + + rk.keepTime(); + } + + send_thread.join(); +} +void pandad_main_thread(std::vector serials) { if (serials.size() == 0) { serials = Panda::list(); @@ -585,16 +478,7 @@ void pandad_main_thread(std::vector serials) { if (!do_exit) { LOGW("connected to all pandas"); - - std::vector threads; - - threads.emplace_back(panda_state_thread, pandas, getenv("STARTED") != nullptr); - threads.emplace_back(peripheral_control_thread, pandas[0], getenv("NO_FAN_CONTROL") != nullptr); - - threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr); - threads.emplace_back(can_recv_thread, pandas); - - for (auto &t : threads) t.join(); + pandad_run(pandas); } for (Panda *panda : pandas) { 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_; +};