locationd: Commissue fix (#23951)

* check allAliveAndValid only at filter init

* revert cereal and bugfix test

* rename filterValid to filterInitialized

* bump cereal

* correct err

* update refs
old-commit-hash: dfc1292614

cereal

@@ -1 +1 @@
-Subproject commit ad2fe885dab99896908b88e765a5f720bfd79b3b
+Subproject commit e20113a5551810da1afdc5eed5dda3bd37dacfdd

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<char> arr = localizer->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK).asChars();
+    kj::ArrayPtr<char> arr = localizer->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, msgValid).asChars();
     assert(buff_size >= arr.size());
     memcpy(buff, arr.begin(), arr.size());
   }

selfdrive/locationd/locationd.cc

@@ -457,15 +457,16 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
 }
 
 kj::ArrayPtr<capnp::byte> 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<capnp::byte> bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK);
+      kj::ArrayPtr<capnp::byte> 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

selfdrive/locationd/locationd.h

@@ -36,7 +36,7 @@ public:
   void determine_gps_mode(double current_time);
 
   kj::ArrayPtr<capnp::byte> 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();

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):

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-37aa2e3afedc8364d2bbecc5f1b7ed4efec52e30
+e4a770725fd7240388e1cadf6baf9ee456f65aaf