From c9ef5b176e679ef8acb3aafbca43fe930cdb5de6 Mon Sep 17 00:00:00 2001 From: Robbe Derks Date: Wed, 8 Dec 2021 14:13:56 +0100 Subject: [PATCH] add check for restoring almanac, and clear like suggested in the datasheet (#23153) Co-authored-by: Comma Device old-commit-hash: bb7208f4c34fe70682f24a3e637b03aeaa4f719c --- selfdrive/boardd/pigeon.cc | 41 +++++++++++++++++++++++++++--- selfdrive/boardd/pigeon.h | 9 +++++++ selfdrive/locationd/test/ubloxd.py | 7 +++++ selfdrive/locationd/ublox_msg.cc | 4 +-- 4 files changed, 56 insertions(+), 5 deletions(-) diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc index b8735cc3b0..912f4b03e7 100644 --- a/selfdrive/boardd/pigeon.cc +++ b/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(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"); diff --git a/selfdrive/boardd/pigeon.h b/selfdrive/boardd/pigeon.h index 0a82bcdd95..c9ea4739dc 100644 --- a/selfdrive/boardd/pigeon.h +++ b/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; diff --git a/selfdrive/locationd/test/ubloxd.py b/selfdrive/locationd/test/ubloxd.py index 2246921434..af9b86127e 100755 --- a/selfdrive/locationd/test/ubloxd.py +++ b/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 diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index 52c3906f13..9e32c7b070 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/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;