From c933fbb0748106bb8b1e4c1e875eb6c83b7e7b52 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 15 Jul 2023 23:39:21 -0700 Subject: [PATCH] boardd: misc cleanup (#28969) * boardd cleanup * no more front frame * faster connect loop * fix that on mac --- selfdrive/boardd/boardd.cc | 76 ++++++++++++++------------------------ selfdrive/boardd/panda.cc | 2 + 2 files changed, 30 insertions(+), 48 deletions(-) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index f4ad918109..a5203994fa 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -51,7 +51,6 @@ #define MIN_IR_POWER 0.0f #define CUTOFF_IL 400 #define SATURATE_IL 1000 -#define NIBBLE_TO_HEX(n) ((n) < 10 ? (n) + '0' : ((n) - 10) + 'a') using namespace std::chrono_literals; std::atomic ignition(false); @@ -230,7 +229,7 @@ void can_send_thread(std::vector pandas, bool fake_send) { capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get())); cereal::Event::Reader event = cmsg.getRoot(); - //Dont send if older than 1 second + // Don't send if older than 1 second if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) { for (const auto& panda : pandas) { LOGT("sending sendcan to panda: %s", (panda->hw_serial()).c_str()); @@ -246,10 +245,9 @@ void can_send_thread(std::vector pandas, bool fake_send) { void can_recv_thread(std::vector pandas) { util::set_thread_name("boardd_can_recv"); - // can = 8006 PubMaster pm({"can"}); - // run at 100hz + // run at 100Hz const uint64_t dt = 10000000ULL; uint64_t next_frame_time = nanos_since_boot() + dt; std::vector raw_can_data; @@ -288,20 +286,6 @@ void can_recv_thread(std::vector pandas) { } } -void send_empty_peripheral_state(PubMaster *pm) { - MessageBuilder msg; - auto peripheralState = msg.initEvent().initPeripheralState(); - peripheralState.setPandaType(cereal::PandaState::PandaType::UNKNOWN); - pm->send("peripheralState", msg); -} - -void send_empty_panda_state(PubMaster *pm) { - MessageBuilder msg; - auto pandaStates = msg.initEvent().initPandaStates(1); - pandaStates[0].setPandaType(cereal::PandaState::PandaType::UNKNOWN); - pm->send("pandaStates", msg); -} - std::optional send_panda_states(PubMaster *pm, const std::vector &pandas, bool spoofing_started) { bool ignition_local = false; const uint32_t pandas_cnt = pandas.size(); @@ -353,7 +337,6 @@ std::optional send_panda_states(PubMaster *pm, const std::vector panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); } - #ifndef __x86_64__ bool power_save_desired = !ignition_local; if (health.power_save_enabled_pkt != power_save_desired) { panda->set_power_saving(power_save_desired); @@ -363,7 +346,6 @@ std::optional send_panda_states(PubMaster *pm, const std::vector if (!ignition_local && (health.safety_mode_pkt != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); } - #endif if (!panda->comms_healthy()) { evt.setValid(false); @@ -470,11 +452,12 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) { pm->send("peripheralState", msg); } -void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofing_started) { +void panda_state_thread(std::vector pandas, bool spoofing_started) { util::set_thread_name("boardd_panda_state"); Params params; SubMaster sm({"controlsState"}); + PubMaster pm({"pandaStates", "peripheralState"}); Panda *peripheral_panda = pandas[0]; bool is_onroad = false; @@ -493,8 +476,8 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin uint64_t start_time = nanos_since_boot(); // send out peripheralState - send_peripheral_state(pm, peripheral_panda); - auto ignition_opt = send_panda_states(pm, pandas, spoofing_started); + send_peripheral_state(&pm, peripheral_panda); + auto ignition_opt = send_panda_states(&pm, pandas, spoofing_started); if (!ignition_opt) { continue; @@ -560,22 +543,20 @@ void peripheral_control_thread(Panda *panda, bool no_fan_control) { SubMaster sm({"deviceState", "driverCameraState"}); - uint64_t last_front_frame_t = 0; + uint64_t last_driver_camera_t = 0; uint16_t prev_fan_speed = 999; uint16_t ir_pwr = 0; uint16_t prev_ir_pwr = 999; - unsigned int cnt = 0; FirstOrderFilter integ_lines_filter(0, 30.0, 0.05); while (!do_exit && panda->connected()) { - cnt++; - sm.update(1000); // TODO: what happens if EINTR is sent while in sm.update? + sm.update(1000); if (sm.updated("deviceState") && !no_fan_control) { // Fan speed uint16_t fan_speed = sm["deviceState"].getDeviceState().getFanSpeedPercentDesired(); - if (fan_speed != prev_fan_speed || cnt % 100 == 0) { + if (fan_speed != prev_fan_speed || sm.frame % 100 == 0) { panda->set_fan_speed(fan_speed); prev_fan_speed = fan_speed; } @@ -586,7 +567,7 @@ void peripheral_control_thread(Panda *panda, bool no_fan_control) { int cur_integ_lines = event.getDriverCameraState().getIntegLines(); cur_integ_lines = integ_lines_filter.update(cur_integ_lines); - last_front_frame_t = event.getLogMonoTime(); + last_driver_camera_t = event.getLogMonoTime(); if (cur_integ_lines <= CUTOFF_IL) { ir_pwr = 100.0 * MIN_IR_POWER; @@ -597,48 +578,48 @@ void peripheral_control_thread(Panda *panda, bool no_fan_control) { } } - // Disable ir_pwr on front frame timeout - uint64_t cur_t = nanos_since_boot(); - if (cur_t - last_front_frame_t > 1e9) { + // Disable IR on input timeout + if (nanos_since_boot() - last_driver_camera_t > 1e9) { ir_pwr = 0; } - if (ir_pwr != prev_ir_pwr || cnt % 100 == 0 || ir_pwr >= 50.0) { + if (ir_pwr != prev_ir_pwr || sm.frame % 100 == 0 || ir_pwr >= 50.0) { panda->set_ir_pwr(ir_pwr); prev_ir_pwr = ir_pwr; } // Write to rtc once per minute when no ignition present - if (!ignition && (cnt % 120 == 1)) { + if (!ignition && (sm.frame % 120 == 1)) { sync_time(panda, SyncTimeDir::TO_PANDA); } } } void boardd_main_thread(std::vector serials) { - PubMaster pm({"pandaStates", "peripheralState"}); - LOGW("attempting to connect"); + LOGW("launching boardd"); if (serials.size() == 0) { - // connect to all serials = Panda::list(); - // exit if no pandas are connected if (serials.size() == 0) { LOGW("no pandas found, exiting"); return; } } + std::string serials_str; + for (int i = 0; i < serials.size(); i++) { + serials_str += serials[i]; + if (i < serials.size() - 1) serials_str += ", "; + } + LOGW("connecting to pandas: %s", serials_str.c_str()); + // connect to all provided serials std::vector pandas; for (int i = 0; i < serials.size() && !do_exit; /**/) { Panda *p = connect(serials[i], i); if (!p) { - // send empty pandaState & peripheralState and try again - send_empty_panda_state(&pm); - send_empty_peripheral_state(&pm); - util::sleep_for(500); + util::sleep_for(100); continue; } @@ -647,12 +628,12 @@ void boardd_main_thread(std::vector serials) { } if (!do_exit) { - LOGW("connected to board"); - Panda *peripheral_panda = pandas[0]; + LOGW("connected to all pandas"); + std::vector threads; - threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr); - threads.emplace_back(peripheral_control_thread, peripheral_panda, getenv("NO_FAN_CONTROL") != nullptr); + 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); @@ -660,8 +641,7 @@ void boardd_main_thread(std::vector serials) { for (auto &t : threads) t.join(); } - // we have exited, clean up pandas for (Panda *panda : pandas) { delete panda; } -} +} \ No newline at end of file diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index c20be23d09..8849a46bd8 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -18,6 +18,8 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { #ifndef __APPLE__ handle = std::make_unique(serial); LOGW("connected to %s over SPI", serial.c_str()); +#else + throw e; #endif }