bring back car state speed limit

selfdrive/controls/plannerd.py

@@ -20,7 +20,7 @@ def main():
   longitudinal_planner = LongitudinalPlanner(CP)
   pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
   sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState' +
-                            "liveMapDataSP"],
+                            "liveMapDataSP" + "carStateSP"],
                            poll='modelV2')
 
   while True:

sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py

@@ -46,13 +46,12 @@ class SpeedLimitResolver:
     self._get_from_map_data()
 
   def _get_from_car_state(self):
-    if not self._is_sock_updated('carState'):
-      debug('SL: No carState instruction for speed limit')
+    if not self._is_sock_updated('carStateSP'):
+      debug('SL: No carStateSP instruction for speed limit')
       return
 
     self._reset_limit_sources(Source.car_state)
-    # TODO-SP: need CS_SP for speedLimit from the car
-    self._limit_solutions[Source.car_state] = 0.  # self._sm['carState'].cruiseState.speedLimit
+    self._limit_solutions[Source.car_state] = self._sm['carStateSP'].speedLimit
     self._distance_solutions[Source.car_state] = 0.
 
   def _get_from_map_data(self):

sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py

@@ -20,17 +20,16 @@ def create_mock(properties, mocker: MockerFixture):
 
 def setup_sm_mock(mocker: MockerFixture):
   cruise_speed_limit = random.uniform(0, 120)
-  nav_instruction_limit = random.uniform(0, 120)
   live_map_data_limit = random.uniform(0, 120)
 
-  cruise_state = create_mock({'speedLimit': cruise_speed_limit}, mocker)
   car_state = create_mock({
-    'cruiseState': cruise_state,
     'gasPressed': False,
     'brakePressed': False,
     'standstill': False,
   }, mocker)
-  nav_instruction = create_mock({'speedLimit': nav_instruction_limit}, mocker)
+  car_state_sp = create_mock({
+    'speedLimit': cruise_speed_limit,
+  }, mocker)
   live_map_data = create_mock({
     'speedLimit': live_map_data_limit,
     'speedLimitValid': True,
@@ -43,14 +42,15 @@ def setup_sm_mock(mocker: MockerFixture):
   sm_mock.__getitem__.side_effect = lambda key: {
     'carState': car_state,
     'liveMapDataSP': live_map_data,
+    'carStateSP': car_state_sp,
   }[key]
   return sm_mock
 
 
 parametrized_policies = pytest.mark.parametrize(
   "policy, sm_key, function_key", [
-    (Policy.car_state_only, 'carState', 'car_state'),
-    (Policy.car_state_priority, 'carState', 'car_state'),
+    (Policy.car_state_only, 'carStateSP', 'car_state'),
+    (Policy.car_state_priority, 'carStateSP', 'car_state'),
     (Policy.map_data_only, 'liveMapDataSP', 'map_data'),
     (Policy.map_data_priority, 'liveMapDataSP', 'map_data'),
   ],
@@ -73,7 +73,7 @@ class TestSpeedLimitResolverValidation:
   def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
     resolver = resolver_class(policy)
     sm_mock = setup_sm_mock(mocker)
-    source_speed_limit = sm_mock[sm_key].cruiseState.speedLimit if sm_key == 'carState' else sm_mock[sm_key].speedLimit
+    source_speed_limit = sm_mock[sm_key].speedLimit
 
     # Assert the resolver
     speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock)
@@ -83,9 +83,9 @@ class TestSpeedLimitResolverValidation:
   def test_resolver_combined(self, resolver_class, mocker: MockerFixture):
     resolver = resolver_class(Policy.combined)
     sm_mock = setup_sm_mock(mocker)
-    socket_to_source = {'carState': Source.car_state, 'liveMapDataSP': Source.map_data}
+    socket_to_source = {'carStateSP': Source.car_state, 'liveMapDataSP': Source.map_data}
     minimum_key, minimum_speed_limit = min(
-      ((key, sm_mock[key].cruiseState.speedLimit) if key == 'carState' else (key, sm_mock[key].speedLimit) for key in
+      ((key, sm_mock[key].speedLimit) for key in
        socket_to_source.keys()), key=lambda x: x[1])
 
     # Assert the resolver
@@ -97,7 +97,7 @@ class TestSpeedLimitResolverValidation:
   def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
     resolver = resolver_class(policy)
     sm_mock = setup_sm_mock(mocker)
-    source_speed_limit = sm_mock[sm_key].cruiseState.speedLimit if sm_key == 'carState' else sm_mock[sm_key].speedLimit
+    source_speed_limit = sm_mock[sm_key].speedLimit
 
     # Assert the parsing
     speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock)