diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index f2557a5455..da1c3e905e 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -39,6 +39,7 @@ using namespace std::chrono_literals; std::atomic ignition(false); +std::atomic pigeon_active(false); ExitHandler do_exit; @@ -268,16 +269,16 @@ bool send_panda_state(PubMaster *pm, Panda *panda, bool spoofing_started) { panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); } - bool ignition = ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0)); + bool ignition_local = ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0)); #ifndef __x86_64__ - bool power_save_desired = !ignition; + bool power_save_desired = !ignition_local && !pigeon_active; if (pandaState.power_save_enabled != power_save_desired) { panda->set_power_saving(power_save_desired); } // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect - if (!ignition && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { + if (!ignition_local && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); } #endif @@ -320,7 +321,7 @@ bool send_panda_state(PubMaster *pm, Panda *panda, bool spoofing_started) { } pm->send("pandaStates", msg); - return ignition; + return ignition_local; } void send_peripheral_state(PubMaster *pm, Panda *panda) { @@ -498,10 +499,11 @@ void pigeon_thread(Panda *panda) { while (!do_exit && panda->connected) { bool need_reset = false; + bool ignition_local = ignition; std::string recv = pigeon->receive(); // Parse message header - if (ignition && recv.length() >= 3) { + if (ignition_local && recv.length() >= 3) { if (recv[0] == (char)ublox::PREAMBLE1 && recv[1] == (char)ublox::PREAMBLE2) { const char msg_cls = recv[2]; uint64_t t = nanos_since_boot(); @@ -514,7 +516,7 @@ void pigeon_thread(Panda *panda) { // Check based on message frequency for (const auto& [msg_cls, max_dt] : cls_max_dt) { int64_t dt = (int64_t)nanos_since_boot() - (int64_t)last_recv_time[msg_cls]; - if (ignition_last && ignition && dt > max_dt) { + if (ignition_last && ignition_local && dt > max_dt) { LOGD("ublox receive timeout, msg class: 0x%02x, dt %llu", msg_cls, dt); // TODO: turn on reset after verification of logs // need_reset = true; @@ -522,7 +524,7 @@ void pigeon_thread(Panda *panda) { } // Check based on null bytes - if (ignition && recv.length() > 0 && recv[0] == (char)0x00) { + if (ignition_local && recv.length() > 0 && recv[0] == (char)0x00) { need_reset = true; LOGW("received invalid ublox message while onroad, resetting panda GPS"); } @@ -533,7 +535,8 @@ void pigeon_thread(Panda *panda) { // init pigeon on rising ignition edge // since it was turned off in low power mode - if((ignition && !ignition_last) || need_reset) { + if((ignition_local && !ignition_last) || need_reset) { + pigeon_active = true; pigeon->init(); // Set receive times to current time @@ -541,14 +544,15 @@ void pigeon_thread(Panda *panda) { for (const auto& [msg_cls, dt] : cls_max_dt) { last_recv_time[msg_cls] = t; } - } else if (!ignition && ignition_last) { + } else if (!ignition_local && ignition_last) { // power off on falling edge of ignition LOGD("powering off pigeon\n"); pigeon->stop(); pigeon->set_power(false); + pigeon_active = false; } - ignition_last = ignition; + ignition_last = ignition_local; // 10ms - 100 Hz util::sleep_for(10); diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc index b063b9e174..c917c1ae8b 100644 --- a/selfdrive/boardd/pigeon.cc +++ b/selfdrive/boardd/pigeon.cc @@ -41,8 +41,9 @@ Pigeon * Pigeon::connect(const char * tty) { return pigeon; } -bool Pigeon::wait_for_ack(const std::string &ack, const std::string &nack) { +bool Pigeon::wait_for_ack(const std::string &ack, const std::string &nack, int timeout_ms) { std::string s; + const double start_t = millis_since_boot(); while (!do_exit) { s += receive(); @@ -52,7 +53,7 @@ bool Pigeon::wait_for_ack(const std::string &ack, const std::string &nack) { } else if (s.find(nack) != std::string::npos) { LOGE("Received NACK from ublox"); return false; - } else if (s.size() > 0x1000) { + } else if (s.size() > 0x1000 || ((millis_since_boot() - start_t) > timeout_ms)) { LOGE("No response from ublox"); return false; } diff --git a/selfdrive/boardd/pigeon.h b/selfdrive/boardd/pigeon.h index ecd42cb7b1..0a82bcdd95 100644 --- a/selfdrive/boardd/pigeon.h +++ b/selfdrive/boardd/pigeon.h @@ -16,7 +16,7 @@ class Pigeon { void init(); void stop(); bool wait_for_ack(); - bool wait_for_ack(const std::string &ack, const std::string &nack); + bool wait_for_ack(const std::string &ack, const std::string &nack, int timeout_ms = 1000); bool send_with_ack(const std::string &cmd); virtual void set_baud(int baud) = 0; virtual void send(const std::string &s) = 0;