boardd: misc cleanup (#28969)

* boardd cleanup

* no more front frame

* faster connect loop

* fix that on mac

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<bool> ignition(false);
@@ -230,7 +229,7 @@ void can_send_thread(std::vector<Panda *> pandas, bool fake_send) {
     capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
     cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
 
-    //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<Panda *> pandas, bool fake_send) {
 void can_recv_thread(std::vector<Panda *> 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<can_frame> raw_can_data;
@@ -288,20 +286,6 @@ void can_recv_thread(std::vector<Panda *> 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<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool spoofing_started) {
   bool ignition_local = false;
   const uint32_t pandas_cnt = pandas.size();
@@ -353,7 +337,6 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
       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<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
     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<Panda *> pandas, bool spoofing_started) {
+void panda_state_thread(std::vector<Panda *> 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<Panda *> pandas, bool spoofin
     uint64_t start_time = nanos_since_boot();
 
     // send out peripheralState
-    send_peripheral_state(pm, peripheral_panda);
+    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) {
       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);
-    sm.update(1000); // TODO: what happens if EINTR is sent while in sm.update?
 
     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
+    // Disable IR on input timeout
-    uint64_t cur_t = nanos_since_boot();
+    if (nanos_since_boot() - last_driver_camera_t > 1e9) {
-    if (cur_t - last_front_frame_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<std::string> serials) {
-  PubMaster pm({"pandaStates", "peripheralState"});
+  LOGW("launching boardd");
-  LOGW("attempting to connect");
 
   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<Panda *> 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
+      util::sleep_for(100);
-      send_empty_panda_state(&pm);
-      send_empty_peripheral_state(&pm);
-      util::sleep_for(500);
       continue;
     }
 
@@ -647,12 +628,12 @@ void boardd_main_thread(std::vector<std::string> serials) {
   }
 
   if (!do_exit) {
-    LOGW("connected to board");
+    LOGW("connected to all pandas");
-    Panda *peripheral_panda = pandas[0];
+
     std::vector<std::thread> threads;
 
-    threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr);
+    threads.emplace_back(panda_state_thread, pandas, getenv("STARTED") != nullptr);
-    threads.emplace_back(peripheral_control_thread, peripheral_panda, getenv("NO_FAN_CONTROL") != 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<std::string> serials) {
     for (auto &t : threads) t.join();
   }
 
-  // we have exited, clean up pandas
   for (Panda *panda : pandas) {
     delete panda;
   }
-}
+}

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<PandaSpiHandle>(serial);
     LOGW("connected to %s over SPI", serial.c_str());
+#else
+    throw e;
 #endif
   }