Store almanac on ublox poweroff (#20967)

* Store almanac on ublox poweroff

* send current UTC time

* move message building to ublox_msg.h

selfdrive/boardd/boardd.cc

@@ -12,7 +12,6 @@
 #include <atomic>
 #include <bitset>
 #include <cassert>
-#include <ctime>
 #include <thread>
 #include <unordered_map>
 
@@ -41,21 +40,7 @@ std::atomic<bool> safety_setter_thread_running(false);
 std::atomic<bool> 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);
     }
 

selfdrive/boardd/pigeon.cc

@@ -6,10 +6,12 @@
 #include <unistd.h>
 
 #include <cassert>
+#include <optional>
 
 #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;
 }

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;

selfdrive/common/util.h

@@ -16,6 +16,9 @@
 #include <string>
 #include <thread>
 #include <map>
+#include <ctime>
+#include <sstream>
+#include <iomanip>
 
 #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<unsigned int>(static_cast<unsigned char>(in[i]));
+    }
+    return ss.str();
+}
+
 }
 
 class ExitHandler {

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('<HBB', 0x0000, 0x08, 0x00)
+  dev.send_message(ublox.CLASS_CFG, ublox.MSG_CFG_RST, payload)
+
+  # UBX-UPD-SOS backup
+  dev.send_message(0x09, 0x14, b"\x00\x00\x00\x00")
+
 
 if __name__ == "__main__":
   class Device:

selfdrive/locationd/ublox_msg.h

@@ -4,11 +4,15 @@
 #include <memory>
 #include <string>
 #include <unordered_map>
+#include <ctime>
 
 #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 {