spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Boardd: Track ublox message frequency and re-init (#20278) 

* track message frequency

* also keep old reset trigger

* cleanup

* improve comments

* Add 10 seconds

* these are now signed

* also check ignition last

* cleanup

* correct include

* lower threshold

* dont clear map
pull/20288/head
Willem Melching 2021-03-09 11:49:58 +01:00 committed by GitHub
parent a7672e2851
commit 4fc81c2a08
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 40 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
selfdrive/boardd/boardd.cc
View File

@ -15,6 +15,7 @@
#include <bitset>
#include <thread>
#include <atomic>
#include <unordered_map>
#include <libusb-1.0/libusb.h>
@ -25,6 +26,7 @@
#include "common/swaglog.h"
#include "common/timing.h"
#include "messaging.hpp"
#include "locationd/ublox_msg.h"
#include "panda.h"
#include "pigeon.h"
@ -482,24 +484,54 @@ void pigeon_thread() {
Pigeon * pigeon = Pigeon::connect(panda);
#endif
std::unordered_map<char, uint64_t> last_recv_time;
std::unordered_map<char, int64_t> cls_max_dt = {
{(char)ublox::CLASS_NAV, int64_t(500000000ULL)}, // 0.5s - msg is 10Hz
{(char)ublox::CLASS_RXM, int64_t(500000000ULL)}, // 0.5s - msg is 10Hz
{(char)ublox::CLASS_MON, int64_t(2000000000ULL)}, // 2.0s - msg is 1Hz
};
while (!do_exit && panda->connected) {
bool need_reset = false;
std::string recv = pigeon->receive();
if (recv.length() > 0) {
if (recv[0] == (char)0x00){
if (ignition) {
LOGW("received invalid ublox message while onroad, resetting panda GPS");
need_reset = true;
}
} else {
pigeon_publish_raw(pm, recv);
// Parse message header
if (ignition && recv.length() >= 3) {
if (recv[0] == (char)ublox::PREAMBLE1 && recv[1] == (char)ublox::PREAMBLE2){
const char msg_cls = recv[2];
last_recv_time[msg_cls] = nanos_since_boot();
}
}
// Check based on message frequency
for (const auto& [msg_cls, max_dt] : cls_max_dt) {
int64_t dt = (int64_t)nanos_since_boot() - (int64_t)last_recv_time[msg_cls];
if (ignition_last && ignition && dt > max_dt) {
LOGE("ublox receive timeout, msg class: 0x%02x, dt %llu, resetting panda GPS", msg_cls, dt);
need_reset = true;
}
}
// Check based on null bytes
if (ignition && recv.length() > 0 && recv[0] == (char)0x00){
need_reset = true;
LOGW("received invalid ublox message while onroad, resetting panda GPS");
}
if (recv.length() > 0){
pigeon_publish_raw(pm, recv);
}
// init pigeon on rising ignition edge
// since it was turned off in low power mode
if((ignition && !ignition_last) || need_reset) {
pigeon->init();
// Set receive times to current time
uint64_t t = nanos_since_boot() + 10000000000ULL; // Give ublox 10 seconds to start
for (const auto& [msg_cls, dt] : cls_max_dt) {
last_recv_time[msg_cls] = t;
}
}
ignition_last = ignition;