sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

boardd: misc cleanup (#28969)

Browse Source 
* boardd cleanup

* no more front frame

* faster connect loop

* fix that on mac
old-commit-hash: c933fbb074
beeps
Adeeb Shihadeh 2 years ago committed by GitHub
parent 1fd2125bfa
commit d6b8eed74d
2 changed files with 30 additions and 48 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 76
      selfdrive/boardd/boardd.cc
  2. 2
      selfdrive/boardd/panda.cc

76
selfdrive/boardd/boardd.cc
Unescape View File

@ -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;
}
}
}

2
selfdrive/boardd/panda.cc
Unescape View File

@ -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
}

Write Preview
Loading…
Cancel
Save