add check for restoring almanac, and clear like suggested in the datasheet (#23153)
Co-authored-by: Comma Device <device@comma.ai>pull/23168/head
parent
c8e64b2c62
commit
bb7208f4c3
|
@ -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");
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue