diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 14049c9997..d47caaa9ad 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -195,7 +195,7 @@ class CarController: # 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: diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 4fbb5ce0e1..f3066bda03 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -78,7 +78,7 @@ class CarController: 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 @@ class CarController: 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 diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 0e43a11ceb..a83cef508a 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -29,13 +29,6 @@ class CarController: CS.out.steeringTorque, CarControllerParams) self.steer_rate_limited = new_steer != apply_steer - if CC.enabled: - if CS.out.standstill and self.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 CC.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 @@ -48,6 +41,10 @@ class CarController: can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL)) else: self.brake_counter = 0 + if CC.cruiseControl.resume and self.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 when planner wants car to move + can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME)) self.apply_steer_last = apply_steer diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 4614463c6e..1643fbe9b6 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -96,9 +96,8 @@ class CarController: # Cancel ACC if it's engaged with OP disengaged. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["cancel"] = True - elif CC.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 CC.cruiseControl.resume: + # Send Resume button when planner wants car to move self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["resumeCruise"] = True diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9e3af9eb63..6f0c9c2ae6 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -658,6 +658,10 @@ class Controls: if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]: CC.cruiseControl.cancel = True + speeds = self.sm['longitudinalPlan'].speeds + if len(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) hudControl.speedVisible = self.enabled diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 9b4d016430..0d605a5fc7 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -16,6 +16,7 @@ class Maneuver(): 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 @@ class Maneuver(): 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) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 13025a9f03..3bd50ebcfa 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -28,6 +28,7 @@ class Plant(): self.distance = 0. self.speed = speed self.acceleration = 0.0 + self.speeds = [] # lead car self.distance_lead = distance_lead @@ -98,6 +99,7 @@ class Plant(): 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 @@ class Plant(): "distance": self.distance, "speed": self.speed, "acceleration": self.acceleration, + "speeds": self.speeds, "distance_lead": self.distance_lead, "fcw": fcw, } diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 698877dd3a..ec698d88fa 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -3,6 +3,7 @@ import os 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 @@ maneuvers = [ 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, + ), ]