|
|
|
@ -10,12 +10,12 @@ |
|
|
|
|
#include <cassert> |
|
|
|
|
#include <cerrno> |
|
|
|
|
#include <chrono> |
|
|
|
|
#include <cmath> |
|
|
|
|
#include <cstdint> |
|
|
|
|
#include <cstdio> |
|
|
|
|
#include <cstdlib> |
|
|
|
|
#include <future> |
|
|
|
|
#include <thread> |
|
|
|
|
#include <unordered_map> |
|
|
|
|
|
|
|
|
|
#include <libusb-1.0/libusb.h> |
|
|
|
|
|
|
|
|
@ -26,7 +26,6 @@ |
|
|
|
|
#include "selfdrive/common/timing.h" |
|
|
|
|
#include "selfdrive/common/util.h" |
|
|
|
|
#include "selfdrive/hardware/hw.h" |
|
|
|
|
#include "selfdrive/locationd/ublox_msg.h" |
|
|
|
|
|
|
|
|
|
#include "selfdrive/boardd/panda.h" |
|
|
|
|
#include "selfdrive/boardd/pigeon.h" |
|
|
|
@ -552,38 +551,11 @@ void pigeon_thread(Panda *panda) { |
|
|
|
|
|
|
|
|
|
std::unique_ptr<Pigeon> pigeon(Hardware::TICI() ? Pigeon::connect("/dev/ttyHS0") : Pigeon::connect(panda)); |
|
|
|
|
|
|
|
|
|
std::unordered_map<char, uint64_t> last_recv_time; |
|
|
|
|
std::unordered_map<char, int64_t> cls_max_dt = { |
|
|
|
|
{(char)ublox::CLASS_NAV, int64_t(900000000ULL)}, // 0.9s
|
|
|
|
|
{(char)ublox::CLASS_RXM, int64_t(900000000ULL)}, // 0.9s
|
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
while (!do_exit && panda->connected) { |
|
|
|
|
bool need_reset = false; |
|
|
|
|
bool ignition_local = ignition; |
|
|
|
|
std::string recv = pigeon->receive(); |
|
|
|
|
|
|
|
|
|
// Parse message header
|
|
|
|
|
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(); |
|
|
|
|
if (t > last_recv_time[msg_cls]) { |
|
|
|
|
last_recv_time[msg_cls] = t; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// 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_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;
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Check based on null bytes
|
|
|
|
|
if (ignition_local && recv.length() > 0 && recv[0] == (char)0x00) { |
|
|
|
|
need_reset = true; |
|
|
|
@ -599,12 +571,6 @@ void pigeon_thread(Panda *panda) { |
|
|
|
|
if((ignition_local && !ignition_last) || need_reset) { |
|
|
|
|
pigeon_active = true; |
|
|
|
|
pigeon->init(); |
|
|
|
|
|
|
|
|
|
// Set receive times to current time
|
|
|
|
|
uint64_t t = nanos_since_boot() + 10000000000ULL; // Give ublox 10 seconds to start
|
|
|
|
|
for (const auto& [msg_cls, dt] : cls_max_dt) { |
|
|
|
|
last_recv_time[msg_cls] = t; |
|
|
|
|
} |
|
|
|
|
} else if (!ignition_local && ignition_last) { |
|
|
|
|
// power off on falling edge of ignition
|
|
|
|
|
LOGD("powering off pigeon\n"); |
|
|
|
|