Comment

selfdrive/locationd/locationd.py

@@ -24,7 +24,7 @@
 MAX_FILTER_REWIND_TIME = 0.8  # s
 MAX_SENSOR_TIME_DIFF = 0.1  # s
 YAWRATE_CROSS_ERR_CHECK_FACTOR = 30
-INPUT_INVALID_LIMIT = 2.0 # 1 bad input[s] ignored
+INPUT_INVALID_LIMIT = 2.0 # 1 (camodo) / 9 (sensor) bad input[s] ignored
 INPUT_INVALID_RECOVERY = 10.0 # ~10 secs to resume after exceeding allowed bad inputs by one
 POSENET_STD_INITIAL_VALUE = 10.0
 POSENET_STD_HIST_HALF = 20
@@ -276,6 +276,7 @@ def main():
   input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services}
   input_invalid_threshold = {s: input_invalid_limit[s] - 0.5 for s in critcal_services}
   input_invalid_decay = {s: calculate_invalid_input_decay(input_invalid_limit[s], INPUT_INVALID_RECOVERY, SERVICE_LIST[s].frequency) for s in critcal_services}
+  print(input_invalid_limit, input_invalid_threshold, input_invalid_decay)
 
   initial_pose = params.get("LocationFilterInitialState")
   if initial_pose is not None:
@@ -310,6 +311,7 @@ def main():
           if res == HandleLogResult.TIMING_INVALID:
             print(f"Observation {which} ignored due to failed timing check")
             observation_input_invalid[which] += 1
+            print(observation_input_invalid[which])
           elif res == HandleLogResult.INPUT_INVALID:
             print(f"Observation {which} ignored due to failed sanity check")
             observation_input_invalid[which] += 1
@@ -320,6 +322,8 @@ def main():
 
     if sm.updated["cameraOdometry"]:
       critical_service_inputs_valid = all(observation_input_invalid[s] < input_invalid_threshold[s] for s in critcal_services)
+      if not critical_service_inputs_valid:
+        print(observation_input_invalid)
       inputs_valid = sm.all_valid() and critical_service_inputs_valid
       sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)