boardd: misc cleanup (#28969)
* boardd cleanup * no more front frame * faster connect loop * fix that on mac old-commit-hash: c933fbb0748106bb8b1e4c1e875eb6c83b7e7b52
This commit is contained in:
Adeeb Shihadeh
@@ -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);
|
||||
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<std::string> 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<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
|
||||
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<std::string> serials) {
|
||||
}
|
||||
|
||||
if (!do_exit) {
|
||||
LOGW("connected to board");
|
||||
Panda *peripheral_panda = pandas[0];
|
||||
LOGW("connected to all pandas");
|
||||
|
||||
std::vector<std::thread> 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<std::string> serials) {
|
||||
for (auto &t : threads) t.join();
|
||||
}
|
||||
|
||||
// we have exited, clean up pandas
|
||||
for (Panda *panda : pandas) {
|
||||
delete panda;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user