diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 6a6e775e23..7d3547e7aa 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -12,7 +12,6 @@ #include #include #include -#include #include #include @@ -41,21 +40,7 @@ std::atomic safety_setter_thread_running(false); std::atomic ignition(false); ExitHandler do_exit; -struct tm get_time(){ - time_t rawtime; - time(&rawtime); - struct tm sys_time; - gmtime_r(&rawtime, &sys_time); - - return sys_time; -} - -bool time_valid(struct tm sys_time){ - int year = 1900 + sys_time.tm_year; - int month = 1 + sys_time.tm_mon; - return (year > 2020) || (year == 2020 && month >= 10); -} void safety_setter_thread() { LOGD("Starting safety setter thread"); @@ -163,10 +148,10 @@ bool usb_connect() { if (tmp_panda->has_rtc){ setenv("TZ","UTC",1); - struct tm sys_time = get_time(); + struct tm sys_time = util::get_time(); struct tm rtc_time = tmp_panda->get_rtc(); - if (!time_valid(sys_time) && time_valid(rtc_time)) { + if (!util::time_valid(sys_time) && util::time_valid(rtc_time)) { LOGE("System time wrong, setting from RTC. " "System: %d-%02d-%02d %02d:%02d:%02d RTC: %d-%02d-%02d %02d:%02d:%02d", sys_time.tm_year + 1900, sys_time.tm_mon + 1, sys_time.tm_mday, @@ -333,9 +318,9 @@ void panda_state_thread(bool spoofing_started) { if ((panda->has_rtc) && !ignition && (no_ignition_cnt % 120 == 1)){ // Write time to RTC if it looks reasonable setenv("TZ","UTC",1); - struct tm sys_time = get_time(); + struct tm sys_time = util::get_time(); - if (time_valid(sys_time)){ + if (util::time_valid(sys_time)){ struct tm rtc_time = panda->get_rtc(); double seconds = difftime(mktime(&rtc_time), mktime(&sys_time)); @@ -545,6 +530,7 @@ void pigeon_thread() { } else if (!ignition && ignition_last) { // power off on falling edge of ignition LOGD("powering off pigeon\n"); + pigeon->stop(); pigeon->set_power(false); } diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc index 937925e487..dc6a5e49a3 100644 --- a/selfdrive/boardd/pigeon.cc +++ b/selfdrive/boardd/pigeon.cc @@ -6,10 +6,12 @@ #include #include +#include #include "selfdrive/common/gpio.h" #include "selfdrive/common/swaglog.h" #include "selfdrive/common/util.h" +#include "selfdrive/locationd/ublox_msg.h" // Termios on macos doesn't define all baud rate constants #ifndef B460800 @@ -22,7 +24,8 @@ 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_ack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00"; +const std::string sos_nack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00"; Pigeon * Pigeon::connect(Panda * p){ PandaPigeon * pigeon = new PandaPigeon(); @@ -38,7 +41,7 @@ Pigeon * Pigeon::connect(const char * tty){ return pigeon; } -bool Pigeon::wait_for_ack(){ +bool Pigeon::wait_for_ack(std::string ack, std::string nack){ std::string s; while (!do_exit){ s += receive(); @@ -59,6 +62,10 @@ bool Pigeon::wait_for_ack(){ return false; } +bool Pigeon::wait_for_ack(){ + return wait_for_ack(ack, nack); +} + bool Pigeon::send_with_ack(std::string cmd){ send(cmd); return wait_for_ack(); @@ -110,6 +117,11 @@ void Pigeon::init() { 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; + 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; @@ -117,6 +129,22 @@ void Pigeon::init() { 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_ack, sos_nack)) { + LOGW("Done storing almanac"); + } else { + LOGE("Error storing almanac"); + } +} + void PandaPigeon::connect(Panda * p) { panda = p; } diff --git a/selfdrive/boardd/pigeon.h b/selfdrive/boardd/pigeon.h index 0543c4828e..ff22042dc9 100644 --- a/selfdrive/boardd/pigeon.h +++ b/selfdrive/boardd/pigeon.h @@ -14,7 +14,9 @@ class Pigeon { virtual ~Pigeon(){}; void init(); + void stop(); bool wait_for_ack(); + bool wait_for_ack(std::string ack, std::string nack); bool send_with_ack(std::string cmd); virtual void set_baud(int baud) = 0; virtual void send(const std::string &s) = 0; diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h index d4d27fb90a..7af78ae798 100644 --- a/selfdrive/common/util.h +++ b/selfdrive/common/util.h @@ -16,6 +16,9 @@ #include #include #include +#include +#include +#include #ifndef sighandler_t typedef void (*sighandler_t)(int sig); @@ -28,6 +31,23 @@ int set_core_affinity(int core); namespace util { +// Time helpers +inline struct tm get_time(){ + time_t rawtime; + time(&rawtime); + + struct tm sys_time; + gmtime_r(&rawtime, &sys_time); + + return sys_time; +} + +inline bool time_valid(struct tm sys_time){ + int year = 1900 + sys_time.tm_year; + int month = 1 + sys_time.tm_mon; + return (year > 2020) || (year == 2020 && month >= 10); +} + // ***** math helpers ***** // map x from [a1, a2] to [b1, b2] @@ -108,6 +128,15 @@ inline bool file_exists(const std::string& fn) { return f.good(); } +inline std::string hexdump(const std::string& in) { + std::stringstream ss; + ss << std::hex << std::setfill('0'); + for (size_t i = 0; i < in.size(); i++) { + ss << std::setw(2) << static_cast(static_cast(in[i])); + } + return ss.str(); +} + } class ExitHandler { diff --git a/selfdrive/locationd/test/ubloxd.py b/selfdrive/locationd/test/ubloxd.py index 871d7b449e..2246921434 100755 --- a/selfdrive/locationd/test/ubloxd.py +++ b/selfdrive/locationd/test/ubloxd.py @@ -61,6 +61,16 @@ def configure_ublox(dev): dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW, 1) dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW2, 1) + print("send on stop:") + + # Save on shutdown + # Controlled GNSS stop and hot start + payload = struct.pack(' #include #include +#include #include "cereal/messaging/messaging.h" +#include "selfdrive/common/util.h" #include "selfdrive/locationd/generated/gps.h" #include "selfdrive/locationd/generated/ubx.h" +using namespace std::string_literals; + // protocol constants namespace ublox { const uint8_t PREAMBLE1 = 0xb5; @@ -22,6 +26,65 @@ namespace ublox { const uint8_t CLASS_NAV = 0x01; const uint8_t CLASS_RXM = 0x02; const uint8_t CLASS_MON = 0x0A; + + struct ubx_mga_ini_time_utc_t { + uint8_t type; + uint8_t version; + uint8_t ref; + int8_t leapSecs; + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t minute; + uint8_t second; + uint8_t reserved1; + uint32_t ns; + uint16_t tAccS; + uint16_t reserved2; + uint32_t tAccNs; + } __attribute__((packed)); + + inline std::string ubx_add_checksum(std::string msg){ + assert(msg.size() > 2); + + uint8_t ck_a = 0, ck_b = 0; + for(int i = 2; i < msg.size(); i++) { + ck_a = (ck_a + msg[i]) & 0xFF; + ck_b = (ck_b + ck_a) & 0xFF; + } + + std::string r = msg; + r.push_back(ck_a); + r.push_back(ck_b); + return r; + } + + inline std::string build_ubx_mga_ini_time_utc(struct tm time) { + ublox::ubx_mga_ini_time_utc_t payload = { + .type = 0x10, + .version = 0x0, + .ref = 0x0, + .leapSecs = -128, // Unknown + .year = (uint16_t)(1900 + time.tm_year), + .month = (uint8_t)(1 + time.tm_mon), + .day = (uint8_t)time.tm_mday, + .hour = (uint8_t)time.tm_hour, + .minute = (uint8_t)time.tm_min, + .second = (uint8_t)time.tm_sec, + .reserved1 = 0x0, + .ns = 0, + .tAccS = 30, + .reserved2 = 0x0, + .tAccNs = 0, + }; + assert(sizeof(payload) == 24); + + std::string msg = "\xb5\x62\x13\x40\x18\x00"s; + msg += std::string((char*)&payload, sizeof(payload)); + + return ubx_add_checksum(msg); + } } class UbloxMsgParser {