Merge remote-tracking branch 'origin/master' into log-compat-test

common/params_keys.h

@@ -72,6 +72,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"LastUpdateException", CLEAR_ON_MANAGER_START},
     {"LastUpdateTime", PERSISTENT},
     {"LiveParameters", PERSISTENT},
+    {"LiveParametersV2", PERSISTENT},
     {"LiveTorqueParameters", PERSISTENT | DONT_LOG},
     {"LocationFilterInitialState", PERSISTENT},
     {"LongitudinalManeuverMode", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},

selfdrive/locationd/paramsd.py

@@ -200,26 +200,28 @@ def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: floa
 
 
 # TODO: Remove this function after few releases (added in 0.9.9)
-def migrate_cached_vehicle_params_if_needed(params_reader: Params):
-  last_parameters_data = params_reader.get("LiveParameters")
-  if last_parameters_data is None:
+def migrate_cached_vehicle_params_if_needed(params: Params):
+  last_parameters_data_old = params.get("LiveParameters")
+  last_parameters_data = params.get("LiveParametersV2")
+  if last_parameters_data_old is None or last_parameters_data is not None:
     return
 
   try:
-    last_parameters_dict = json.loads(last_parameters_data)
+    last_parameters_dict = json.loads(last_parameters_data_old)
     last_parameters_msg = messaging.new_message('liveParameters')
     last_parameters_msg.liveParameters.valid = True
     last_parameters_msg.liveParameters.steerRatio = last_parameters_dict['steerRatio']
     last_parameters_msg.liveParameters.stiffnessFactor = last_parameters_dict['stiffnessFactor']
     last_parameters_msg.liveParameters.angleOffsetAverageDeg = last_parameters_dict['angleOffsetAverageDeg']
-    params_reader.put("LiveParameters", last_parameters_msg.to_bytes())
-  except Exception:
-    pass
+    params.put("LiveParametersV2", last_parameters_msg.to_bytes())
+  except Exception as e:
+    cloudlog.error(f"Failed to perform parameter migration: {e}")
+    params.remove("LiveParameters")
 
 
-def retrieve_initial_vehicle_params(params_reader: Params, CP: car.CarParams, replay: bool, debug: bool):
-  last_parameters_data = params_reader.get("LiveParameters")
-  last_carparams_data = params_reader.get("CarParamsPrevRoute")
+def retrieve_initial_vehicle_params(params: Params, CP: car.CarParams, replay: bool, debug: bool):
+  last_parameters_data = params.get("LiveParametersV2")
+  last_carparams_data = params.get("CarParamsPrevRoute")
 
   steer_ratio, stiffness_factor, angle_offset_deg, p_initial = CP.steerRatio, 1.0, 0.0, None
 
@@ -267,12 +269,12 @@ def main():
   pm = messaging.PubMaster(['liveParameters'])
   sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState'], poll='livePose')
 
-  params_reader = Params()
-  CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
+  params = Params()
+  CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
 
-  migrate_cached_vehicle_params_if_needed(params_reader)
+  migrate_cached_vehicle_params_if_needed(params)
 
-  steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params_reader, CP, REPLAY, DEBUG)
+  steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params, CP, REPLAY, DEBUG)
   learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial)
 
   while True:
@@ -288,7 +290,7 @@ def main():
 
       msg_dat = msg.to_bytes()
       if sm.frame % 1200 == 0:  # once a minute
-        params_reader.put_nonblocking("LiveParameters", msg_dat)
+        params.put_nonblocking("LiveParametersV2", msg_dat)
 
       pm.send('liveParameters', msg_dat)
 

selfdrive/locationd/test/test_paramsd.py

@@ -28,7 +28,7 @@ class TestParamsd:
     CP = next(m for m in lr if m.which() == "carParams").carParams
 
     msg = get_random_live_parameters(CP)
-    params.put("LiveParameters", msg.to_bytes())
+    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
@@ -49,9 +49,20 @@ class TestParamsd:
     msg = get_random_live_parameters(CP)
     params.put("LiveParameters", json.dumps(msg.liveParameters.to_dict()))
     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.liveParameters.steerRatio)
     np.testing.assert_allclose(sf, msg.liveParameters.stiffnessFactor)
     np.testing.assert_allclose(offset, msg.liveParameters.angleOffsetAverageDeg)
+    assert params.get("LiveParametersV2") is not None
+
+  def test_read_saved_params_corrupted_old_format(self):
+    params = Params()
+    params.put("LiveParameters", b'\x00\x00\x02\x00\x01\x00:F\xde\xed\xae;')
+    params.remove("LiveParametersV2")
+
+    migrate_cached_vehicle_params_if_needed(params)
+    assert params.get("LiveParameters") is None
+    assert params.get("LiveParametersV2") is None