locationd, paramsd: Check conditions before updating kalman filters (commaai#23789)

* update filters only when all messages are alivbe and valid

* update message valid and fix unit test

* update refs

* move check outside loop

* modify fake message fn in test

* deprecate inputsOK and resolve PR comments

* avoid double looped list comprehension

* follow import conventions

* modify paramsd valid to only be invalid in case of commIssue

* update refs

Author: nuwandavek

cereal

@@ -262,7 +262,6 @@ void Localizer::input_fake_gps_observations(double current_time) {
 
 void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log) {
   // ignore the message if the fix is invalid
-
   bool gps_invalid_flag = (log.getFlags() % 2 == 0);
   bool gps_unreasonable = (Vector2d(log.getAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
   bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
@@ -462,9 +461,9 @@ kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_build
 {
   cereal::Event::Builder evt = msg_builder.initEvent();
   evt.setLogMonoTime(logMonoTime);
+  evt.setValid(inputsOK);
   cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
   this->build_live_location(liveLoc);
-  liveLoc.setInputsOK(inputsOK);
   liveLoc.setSensorsOK(sensorsOK);
   liveLoc.setGpsOK(gpsOK);
   return msg_builder.toBytes();
@@ -499,12 +498,15 @@ int Localizer::locationd_thread() {
 
   while (!do_exit) {
     sm.update();
-    for (const char* service : service_list) {
-      if (sm.updated(service) && sm.valid(service)) {
-        const cereal::Event::Reader log = sm[service];
-        this->handle_msg(log);
+    if (sm.allAliveAndValid()){
+      for (const char* service : service_list) {
+        if (sm.updated(service)){
+          const cereal::Event::Reader log = sm[service];
+          this->handle_msg(log);
+        }
       }
     }
+    
 
     if (sm.updated("cameraOdometry")) {
       uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime();

selfdrive/locationd/locationd.cc

@@ -62,7 +62,7 @@ def handle_log(self, t, which, msg):
       yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1  # rad/s
 
       if self.active:
-        if msg.inputsOK and msg.posenetOK:
+        if msg.posenetOK:
 
           if yaw_rate_valid:
             self.kf.predict_and_observe(t,
@@ -160,10 +160,11 @@ def main(sm=None, pm=None):
 
   while True:
     sm.update()
-    for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]):
-      if sm.updated[which]:
-        t = sm.logMonoTime[which] * 1e-9
-        learner.handle_log(t, which, sm[which])
+    if sm.all_alive_and_valid():
+      for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]):
+        if sm.updated[which]:
+          t = sm.logMonoTime[which] * 1e-9
+          learner.handle_log(t, which, sm[which])
 
     if sm.updated['liveLocationKalman']:
       x = learner.kf.x
@@ -198,6 +199,8 @@ def main(sm=None, pm=None):
       liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET])
       liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST])
 
+      msg.valid = sm.all_alive_and_valid()
+
       if sm.frame % 1200 == 0:  # once a minute
         params = {
           'carFingerprint': CP.carFingerprint,

selfdrive/locationd/paramsd.py

@@ -4,10 +4,12 @@
 import random
 import unittest
 import time
+import capnp
 from cffi import FFI
 
 from cereal import log
 import cereal.messaging as messaging
+from cereal.services import service_list
 from common.params import Params
 
 from selfdrive.manager.process_config import managed_processes
@@ -103,13 +105,15 @@ def test_posenet_spike(self):
     ret = self.localizer_get_msg()
     self.assertFalse(ret.liveLocationKalman.posenetOK)
 
+
 class TestLocationdProc(unittest.TestCase):
   MAX_WAITS = 1000
+  LLD_MSGS = {'gpsLocationExternal', 'cameraOdometry', 'carState', 'sensorEvents', 'liveCalibration'}
 
   def setUp(self):
     random.seed(123489234)
 
-    self.pm = messaging.PubMaster({'gpsLocationExternal', 'cameraOdometry'})
+    self.pm = messaging.PubMaster(self.LLD_MSGS)
 
     managed_processes['locationd'].prepare()
     managed_processes['locationd'].start()
@@ -127,43 +131,53 @@ def send_msg(self, msg):
       waits_left -= 1
     time.sleep(0.0001)
 
-  def test_params_gps(self):
-    # first reset params
-    Params().put('LastGPSPosition', json.dumps({"latitude": 0.0, "longitude": 0.0, "altitude": 0.0}))
-
-    lat = 30 + (random.random() * 10.0)
-    lon = -70 + (random.random() * 10.0)
-    alt = 5 + (random.random() * 10.0)
+  def get_fake_msg(self, name, t):
+    try:
+      msg = messaging.new_message(name)
+    except capnp.lib.capnp.KjException:
+      msg = messaging.new_message(name, 0)
 
-    for _ in range(1000):  # because of kalman filter, send often
-      msg = messaging.new_message('gpsLocationExternal')
-      msg.logMonoTime = 0
+    if name == "gpsLocationExternal":
       msg.gpsLocationExternal.flags = 1
       msg.gpsLocationExternal.verticalAccuracy = 1.0
       msg.gpsLocationExternal.speedAccuracy = 1.0
       msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
       msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
-      msg.gpsLocationExternal.latitude = lat
-      msg.gpsLocationExternal.longitude = lon
-      msg.gpsLocationExternal.altitude = alt
-      self.send_msg(msg)
-
-    for _ in range(250):  # params is only written so often
-      msg = messaging.new_message('cameraOdometry')
-      msg.logMonoTime = 0
+      msg.gpsLocationExternal.latitude = self.lat
+      msg.gpsLocationExternal.longitude = self.lon
+      msg.gpsLocationExternal.altitude = self.alt
+    elif name == 'cameraOdometry':
       msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
       msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
       msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
       msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
-      self.send_msg(msg)
+    msg.logMonoTime = t
+    return msg
 
+  def test_params_gps(self):
+    # first reset params
+    Params().delete('LastGPSPosition')
+
+    self.lat = 30 + (random.random() * 10.0)
+    self.lon = -70 + (random.random() * 10.0)
+    self.alt = 5 + (random.random() * 10.0)
+    self.fake_duration = 90  # secs
+    # get fake messages at the correct frequency, listed in services.py
+    fake_msgs = []
+    for sec in range(self.fake_duration):
+      for name in self.LLD_MSGS:
+        for j in range(int(service_list[name].frequency)):
+          fake_msgs.append(self.get_fake_msg(name, int((sec + j / service_list[name].frequency) * 1e9)))
+
+    for fake_msg in sorted(fake_msgs, key=lambda x: x.logMonoTime):
+      self.send_msg(fake_msg)
     time.sleep(1)  # wait for async params write
 
     lastGPS = json.loads(Params().get('LastGPSPosition'))
 
-    self.assertAlmostEqual(lastGPS['latitude'], lat, places=3)
-    self.assertAlmostEqual(lastGPS['longitude'], lon, places=3)
-    self.assertAlmostEqual(lastGPS['altitude'], alt, places=3)
+    self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=3)
+    self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=3)
+    self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=3)
 
 
 if __name__ == "__main__":

selfdrive/locationd/test/test_locationd.py

@@ -1 +1 @@
-7e6072a254791e4106a15ecbf94c16f40d54b459
+072ee2ba6bca1ea5943fef27b0cc764e40275568