From cfc59917fc052e822de00e2d753fb1f8812f238e Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 6 Feb 2022 09:32:26 -0800 Subject: [PATCH 1/5] use future plan on engagement --- selfdrive/controls/controlsd.py | 5 +++-- selfdrive/controls/lib/longcontrol.py | 19 ++++++++++++++----- .../controls/lib/longitudinal_planner.py | 4 ++-- 3 files changed, 19 insertions(+), 9 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 788f3fe26a5b8d..0d9ccadf1bd208 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -484,7 +484,7 @@ def state_transition(self, CS): # if stock cruise is completely disabled, then we can use our own set speed logic if not self.CP.pcmCruise: self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.buttonEvents, self.button_timers, self.enabled, self.is_metric) - elif CS.cruiseState.enabled: + else: self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH # decrement the soft disable timer at every step, as it's reset on @@ -543,7 +543,8 @@ def state_transition(self, CS): else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last) + if not self.CP.pcmCruise: + self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last) # Check if actuators are enabled self.active = self.state == State.enabled or self.state == State.softDisabling diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index e767c9b508f949..721b1e7b3dba2a 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -52,11 +52,13 @@ def __init__(self, CP): derivative_period=0.5) self.v_pid = 0.0 self.last_output_accel = 0.0 + self.active_frames = 100 def reset(self, v_pid): """Reset PID controller and change setpoint""" self.pid.reset() self.v_pid = v_pid + self.active_frames = 100 def update(self, active, CS, CP, long_plan, accel_limits, extras): """Update longitudinal control. This updates the state machine and runs a PID loop""" @@ -64,12 +66,19 @@ def update(self, active, CS, CP, long_plan, accel_limits, extras): # TODO estimate car specific lag, use .15s for now speeds = long_plan.speeds if len(speeds) == CONTROL_N: - v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound, T_IDXS[:CONTROL_N], speeds) - a_target_lower = 2 * (v_target_lower - speeds[0])/CP.longitudinalActuatorDelayLowerBound - long_plan.accels[0] + if self.long_control_state == LongCtrlState.off or CS.gasPressed: + self.active_frames = 100 + elif self.long_control_state == LongCtrlState.pid: + self.active_frames = max(self.active_frames - 1, 0) - v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], speeds) - a_target_upper = 2 * (v_target_upper - speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0] - a_target = min(a_target_lower, a_target_upper) + actuator_delay = interp(self.active_frames, [100, 50, 0], [1.0, 1.0, CP.longitudinalActuatorDelayLowerBound]) + + v_target = interp(actuator_delay, T_IDXS[:CONTROL_N], speeds) + a_target = 2 * (v_target - speeds[0])/actuator_delay - long_plan.accels[0] + + # v_target_upper = interp(actuator_delay, T_IDXS[:CONTROL_N], speeds) + # a_target_upper = 2 * (v_target_upper - speeds[0])/actuator_delay - long_plan.accels[0] + # a_target = min(a_target_lower, a_target_upper) v_target = speeds[0] v_target_future = speeds[-1] diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 262777fee2afed..007a0918d2f341 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -69,8 +69,8 @@ def update(self, sm): prev_accel_constraint = True if long_control_state == LongCtrlState.off or sm['carState'].gasPressed: - self.v_desired_filter.x = v_ego - self.a_desired = a_ego + self.v_desired_filter.x = 0. + self.a_desired = 0. # Smoothly changing between accel trajectory is only relevant when OP is driving prev_accel_constraint = False From ebcb8a6a04e7f5504fc06a2f1a00a9dc128a051b Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 6 Feb 2022 09:36:21 -0800 Subject: [PATCH 2/5] flip around so we reset when not in pid --- selfdrive/controls/lib/longcontrol.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 721b1e7b3dba2a..22160be340d938 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -66,10 +66,11 @@ def update(self, active, CS, CP, long_plan, accel_limits, extras): # TODO estimate car specific lag, use .15s for now speeds = long_plan.speeds if len(speeds) == CONTROL_N: - if self.long_control_state == LongCtrlState.off or CS.gasPressed: - self.active_frames = 100 - elif self.long_control_state == LongCtrlState.pid: + if self.long_control_state == LongCtrlState.pid: self.active_frames = max(self.active_frames - 1, 0) + else: + # reset when gas pressed, not active, in other longctrlstates, etc. + self.active_frames = 100 actuator_delay = interp(self.active_frames, [100, 50, 0], [1.0, 1.0, CP.longitudinalActuatorDelayLowerBound]) From c2bed981a5d3140f6e19d5a25427d9b850d65f99 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 6 Feb 2022 09:38:58 -0800 Subject: [PATCH 3/5] messy for now but make it correct --- selfdrive/controls/lib/longcontrol.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 22160be340d938..145cdc76e52682 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -65,6 +65,16 @@ def update(self, active, CS, CP, long_plan, accel_limits, extras): # Interp control trajectory # TODO estimate car specific lag, use .15s for now speeds = long_plan.speeds + v_target_future = 0. + if len(speeds) == CONTROL_N: + v_target_future = speeds[-1] + + # Update state machine + output_accel = self.last_output_accel + self.long_control_state = long_control_state_trans(CP, active, self.long_control_state, CS.vEgo, + v_target_future, CS.brakePressed, + CS.cruiseState.standstill) + if len(speeds) == CONTROL_N: if self.long_control_state == LongCtrlState.pid: self.active_frames = max(self.active_frames - 1, 0) @@ -82,10 +92,8 @@ def update(self, active, CS, CP, long_plan, accel_limits, extras): # a_target = min(a_target_lower, a_target_upper) v_target = speeds[0] - v_target_future = speeds[-1] else: v_target = 0.0 - v_target_future = 0.0 a_target = 0.0 # TODO: This check is not complete and needs to be enforced by MPC @@ -94,12 +102,6 @@ def update(self, active, CS, CP, long_plan, accel_limits, extras): self.pid.neg_limit = accel_limits[0] self.pid.pos_limit = accel_limits[1] - # Update state machine - output_accel = self.last_output_accel - self.long_control_state = long_control_state_trans(CP, active, self.long_control_state, CS.vEgo, - v_target_future, CS.brakePressed, - CS.cruiseState.standstill) - if self.long_control_state == LongCtrlState.off or CS.gasPressed: self.reset(CS.vEgo) output_accel = 0. From 5f082110d8226fa3cd49730144333ca108880548 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 6 Feb 2022 14:14:37 -0600 Subject: [PATCH 4/5] dummy move --- selfdrive/controls/lib/longitudinal_planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 007a0918d2f341..668cda9f2db533 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -69,7 +69,7 @@ def update(self, sm): prev_accel_constraint = True if long_control_state == LongCtrlState.off or sm['carState'].gasPressed: - self.v_desired_filter.x = 0. + self.v_desired_filter.x = v_ego self.a_desired = 0. # Smoothly changing between accel trajectory is only relevant when OP is driving prev_accel_constraint = False From 18bc943d34cdf875de93c6250d50a1641b218e50 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 6 Feb 2022 14:16:12 -0600 Subject: [PATCH 5/5] better tuning? --- selfdrive/controls/lib/longcontrol.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 145cdc76e52682..2c11a3815f5182 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -52,13 +52,13 @@ def __init__(self, CP): derivative_period=0.5) self.v_pid = 0.0 self.last_output_accel = 0.0 - self.active_frames = 100 + self.active_frames = 50 def reset(self, v_pid): """Reset PID controller and change setpoint""" self.pid.reset() self.v_pid = v_pid - self.active_frames = 100 + self.active_frames = 50 def update(self, active, CS, CP, long_plan, accel_limits, extras): """Update longitudinal control. This updates the state machine and runs a PID loop""" @@ -80,9 +80,9 @@ def update(self, active, CS, CP, long_plan, accel_limits, extras): self.active_frames = max(self.active_frames - 1, 0) else: # reset when gas pressed, not active, in other longctrlstates, etc. - self.active_frames = 100 + self.active_frames = 50 - actuator_delay = interp(self.active_frames, [100, 50, 0], [1.0, 1.0, CP.longitudinalActuatorDelayLowerBound]) + actuator_delay = interp(self.active_frames, [50, 25, 0], [1.0, 1.0, CP.longitudinalActuatorDelayLowerBound]) v_target = interp(actuator_delay, T_IDXS[:CONTROL_N], speeds) a_target = 2 * (v_target - speeds[0])/actuator_delay - long_plan.accels[0]