add check for restoring almanac, and clear like suggested in the datasheet (#23153)

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: bb7208f4c3

selfdrive/boardd/pigeon.cc

@@ -24,8 +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";
+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();
@@ -72,6 +72,25 @@ bool Pigeon::send_with_ack(const std::string &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<sos_restore_response>(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;
@@ -118,6 +137,22 @@ 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;
 
+    // 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");
@@ -139,7 +174,7 @@ void Pigeon::stop() {
   // 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)) {
+  if (wait_for_ack(sos_save_ack, sos_save_nack)) {
     LOGW("Done storing almanac");
   } else {
     LOGE("Error storing almanac");

selfdrive/boardd/pigeon.h

@@ -7,6 +7,14 @@
 
 #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);
@@ -18,6 +26,7 @@ class Pigeon {
   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;

selfdrive/locationd/test/ubloxd.py

@@ -61,6 +61,13 @@ 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)
 
+  # Query the backup restore status
+  print("backup restore polling message (implement custom response handler!):")
+  dev.configure_poll(0x09, 0x14)
+
+  print("if succesfull, send this to clear the flash:")
+  dev.send_message(0x09, 0x14, b"\x01\x00\x00\x00")
+
   print("send on stop:")
 
   # Save on shutdown

selfdrive/locationd/ublox_msg.cc

@@ -37,11 +37,11 @@ inline bool UbloxMsgParser::valid_cheksum() {
     ck_b = (ck_b + ck_a) & 0xFF;
   }
   if(ck_a != msg_parse_buf[bytes_in_parse_buf - 2]) {
-    LOGD("Checksum a mismtach: %02X, %02X", ck_a, msg_parse_buf[6]);
+    LOGD("Checksum a mismatch: %02X, %02X", ck_a, msg_parse_buf[6]);
     return false;
   }
   if(ck_b != msg_parse_buf[bytes_in_parse_buf - 1]) {
-    LOGD("Checksum b mismtach: %02X, %02X", ck_b, msg_parse_buf[7]);
+    LOGD("Checksum b mismatch: %02X, %02X", ck_b, msg_parse_buf[7]);
     return false;
   }
   return true;