IQ.Pilot Release Commit @ bec7652

2026-08-22 23:42:41 -05:00
commit 58039e647c
4603 changed files with 1236178 additions and 0 deletions
--- a/iqpilot/selfdrive/locationd/test/test_paramsd.py
+++ b/iqpilot/selfdrive/locationd/test/test_paramsd.py
@@ -0,0 +1,71 @@
+import random
+import numpy as np
+
+from iqpilot.cereal import messaging
+from iqpilot.selfdrive.locationd.paramsd import retrieve_initial_vehicle_params, migrate_cached_vehicle_params_if_needed
+from iqpilot.selfdrive.locationd.models.car_kf import CarKalman
+from iqpilot.selfdrive.locationd.test.test_locationd_scenarios import TEST_ROUTE
+from iqpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams
+from iqpilot.common.params import Params
+from iqpilot.tools.lib.logreader import LogReader
+
+
+def get_random_live_parameters(CP):
+  msg = messaging.new_message("vehicleParameters")
+  msg.vehicleParameters.steerRatio = (random.random() + 0.5) * CP.steerRatio
+  msg.vehicleParameters.stiffnessFactor = random.random()
+  msg.vehicleParameters.angleOffsetAverageDeg = random.random()
+  msg.vehicleParameters.debugFilterState.std = [random.random() for _ in range(CarKalman.P_initial.shape[0])]
+  return msg
+
+
+class TestParamsd:
+  def test_read_saved_params(self):
+    params = Params()
+
+    lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
+    CP = next(m for m in lr if m.which() == "carParams").carParams
+
+    msg = get_random_live_parameters(CP)
+    params.put("LiveParametersV2", msg.to_bytes())
+    params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
+
+    migrate_cached_vehicle_params_if_needed(params) # this is not tested here but should not mess anything up or throw an error
+    sr, sf, offset, p_init = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True)
+    np.testing.assert_allclose(sr, msg.vehicleParameters.steerRatio)
+    np.testing.assert_allclose(sf, msg.vehicleParameters.stiffnessFactor)
+    np.testing.assert_allclose(offset, msg.vehicleParameters.angleOffsetAverageDeg)
+    np.testing.assert_equal(p_init.shape, CarKalman.P_initial.shape)
+    np.testing.assert_allclose(np.diagonal(p_init), msg.vehicleParameters.debugFilterState.std)
+
+  # TODO Remove this test after the support for old format is removed
+  def test_read_saved_params_old_format(self):
+    params = Params()
+
+    lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
+    CP = next(m for m in lr if m.which() == "carParams").carParams
+
+    msg = get_random_live_parameters(CP)
+    params.put("LiveParameters", {
+      "steerRatio": msg.vehicleParameters.steerRatio,
+      "stiffnessFactor": msg.vehicleParameters.stiffnessFactor,
+      "angleOffsetAverageDeg": msg.vehicleParameters.angleOffsetAverageDeg,
+    })
+    params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
+    params.remove("LiveParametersV2")
+
+    migrate_cached_vehicle_params_if_needed(params)
+    sr, sf, offset, _ = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True)
+    np.testing.assert_allclose(sr, msg.vehicleParameters.steerRatio)
+    np.testing.assert_allclose(sf, msg.vehicleParameters.stiffnessFactor)
+    np.testing.assert_allclose(offset, msg.vehicleParameters.angleOffsetAverageDeg)
+    assert params.get("LiveParametersV2") is not None
+
+  def test_read_saved_params_corrupted_old_format(self):
+    params = Params()
+    params.put("LiveParameters", {})
+    params.remove("LiveParametersV2")
+
+    migrate_cached_vehicle_params_if_needed(params)
+    assert params.get("LiveParameters") is None
+    assert params.get("LiveParametersV2") is None