Store almanac on ublox poweroff (#20967)

* Store almanac on ublox poweroff

* send current UTC time

* move message building to ublox_msg.h
old-commit-hash: ea5141d9096d64890a23a806612eb4edf58ea548

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 {