IQ.Pilot Release Commit @ d2ce8a8

iqpilot/selfdrive/car/card.py

@@ -253,6 +253,9 @@ class Car:
     # TODO: mirror the carState.cruiseState struct?
     CS.vCruise = float(self.v_cruise_helper.v_cruise_kph)
     CS.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph)
+    CS_IQ.slcSetSpeedRequestId = self.v_cruise_helper.slc_set_speed_request_id
+    CS_IQ.slcSetSpeedGestureId = self.v_cruise_helper.slc_set_speed_gesture_id
+    CS_IQ.slcSetSpeedRequestKph = self.v_cruise_helper.slc_set_speed_request_kph
 
     return CS, CS_IQ, RD
 

iqpilot/selfdrive/car/cruise.py

@@ -6,6 +6,7 @@ from iqpilot.common.constants import CV
 from iqpilot.cereal import car, custom
 from iqdbc.car import structs
 from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_CTRL
 from iqpilot.selfdrive.car.long_increments import LongIncrementConfig, read_long_increment_config, resolve_button_step
 
 
@@ -237,6 +238,35 @@ class VCruiseHelper(VCruiseHelperIQ):
     self.v_cruise_kph_last = 0
     self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0}
     self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}
+    self.slc_set_speed_request_id = 0
+    self.slc_set_speed_gesture_id = 0
+    self.slc_set_speed_request_kph = 0.0
+    self._slc_accel_held = False
+    self._slc_accel_release_frames = 0
+    self._slc_pcm_speed_last = None
+
+  def _update_slc_accel_gesture(self, CS):
+    self._slc_accel_release_frames = max(0, self._slc_accel_release_frames - 1)
+    for button in CS.buttonEvents:
+      if button.type in (ButtonType.accelCruise, ButtonType.resumeCruise):
+        if button.pressed:
+          self.slc_set_speed_gesture_id = (self.slc_set_speed_gesture_id + 1) % (1 << 32)
+          self._slc_accel_release_frames = 0
+        else:
+          self._slc_accel_release_frames = int(0.5 / DT_CTRL)
+        self._slc_accel_held = button.pressed
+      elif button.pressed:
+        self._slc_accel_held = False
+        self._slc_accel_release_frames = 0
+    if not CS.cruiseState.available:
+      self._slc_accel_held = False
+      self._slc_accel_release_frames = 0
+
+  def _record_slc_set_speed_increase(self, previous_kph, enabled):
+    if enabled and (self._slc_accel_held or self._slc_accel_release_frames > 0) and \
+       previous_kph is not None and 0 < previous_kph < self.v_cruise_kph <= V_CRUISE_MAX:
+      self.slc_set_speed_request_id = (self.slc_set_speed_request_id + 1) % (1 << 32)
+      self.slc_set_speed_request_kph = float(self.v_cruise_kph)
 
   @property
   def v_cruise_initialized(self):
@@ -248,6 +278,7 @@ class VCruiseHelper(VCruiseHelperIQ):
 
   def update_v_cruise(self, CS, enabled, is_metric):
     self.v_cruise_kph_last = self.v_cruise_kph
+    self._update_slc_accel_gesture(CS)
 
     self.get_minimum_set_speed(is_metric)
 
@@ -256,12 +287,15 @@ class VCruiseHelper(VCruiseHelperIQ):
       if not self.CP.pcmCruise or (not self.CP_IQ.pcmCruiseSpeed and _enabled):
         # if stock cruise is completely disabled, then we can use our own set speed logic
         self._update_v_cruise_non_pcm(CS, _enabled, is_metric, self.volkswagen_standby_set_speed)
+        self._record_slc_set_speed_increase(self.v_cruise_kph_last, _enabled)
         self.update_speed_limit_assist_v_cruise_non_pcm()
         self.v_cruise_cluster_kph = self.v_cruise_kph
         self.update_button_timers(CS, enabled)
       else:
         self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
         self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
+        self._record_slc_set_speed_increase(self._slc_pcm_speed_last, _enabled)
+        self._slc_pcm_speed_last = self.v_cruise_kph
         if CS.cruiseState.speed == 0:
           self.v_cruise_kph = V_CRUISE_UNSET
           self.v_cruise_cluster_kph = V_CRUISE_UNSET
@@ -273,6 +307,7 @@ class VCruiseHelper(VCruiseHelperIQ):
     else:
       self.v_cruise_kph = V_CRUISE_UNSET
       self.v_cruise_cluster_kph = V_CRUISE_UNSET
+      self._slc_pcm_speed_last = None
 
   def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, allow_standby_adjustment=False):
     # handle button presses. TODO: this should be in state_control, but a decelCruise press

iqpilot/selfdrive/car/tests/test_speed_limit_set_speed.py

@@ -73,6 +73,65 @@ class TestSpeedLimitSetSpeedMirror:
     assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(13.41 * CV.MS_TO_KPH, abs=0.1)
 
 
+@pytest.mark.parametrize("pcm_cruise", [False, True])
+@pytest.mark.parametrize("is_metric", [False, True])
+def test_driver_increase_marker_survives_button_release(pcm_cruise, is_metric):
+  helper = VCruiseHelper(car.CarParams(pcmCruise=pcm_cruise), custom.IQCarParams(pcmCruiseSpeed=True))
+  helper.set_speed_to_limit = False
+  helper.v_cruise_kph = helper.v_cruise_cluster_kph = 80.0
+  state = car.CarState(cruiseState={"available": True, "speed": 80 * CV.KPH_TO_MS, "speedCluster": 80 * CV.KPH_TO_MS})
+  helper.update_v_cruise(state, True, is_metric)
+  state.buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=True)]
+  helper.update_v_cruise(state, True, is_metric)
+  assert helper.slc_set_speed_request_id == 0
+  state.buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=False)]
+  if pcm_cruise:
+    state.cruiseState.speed = state.cruiseState.speedCluster = 81 * CV.KPH_TO_MS
+  helper.update_v_cruise(state, True, is_metric)
+  request = custom.IQCarState.new_message(
+    slcSetSpeedRequestId=helper.slc_set_speed_request_id,
+    slcSetSpeedGestureId=helper.slc_set_speed_gesture_id,
+    slcSetSpeedRequestKph=helper.slc_set_speed_request_kph,
+  )
+  assert request.slcSetSpeedRequestId == 1
+  assert request.slcSetSpeedRequestKph == pytest.approx(helper.v_cruise_kph)
+  state.buttonEvents = []
+  for _ in range(100):
+    helper.update_v_cruise(state, True, is_metric)
+  assert helper.slc_set_speed_request_id == request.slcSetSpeedRequestId
+  assert helper.slc_set_speed_gesture_id == request.slcSetSpeedGestureId
+
+
+def test_stock_cruise_sync_is_not_a_driver_increase():
+  helper = VCruiseHelper(car.CarParams(pcmCruise=True, openpilotLongitudinalControl=True),
+                        custom.IQCarParams(pcmCruiseSpeed=True))
+  helper.set_speed_to_limit = True
+  helper.update_speed_limit_assist(True, TestSpeedLimitSetSpeedMirror._iq_plan(
+    50 * CV.KPH_TO_MS, custom.IQPlan.SpeedLimit.AssistState.active))
+  state = car.CarState(cruiseState={"available": True, "speed": 80 * CV.KPH_TO_MS, "speedCluster": 80 * CV.KPH_TO_MS})
+  for _ in range(10):
+    helper.update_v_cruise(state, True, True)
+  assert helper.slc_set_speed_request_id == 0
+  state.cruiseState.speed = state.cruiseState.speedCluster = 90 * CV.KPH_TO_MS
+  helper.update_v_cruise(state, True, True)
+  assert helper.slc_set_speed_request_id == 0
+
+
+def test_held_increase_uses_one_gesture_and_multiple_requests():
+  helper = VCruiseHelper(car.CarParams(pcmCruise=False), custom.IQCarParams(pcmCruiseSpeed=True))
+  helper.set_speed_to_limit = False
+  helper.v_cruise_kph = 80.0
+  state = car.CarState(cruiseState={"available": True})
+  state.buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=True)]
+  helper.update_v_cruise(state, True, True)
+  state.buttonEvents = []
+  for _ in range(110):
+    helper.update_v_cruise(state, True, True)
+  assert helper.slc_set_speed_gesture_id == 1
+  assert helper.slc_set_speed_request_id >= 2
+  assert helper.slc_set_speed_request_kph == pytest.approx(helper.v_cruise_kph)
+
+
 def test_set_speed_does_not_follow_limit_when_feature_off():
   # Default off: set speed must stay the driver's value (limiter-only via planner min-blend).
   CP = car.CarParams(pcmCruise=True, openpilotLongitudinalControl=True)