Stock longitudinal: spam resume button when lead starts moving

selfdrive/car/honda/carcontroller.py

@@ -195,7 +195,7 @@ def update(self, CC, CS):
       # If using stock ACC, spam cancel command to kill gas when OP disengages.
       if pcm_cancel_cmd:
         can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, self.CP.carFingerprint))
-      elif CS.out.cruiseState.standstill:
+      elif CC.cruiseControl.resume:
         can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, self.CP.carFingerprint))
 
     else:

selfdrive/car/hyundai/carcontroller.py

@@ -78,7 +78,7 @@ def update(self, CC, CS):
           self.last_button_frame = self.frame
 
         # cruise standstill resume
-        elif CC.enabled and CS.out.cruiseState.standstill:
+        elif CC.cruiseControl.resume:
           can_sends.append(hda2can.create_buttons(self.packer, CS.buttons_counter+1, False, True))
           self.last_button_frame = self.frame
     else:
@@ -96,7 +96,7 @@ def update(self, CC, CS):
       if not self.CP.openpilotLongitudinalControl:
         if CC.cruiseControl.cancel:
           can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL))
-        elif CS.out.cruiseState.standstill:
+        elif CC.cruiseControl.resume:
           # send resume at a max freq of 10Hz
           if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
             # send 25 messages at a time to increases the likelihood of resume being accepted

selfdrive/car/mazda/carcontroller.py

@@ -27,13 +27,6 @@ def update(self, c, CS, frame):
                                                   CS.out.steeringTorque, CarControllerParams)
       self.steer_rate_limited = new_steer != apply_steer
 
-    if c.enabled:
-      if CS.out.standstill and frame % 5 == 0:
-        # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
-        # Send Resume button at 20hz if we're engaged at standstill to support full stop and go!
-        # TODO: improve the resume trigger logic by looking at actual radar data
-        can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
-
     if c.cruiseControl.cancel:
       # If brake is pressed, let us wait >70ms before trying to disable crz to avoid
       # a race condition with the stock system, where the second cancel from openpilot
@@ -44,7 +37,12 @@ def update(self, c, CS, frame):
         # Cancel Stock ACC if it's enabled while OP is disengaged
         # Send at a rate of 10hz until we sync with stock ACC state
         can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
-    else:
+    elif c.cruiseControl.resume and frame % 5 == 0:
+      # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
+      # Send Resume button at when planner wants car to move
+      can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
+
+    if not c.cruiseControl.cancel:
       self.brake_counter = 0
 
     self.apply_steer_last = apply_steer

selfdrive/car/volkswagen/carcontroller.py

@@ -93,9 +93,8 @@ def update(self, c, CS, frame, ext_bus, actuators, visual_alert, left_lane_visib
           # Cancel ACC if it's engaged with OP disengaged.
           self.graButtonStatesToSend = BUTTON_STATES.copy()
           self.graButtonStatesToSend["cancel"] = True
-        elif c.enabled and CS.out.cruiseState.standstill:
-          # Blip the Resume button if we're engaged at standstill.
-          # FIXME: This is a naive implementation, improve with visiond or radar input.
+        elif c.cruiseControl.resume:
+          # Send Resume button when planner wants car to move
           self.graButtonStatesToSend = BUTTON_STATES.copy()
           self.graButtonStatesToSend["resumeCruise"] = True
 

selfdrive/controls/controlsd.py

@@ -646,6 +646,8 @@ def publish_logs(self, CS, start_time, CC, lac_log):
     CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
     if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
       CC.cruiseControl.cancel = True
+    if len(speeds := self.sm['longitudinalPlan'].speeds):
+      CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1
 
     hudControl = CC.hudControl
     hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)

selfdrive/controls/lib/longitudinal_planner.py

@@ -69,8 +69,9 @@ def update(self, sm):
     long_control_state = sm['controlsState'].longControlState
     force_slow_decel = sm['controlsState'].forceDecel
 
-    # Reset current state when not engaged, or user is controlling the speed
-    reset_state = long_control_state == LongCtrlState.off
+    # Reset current state when not engaged or user is controlling the speed
+    # No reset if car is not using openpilot longitudinal (for accurate planner predictions)
+    reset_state = long_control_state == LongCtrlState.off and self.CP.openpilotLongitudinalControl
 
     # No change cost when user is controlling the speed, or when standstill
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)

selfdrive/controls/radard.py

@@ -102,7 +102,7 @@ def __init__(self, radar_ts, delay=0):
 
     self.ready = False
 
-  def update(self, sm, rr, enable_lead):
+  def update(self, sm, rr):
     self.current_time = 1e-9*max(sm.logMonoTime.values())
 
     if sm.updated['carState']:
@@ -169,11 +169,10 @@ def update(self, sm, rr, enable_lead):
     radarState.radarErrors = list(rr.errors)
     radarState.carStateMonoTime = sm.logMonoTime['carState']
 
-    if enable_lead:
-      leads_v3 = sm['modelV2'].leadsV3
-      if len(leads_v3) > 1:
-        radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True)
-        radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False)
+    leads_v3 = sm['modelV2'].leadsV3
+    if len(leads_v3) > 1:
+      radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True)
+      radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False)
     return dat
 
 
@@ -203,9 +202,6 @@ def radard_thread(sm=None, pm=None, can_sock=None):
   rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
   RD = RadarD(CP.radarTimeStep, RI.delay)
 
-  # TODO: always log leads once we can hide them conditionally
-  enable_lead = CP.openpilotLongitudinalControl or not CP.radarOffCan
-
   while 1:
     can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
     rr = RI.update(can_strings)
@@ -215,7 +211,7 @@ def radard_thread(sm=None, pm=None, can_sock=None):
 
     sm.update(0)
 
-    dat = RD.update(sm, rr, enable_lead)
+    dat = RD.update(sm, rr)
     dat.radarState.cumLagMs = -rk.remaining*1000.
 
     pm.send('radarState', dat)

selfdrive/test/longitudinal_maneuvers/maneuver.py

@@ -16,6 +16,7 @@ def __init__(self, title, duration, **kwargs):
 
     self.only_lead2 = kwargs.get("only_lead2", False)
     self.only_radar = kwargs.get("only_radar", False)
+    self.ensure_start = kwargs.get("ensure_start", False)
 
     self.duration = duration
     self.title = title
@@ -52,5 +53,9 @@ def evaluate(self):
         print("Crashed!!!!")
         valid = False
 
+      if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1:
+        print('Planner not starting!')
+        valid = False
+
     print("maneuver end", valid)
     return valid, np.array(logs)

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -28,6 +28,7 @@ def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0,
     self.distance = 0.
     self.speed = speed
     self.acceleration = 0.0
+    self.speeds = []
 
     # lead car
     self.distance_lead = distance_lead
@@ -98,6 +99,7 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.):
     self.planner.update(sm)
     self.speed = self.planner.v_desired_filter.x
     self.acceleration = self.planner.a_desired
+    self.speeds = self.planner.v_desired_trajectory.tolist()
     fcw = self.planner.fcw
     self.distance_lead = self.distance_lead + v_lead * self.ts
 
@@ -129,6 +131,7 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.):
       "distance": self.distance,
       "speed": self.speed,
       "acceleration": self.acceleration,
+      "speeds": self.speeds,
       "distance_lead": self.distance_lead,
       "fcw": fcw,
     }

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -3,6 +3,7 @@
 import unittest
 
 from common.params import Params
+from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
 from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 
 
@@ -106,6 +107,17 @@
     breakpoints=[1., 1.01, 11.],
     cruise_values=[float("nan"), 15., 15.],
   ),
+  # controls relies on planner commanding to move for stock-ACC resume spamming
+  Maneuver(
+    "resume from a stop",
+    duration=20.,
+    initial_speed=0.,
+    lead_relevancy=True,
+    initial_distance_lead=STOP_DISTANCE,
+    speed_lead_values=[0., 0., 2.],
+    breakpoints=[1., 10., 15.],
+    ensure_start=True,
+  ),
 ]