diff --git a/cereal b/cereal index ad2fe885d..e20113a55 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit ad2fe885dab99896908b88e765a5f720bfd79b3b +Subproject commit e20113a5551810da1afdc5eed5dda3bd37dacfdd diff --git a/selfdrive/locationd/liblocationd.cc b/selfdrive/locationd/liblocationd.cc index 250d90b38..07216365b 100755 --- a/selfdrive/locationd/liblocationd.cc +++ b/selfdrive/locationd/liblocationd.cc @@ -8,10 +8,10 @@ extern "C" { } void localizer_get_message_bytes(Localizer *localizer, uint64_t logMonoTime, - bool inputsOK, bool sensorsOK, bool gpsOK, char *buff, size_t buff_size) + bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size) { MessageBuilder msg_builder; - kj::ArrayPtr arr = localizer->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK).asChars(); + kj::ArrayPtr arr = localizer->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, msgValid).asChars(); assert(buff_size >= arr.size()); memcpy(buff, arr.begin(), arr.size()); } diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 5cec492d6..6864ada0a 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -457,15 +457,16 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) { } kj::ArrayPtr Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, - bool inputsOK, bool sensorsOK, bool gpsOK) + bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid) { cereal::Event::Builder evt = msg_builder.initEvent(); evt.setLogMonoTime(logMonoTime); - evt.setValid(inputsOK); + evt.setValid(msgValid); cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman(); this->build_live_location(liveLoc); liveLoc.setSensorsOK(sensorsOK); liveLoc.setGpsOK(gpsOK); + liveLoc.setInputsOK(inputsOK); return msg_builder.toBytes(); } @@ -497,19 +498,21 @@ int Localizer::locationd_thread() { SubMaster sm(service_list, nullptr, { "gpsLocationExternal" }); uint64_t cnt = 0; + bool filterInitialized = false; while (!do_exit) { sm.update(); - if (sm.allAliveAndValid()){ + if (filterInitialized){ for (const char* service : service_list) { - if (sm.updated(service)){ + if (sm.updated(service) && sm.valid(service)){ const cereal::Event::Reader log = sm[service]; this->handle_msg(log); } } + } else { + filterInitialized = sm.allAliveAndValid(); } - if (sm.updated("cameraOdometry")) { uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime(); bool inputsOK = sm.allAliveAndValid(); @@ -517,7 +520,7 @@ int Localizer::locationd_thread() { bool gpsOK = this->isGpsOK(); MessageBuilder msg_builder; - kj::ArrayPtr bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK); + kj::ArrayPtr bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, filterInitialized); pm.send("liveLocationKalman", bytes.begin(), bytes.size()); if (cnt % 1200 == 0 && gpsOK) { // once a minute diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index 9bc864bf6..f75e33442 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -36,7 +36,7 @@ public: void determine_gps_mode(double current_time); kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime, - bool inputsOK, bool sensorsOK, bool gpsOK); + bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid); void build_live_location(cereal::LiveLocationKalman::Builder& fix); Eigen::VectorXd get_position_geodetic(); diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 515bd5943..e804917a0 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -24,7 +24,7 @@ class TestLocationdLib(unittest.TestCase): def setUp(self): header = '''typedef ...* Localizer_t; Localizer_t localizer_init(); -void localizer_get_message_bytes(Localizer_t localizer, uint64_t logMonoTime, bool inputsOK, bool sensorsOK, bool gpsOK, char *buff, size_t buff_size); +void localizer_get_message_bytes(Localizer_t localizer, uint64_t logMonoTime, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size); void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t size);''' self.ffi = FFI() @@ -40,8 +40,8 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t bytstr = msg_builder.to_bytes() self.lib.localizer_handle_msg_bytes(self.localizer, self.ffi.from_buffer(bytstr), len(bytstr)) - def localizer_get_msg(self, t=0, inputsOK=True, sensorsOK=True, gpsOK=True): - self.lib.localizer_get_message_bytes(self.localizer, t, inputsOK, sensorsOK, gpsOK, self.ffi.addressof(self.msg_buff, 0), self.buff_size) + def localizer_get_msg(self, t=0, inputsOK=True, sensorsOK=True, gpsOK=True, msgValid=True): + self.lib.localizer_get_message_bytes(self.localizer, t, inputsOK, sensorsOK, gpsOK, msgValid, self.ffi.addressof(self.msg_buff, 0), self.buff_size) return log.Event.from_bytes(self.ffi.buffer(self.msg_buff), nesting_limit=self.buff_size // 8) def test_liblocalizer(self): diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 3226b8614..7135a57ce 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -37aa2e3afedc8364d2bbecc5f1b7ed4efec52e30 \ No newline at end of file +e4a770725fd7240388e1cadf6baf9ee456f65aaf \ No newline at end of file