diff --git a/common/params.cc b/common/params.cc index f89e7ff002..a25cd278a6 100644 --- a/common/params.cc +++ b/common/params.cc @@ -84,6 +84,7 @@ private: std::unordered_map keys = { {"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG}, + {"AssistNowToken", PERSISTENT}, {"AthenadPid", PERSISTENT}, {"AthenadUploadQueue", PERSISTENT}, {"CalibrationParams", PERSISTENT}, diff --git a/release/files_common b/release/files_common index 663d6d3552..04d13c8a5d 100644 --- a/release/files_common +++ b/release/files_common @@ -90,8 +90,6 @@ selfdrive/boardd/boardd_api_impl.pyx selfdrive/boardd/can_list_to_can_capnp.cc selfdrive/boardd/panda.cc selfdrive/boardd/panda.h -selfdrive/boardd/pigeon.cc -selfdrive/boardd/pigeon.h selfdrive/boardd/set_time.py selfdrive/boardd/pandad.py @@ -282,6 +280,7 @@ selfdrive/sensord/sensors_qcom2.cc selfdrive/sensord/sensors/*.cc selfdrive/sensord/sensors/*.h selfdrive/sensord/sensord +selfdrive/sensord/pigeond.py selfdrive/thermald/thermald.py selfdrive/thermald/power_monitoring.py diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript index 922107509a..dcbea03d3c 100644 --- a/selfdrive/boardd/SConscript +++ b/selfdrive/boardd/SConscript @@ -1,7 +1,7 @@ Import('env', 'envCython', 'common', 'cereal', 'messaging') libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'] -env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=libs) +env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc'], LIBS=libs) env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc']) envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"]) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 23bf8f2929..bd3a6c4384 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -29,8 +29,6 @@ #include "common/util.h" #include "system/hardware/hw.h" -#include "selfdrive/boardd/pigeon.h" - // -- Multi-panda conventions -- // Ordering: // - The internal panda will always be the first panda @@ -561,56 +559,6 @@ void peripheral_control_thread(Panda *panda) { } } -static void pigeon_publish_raw(PubMaster &pm, const std::string &dat) { - // create message - MessageBuilder msg; - msg.initEvent().setUbloxRaw(capnp::Data::Reader((uint8_t*)dat.data(), dat.length())); - pm.send("ubloxRaw", msg); -} - -void pigeon_thread(Panda *panda) { - util::set_thread_name("boardd_pigeon"); - - PubMaster pm({"ubloxRaw"}); - bool ignition_last = false; - - std::unique_ptr pigeon(Hardware::TICI() ? Pigeon::connect("/dev/ttyHS0") : Pigeon::connect(panda)); - - while (!do_exit && panda->connected) { - bool need_reset = false; - bool ignition_local = ignition; - std::string recv = pigeon->receive(); - - // Check based on null bytes - if (ignition_local && recv.length() > 0 && recv[0] == (char)0x00) { - need_reset = true; - LOGW("received invalid ublox message while onroad, resetting panda GPS"); - } - - if (recv.length() > 0) { - pigeon_publish_raw(pm, recv); - } - - // init pigeon on rising ignition edge - // since it was turned off in low power mode - if((ignition_local && !ignition_last) || need_reset) { - pigeon_active = true; - pigeon->init(); - } 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_local; - - // 10ms - 100 Hz - util::sleep_for(10); - } -} - void boardd_main_thread(std::vector serials) { PubMaster pm({"pandaStates", "peripheralState"}); LOGW("attempting to connect"); @@ -649,7 +597,6 @@ void boardd_main_thread(std::vector serials) { threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr); threads.emplace_back(peripheral_control_thread, peripheral_panda); - threads.emplace_back(pigeon_thread, peripheral_panda); threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr); threads.emplace_back(can_recv_thread, pandas); diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc deleted file mode 100644 index d23ff90d3d..0000000000 --- a/selfdrive/boardd/pigeon.cc +++ /dev/null @@ -1,333 +0,0 @@ -#include "selfdrive/boardd/pigeon.h" - -#include -#include -#include - -#include -#include -#include - -#include "common/gpio.h" -#include "common/swaglog.h" -#include "common/util.h" -#include "selfdrive/locationd/ublox_msg.h" - -// Termios on macos doesn't define all baud rate constants -#ifndef B460800 -#define B460800 0010004 -#endif - -using namespace std::string_literals; - -extern ExitHandler do_exit; - -const std::string ack = "\xb5\x62\x05\x01\x02\x00"; -const std::string nack = "\xb5\x62\x05\x00\x02\x00"; -const std::string sos_save_ack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00"; -const std::string sos_save_nack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00"; - -Pigeon * Pigeon::connect(Panda * p) { - PandaPigeon * pigeon = new PandaPigeon(); - pigeon->connect(p); - - return pigeon; -} - -Pigeon * Pigeon::connect(const char * tty) { - TTYPigeon * pigeon = new TTYPigeon(); - pigeon->connect(tty); - - return pigeon; -} - -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(); - - if (s.find(ack_) != std::string::npos) { - LOGD("Received ACK from ublox"); - return true; - } else if (s.find(nack_) != std::string::npos) { - LOGE("Received NACK from ublox"); - return false; - } else if (s.size() > 0x1000 || ((millis_since_boot() - start_t) > timeout_ms)) { - LOGE("No response from ublox"); - return false; - } - - util::sleep_for(1); // Allow other threads to be scheduled - } - return false; -} - -bool Pigeon::wait_for_ack() { - return wait_for_ack(ack, nack); -} - -bool Pigeon::send_with_ack(const std::string &cmd) { - send(cmd); - return wait_for_ack(); -} - -sos_restore_response Pigeon::wait_for_backup_restore_status(int timeout_ms) { - std::string s; - const double start_t = millis_since_boot(); - while (!do_exit) { - s += receive(); - - size_t position = s.find("\xb5\x62\x09\x14\x08\x00\x03"); - if (position != std::string::npos && s.size() >= (position + 11)) { - return static_cast(s[position + 10]); - } else if (s.size() > 0x1000 || ((millis_since_boot() - start_t) > timeout_ms)) { - LOGE("No backup restore response from ublox"); - return error; - } - - util::sleep_for(1); // Allow other threads to be scheduled - } - return error; -} - -void Pigeon::init() { - for (int i = 0; i < 10; i++) { - if (do_exit) return; - LOGW("panda GPS start"); - - // power off pigeon - set_power(false); - util::sleep_for(100); - - // 9600 baud at init - set_baud(9600); - - // power on pigeon - set_power(true); - util::sleep_for(500); - - // baud rate upping - send("\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A"s); - util::sleep_for(100); - - // set baud rate to 460800 - set_baud(460800); - - // init from ubloxd - // To generate this data, run selfdrive/locationd/test/ubloxd.py - if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x00\x00\x00\x06\x18"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x00\x01\x00\x01\x08\x22"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x24\x00\x00\x2A\x84"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x23\x00\x00\x29\x81"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x1E\x00\x00\x24\x72"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x39\x00\x00\x3F\xC3"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s)) continue; - if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s)) continue; - - // check the backup restore status - send("\xB5\x62\x09\x14\x00\x00\x1D\x60"s); - sos_restore_response restore_status = wait_for_backup_restore_status(); - switch(restore_status) { - case restored: - LOGW("almanac backup restored"); - // clear the backup - send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s); - break; - case no_backup: - LOGW("no almanac backup found"); - break; - default: - LOGE("failed to restore almanac backup, status: %d", restore_status); - } - - auto time = util::get_time(); - if (util::time_valid(time)) { - LOGW("Sending current time to ublox"); - send(ublox::build_ubx_mga_ini_time_utc(time)); - } - - LOGW("panda GPS on"); - return; - } - LOGE("failed to initialize panda GPS"); -} - -void Pigeon::stop() { - LOGW("Storing almanac in ublox flash"); - - // Controlled GNSS stop - send("\xB5\x62\x06\x04\x04\x00\x00\x00\x08\x00\x16\x74"s); - - // Store almanac in flash - send("\xB5\x62\x09\x14\x04\x00\x00\x00\x00\x00\x21\xEC"s); - - if (wait_for_ack(sos_save_ack, sos_save_nack)) { - LOGW("Done storing almanac"); - } else { - LOGE("Error storing almanac"); - } -} - -void PandaPigeon::connect(Panda * p) { - panda = p; -} - -void PandaPigeon::set_baud(int baud) { - panda->usb_write(0xe2, 1, 0); - panda->usb_write(0xe4, 1, baud/300); -} - -void PandaPigeon::send(const std::string &s) { - int len = s.length(); - const char * dat = s.data(); - - unsigned char a[0x20+1]; - a[0] = 1; - for (int i=0; iusb_bulk_write(2, a, ll+1); - } -} - -std::string PandaPigeon::receive() { - std::string r; - r.reserve(0x1000 + 0x40); - unsigned char dat[0x40]; - while (r.length() < 0x1000) { - int len = panda->usb_read(0xe0, 1, 0, dat, sizeof(dat)); - if (len <= 0) break; - r.append((char*)dat, len); - } - - return r; -} - -void PandaPigeon::set_power(bool power) { - panda->usb_write(0xd9, power, 0); -} - -PandaPigeon::~PandaPigeon() { -} - -void handle_tty_issue(int err, const char func[]) { - LOGE_100("tty error %d \"%s\" in %s", err, strerror(err), func); -} - -void TTYPigeon::connect(const char * tty) { - pigeon_tty_fd = HANDLE_EINTR(open(tty, O_RDWR)); - if (pigeon_tty_fd < 0) { - handle_tty_issue(errno, __func__); - assert(pigeon_tty_fd >= 0); - } - int err = tcgetattr(pigeon_tty_fd, &pigeon_tty); - assert(err == 0); - - // configure tty - pigeon_tty.c_cflag &= ~PARENB; // disable parity - pigeon_tty.c_cflag &= ~CSTOPB; // single stop bit - pigeon_tty.c_cflag |= CS8; // 8 bits per byte - pigeon_tty.c_cflag &= ~CRTSCTS; // no RTS/CTS flow control - pigeon_tty.c_cflag |= CREAD | CLOCAL; // turn on READ & ignore ctrl lines - pigeon_tty.c_lflag &= ~ICANON; // disable canonical mode - pigeon_tty.c_lflag &= ~ISIG; // disable interpretation of INTR, QUIT and SUSP - pigeon_tty.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off software flow ctrl - pigeon_tty.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL); // disable any special handling of received bytes - pigeon_tty.c_oflag &= ~OPOST; // prevent special interpretation of output bytes - pigeon_tty.c_oflag &= ~ONLCR; // prevent conversion of newline to carriage return/line feed - - // configure blocking behavior - pigeon_tty.c_cc[VMIN] = 0; // min amount of characters returned - pigeon_tty.c_cc[VTIME] = 0; // max blocking time in s/10 (0=inf) - - err = tcsetattr(pigeon_tty_fd, TCSANOW, &pigeon_tty); - assert(err == 0); -} - -void TTYPigeon::set_baud(int baud) { - speed_t baud_const = 0; - switch(baud) { - case 9600: - baud_const = B9600; - break; - case 460800: - baud_const = B460800; - break; - default: - assert(false); - } - - // make sure everything is tx'ed before changing baud - int err = tcdrain(pigeon_tty_fd); - assert(err == 0); - - // change baud - err = tcgetattr(pigeon_tty_fd, &pigeon_tty); - assert(err == 0); - err = cfsetspeed(&pigeon_tty, baud_const); - assert(err == 0); - err = tcsetattr(pigeon_tty_fd, TCSANOW, &pigeon_tty); - assert(err == 0); - - // flush - err = tcflush(pigeon_tty_fd, TCIOFLUSH); - assert(err == 0); -} - -void TTYPigeon::send(const std::string &s) { - int err = HANDLE_EINTR(write(pigeon_tty_fd, s.data(), s.length())); - - if(err < 0) { handle_tty_issue(err, __func__); } - err = tcdrain(pigeon_tty_fd); - if(err < 0) { handle_tty_issue(err, __func__); } -} - -std::string TTYPigeon::receive() { - std::string r; - r.reserve(0x1000 + 0x40); - char dat[0x40]; - while (r.length() < 0x1000) { - int len = read(pigeon_tty_fd, dat, sizeof(dat)); - if(len < 0) { - handle_tty_issue(len, __func__); - } else if (len == 0) { - break; - } else { - r.append(dat, len); - } - - } - return r; -} - -void TTYPigeon::set_power(bool power) { -#ifdef QCOM2 - int err = 0; - err += gpio_init(GPIO_UBLOX_RST_N, true); - err += gpio_init(GPIO_UBLOX_SAFEBOOT_N, true); - err += gpio_init(GPIO_UBLOX_PWR_EN, true); - - err += gpio_set(GPIO_UBLOX_RST_N, power); - err += gpio_set(GPIO_UBLOX_SAFEBOOT_N, power); - err += gpio_set(GPIO_UBLOX_PWR_EN, power); - assert(err == 0); -#endif -} - -TTYPigeon::~TTYPigeon() { - close(pigeon_tty_fd); -} diff --git a/selfdrive/boardd/pigeon.h b/selfdrive/boardd/pigeon.h deleted file mode 100644 index c9ea4739dc..0000000000 --- a/selfdrive/boardd/pigeon.h +++ /dev/null @@ -1,58 +0,0 @@ -#pragma once - -#include - -#include -#include - -#include "selfdrive/boardd/panda.h" - -enum sos_restore_response : int { - unknown = 0, - failed = 1, - restored = 2, - no_backup = 3, - error = -1 -}; - -class Pigeon { - public: - static Pigeon* connect(Panda * p); - static Pigeon* connect(const char * tty); - virtual ~Pigeon(){}; - - void init(); - void stop(); - bool wait_for_ack(); - bool wait_for_ack(const std::string &ack, const std::string &nack, int timeout_ms = 1000); - bool send_with_ack(const std::string &cmd); - sos_restore_response wait_for_backup_restore_status(int timeout_ms = 1000); - virtual void set_baud(int baud) = 0; - virtual void send(const std::string &s) = 0; - virtual std::string receive() = 0; - virtual void set_power(bool power) = 0; -}; - -class PandaPigeon : public Pigeon { - Panda * panda = NULL; -public: - ~PandaPigeon(); - void connect(Panda * p); - void set_baud(int baud); - void send(const std::string &s); - std::string receive(); - void set_power(bool power); -}; - - -class TTYPigeon : public Pigeon { - int pigeon_tty_fd = -1; - struct termios pigeon_tty; -public: - ~TTYPigeon(); - void connect(const char* tty); - void set_baud(int baud); - void send(const std::string &s); - std::string receive(); - void set_power(bool power); -}; diff --git a/selfdrive/locationd/test/ubloxd.py b/selfdrive/locationd/test/ubloxd.py index af9b86127e..b0339851d4 100755 --- a/selfdrive/locationd/test/ubloxd.py +++ b/selfdrive/locationd/test/ubloxd.py @@ -41,13 +41,13 @@ def configure_ublox(dev): ITMF_config2 = 25374 payload = struct.pack(' 0: + assert dat[:2] == b"\xB5\x62" + msg_len = 6 + (dat[5] << 8 | dat[4]) + 2 + msgs.append(dat[:msg_len]) + dat = dat[msg_len:] + return msgs + + +class TTYPigeon(): + def __init__(self, path): + self.path = path + self.tty = serial.VTIMESerial(UBLOX_TTY, baudrate=9600, timeout=0) + + def send(self, dat): + self.tty.write(dat) + + def receive(self): + dat = b'' + while len(dat) < 0x1000: + d = self.tty.read(0x40) + dat += d + if len(d) == 0: + break + return dat + + def set_baud(self, baud): + self.tty.baudrate = baud + + def wait_for_ack(self, ack=UBLOX_ACK, nack=UBLOX_NACK, timeout=0.5): + dat = b'' + st = time.monotonic() + while True: + dat += self.receive() + if ack in dat: + cloudlog.debug("Received ACK from ublox") + return True + elif nack in dat: + cloudlog.error("Received NACK from ublox") + return False + elif time.monotonic() - st > timeout: + cloudlog.error("No response from ublox") + raise TimeoutError('No response from ublox') + time.sleep(0.001) + + def send_with_ack(self, dat, ack=UBLOX_ACK, nack=UBLOX_NACK): + self.send(dat) + self.wait_for_ack(ack, nack) + + def wait_for_backup_restore_status(self, timeout=1): + dat = b'' + st = time.monotonic() + while True: + dat += self.receive() + position = dat.find(UBLOX_BACKUP_RESTORE_MSG) + if position >= 0 and len(dat) >= position + 11: + return dat[position + 10] + elif time.monotonic() - st > timeout: + cloudlog.error("No backup restore response from ublox") + raise TimeoutError('No response from ublox') + time.sleep(0.001) + + +def initialize_pigeon(pigeon): + # try initializing a few times + for _ in range(10): + try: + pigeon.set_baud(9600) + + # up baud rate + pigeon.send(b"\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A") + time.sleep(0.1) + pigeon.set_baud(460800) + + # other configuration messages + pigeon.send_with_ack(b"\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F") + pigeon.send_with_ack(b"\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35") + pigeon.send_with_ack(b"\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80") + pigeon.send_with_ack(b"\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85") + pigeon.send_with_ack(b"\xB5\x62\x06\x00\x00\x00\x06\x18") + pigeon.send_with_ack(b"\xB5\x62\x06\x00\x01\x00\x01\x08\x22") + pigeon.send_with_ack(b"\xB5\x62\x06\x00\x01\x00\x03\x0A\x24") + pigeon.send_with_ack(b"\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10") + pigeon.send_with_ack(b"\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63") + pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37") + pigeon.send_with_ack(b"\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C") + pigeon.send_with_ack(b"\xB5\x62\x06\x23\x28\x00\x00\x00\x00\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x56\x24") + pigeon.send_with_ack(b"\xB5\x62\x06\x24\x00\x00\x2A\x84") + pigeon.send_with_ack(b"\xB5\x62\x06\x23\x00\x00\x29\x81") + pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x00\x00\x24\x72") + pigeon.send_with_ack(b"\xB5\x62\x06\x39\x00\x00\x3F\xC3") + pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51") + pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70") + pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C") + pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70") + pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74") + cloudlog.debug("pigeon configured") + + # try restoring almanac backup + pigeon.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60") + restore_status = pigeon.wait_for_backup_restore_status() + if restore_status == 2: + cloudlog.warning("almanac backup restored") + elif restore_status == 3: + cloudlog.warning("no almanac backup found") + else: + cloudlog.error(f"failed to restore almanac backup, status: {restore_status}") + + # sending time to ublox + t_now = datetime.utcnow() + if t_now >= datetime(2021, 6, 1): + cloudlog.warning("Sending current time to ublox") + + # UBX-MGA-INI-TIME_UTC + msg = add_ubx_checksum(b"\xB5\x62\x13\x40\x18\x00" + struct.pack(" 0: + if dat[0] == 0x00: + cloudlog.warning("received invalid data from ublox, re-initing!") + initialize_pigeon(pigeon) + continue + + # send out to socket + msg = messaging.new_message('ubloxRaw', len(dat)) + msg.ubloxRaw = dat[:] + pm.send('ubloxRaw', msg) + +if __name__ == "__main__": + main() diff --git a/selfdrive/sensord/test/ttff_test.py b/selfdrive/sensord/test/ttff_test.py new file mode 100755 index 0000000000..e2cbc6d144 --- /dev/null +++ b/selfdrive/sensord/test/ttff_test.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 + +import time +import atexit + +from cereal import messaging +from selfdrive.manager.process_config import managed_processes + +TIMEOUT = 10*60 + +def kill(): + for proc in ['ubloxd', 'pigeond']: + managed_processes[proc].stop(retry=True, block=True) + +if __name__ == "__main__": + # start ubloxd + managed_processes['ubloxd'].start() + atexit.register(kill) + + sm = messaging.SubMaster(['ubloxGnss']) + + times = [] + for i in range(20): + # start pigeond + st = time.monotonic() + managed_processes['pigeond'].start() + + # wait for a >4 satellite fix + while True: + sm.update(0) + msg = sm['ubloxGnss'] + if msg.which() == 'measurementReport' and sm.updated["ubloxGnss"]: + report = msg.measurementReport + if report.numMeas > 4: + times.append(time.monotonic() - st) + print(f"\033[94m{i}: Got a fix in {round(times[-1], 2)} seconds\033[0m") + break + + if time.monotonic() - st > TIMEOUT: + raise TimeoutError("\033[91mFailed to get a fix in {TIMEOUT} seconds!\033[0m") + + time.sleep(0.1) + + # stop pigeond + managed_processes['pigeond'].stop(retry=True, block=True) + time.sleep(20) + + print(f"\033[92mAverage TTFF: {round(sum(times) / len(times), 2)}s\033[0m")