diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index e67ae793..0f562348 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -31,7 +31,6 @@ #include "panda.h" #include "pigeon.h" - #define MAX_IR_POWER 0.5f #define MIN_IR_POWER 0.0f #define CUTOFF_IL 200 @@ -499,8 +498,8 @@ void pigeon_thread() { std::unordered_map last_recv_time; std::unordered_map cls_max_dt = { - {(char)ublox::CLASS_NAV, int64_t(250000000ULL)}, // 0.25s - {(char)ublox::CLASS_RXM, int64_t(250000000ULL)}, // 0.25s + {(char)ublox::CLASS_NAV, int64_t(900000000ULL)}, // 0.9s + {(char)ublox::CLASS_RXM, int64_t(900000000ULL)}, // 0.9s }; while (!do_exit && panda->connected) { diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc index 33654838..60b60f46 100644 --- a/selfdrive/boardd/pigeon.cc +++ b/selfdrive/boardd/pigeon.cc @@ -17,6 +17,11 @@ 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"; + Pigeon * Pigeon::connect(Panda * p){ PandaPigeon * pigeon = new PandaPigeon(); @@ -32,50 +37,77 @@ Pigeon * Pigeon::connect(const char * tty){ return pigeon; } +bool Pigeon::wait_for_ack(){ + std::string s; + 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) { + LOGE("No response from ublox"); + return false; + } + + util::sleep_for(1); // Allow other threads to be scheduled + } + return false; +} + +bool Pigeon::send_with_ack(std::string cmd){ + send(cmd); + return wait_for_ack(); +} + void Pigeon::init() { - util::sleep_for(1000); - LOGW("panda GPS start"); + while (!do_exit){ + LOGW("panda GPS start"); - // power off pigeon - set_power(false); - util::sleep_for(100); + // power off pigeon + set_power(false); + util::sleep_for(100); - // 9600 baud at init - set_baud(9600); + // 9600 baud at init + set_baud(9600); - // power on pigeon - set_power(true); - util::sleep_for(500); + // 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); + // 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); - util::sleep_for(100); + // set baud rate to 460800 + set_baud(460800); - // init from ubloxd - // To generate this data, run test/ubloxd.py - send("\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); - send("\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); - send("\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); - send("\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); - send("\xB5\x62\x06\x00\x00\x00\x06\x18"s); - send("\xB5\x62\x06\x00\x01\x00\x01\x08\x22"s); - send("\xB5\x62\x06\x00\x01\x00\x02\x09\x23"s); - send("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24"s); - send("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10"s); - send("\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); - send("\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); - send("\xB5\x62\x06\x24\x00\x00\x2A\x84"s); - send("\xB5\x62\x06\x23\x00\x00\x29\x81"s); - send("\xB5\x62\x06\x1E\x00\x00\x24\x72"s); - send("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"s); - send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"s); - send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"s); - send("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s); - send("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s); + // 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\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\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; + + break; + } LOGW("panda GPS on"); } diff --git a/selfdrive/boardd/pigeon.h b/selfdrive/boardd/pigeon.h index 927d7405..edc1d47a 100644 --- a/selfdrive/boardd/pigeon.h +++ b/selfdrive/boardd/pigeon.h @@ -12,6 +12,8 @@ class Pigeon { virtual ~Pigeon(){}; void init(); + bool wait_for_ack(); + bool send_with_ack(std::string cmd); virtual void set_baud(int baud) = 0; virtual void send(const std::string &s) = 0; virtual std::string receive() = 0; diff --git a/selfdrive/locationd/test/ubloxd.py b/selfdrive/locationd/test/ubloxd.py index 470e679d..81bee2fc 100755 --- a/selfdrive/locationd/test/ubloxd.py +++ b/selfdrive/locationd/test/ubloxd.py @@ -5,7 +5,6 @@ from selfdrive.locationd.test import ublox import struct -panda = True baudrate = 460800 rate = 100 # send new data every 100ms @@ -15,17 +14,12 @@ def configure_ublox(dev): dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1) # enable only UBX on USB dev.configure_port(port=0, inMask=0, outMask=0) # disable DDC - if panda: - payload = struct.pack('