From f13a3cea0bcee09b34f2d0f5be4841fb3a18c810 Mon Sep 17 00:00:00 2001 From: ntegan Date: Wed, 22 Feb 2023 17:50:09 -0500 Subject: [PATCH 1/4] Revert "remove tune" This reverts commit b3f9ad095aac1791bf139b0311f25a88e554230d. --- selfdrive/car/toyota/carcontroller.py | 4 +++- selfdrive/car/toyota/interface.py | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index f004ae20b2e294..51b730c9925911 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -60,13 +60,15 @@ def update(self, CC, CS, now_nanos): # gas and brake a = actuators.accel if manual_accel is None else manual_accel actuators.accel = a # need this??? to show up in logs?? + MPH_TO_MS = .447 + mtm = MPH_TO_MS if self.CP.enableGasInterceptor and CC.longActive: MAX_INTERCEPTOR_GAS = 0.5 # RAV4 has very sensitive gas pedal if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH): PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0]) elif self.CP.carFingerprint in (CAR.COROLLA,): - PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0]) + PEDAL_SCALE = interp(CS.out.vEgo, [0.0, 4.*mtm, 8.*mtm, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [.12, .19, 0.3, 0.4, 0.0]) else: PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0]) # offset for creep and windbrake diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 8b3fd048d996e9..b07795c41d039d 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -224,6 +224,8 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): tune.kpV = [1.3, 1.0, 0.7] tune.kiBP = [0., 5., 12., 20., 27.] tune.kiV = [.35, .23, .20, .17, .1] + ret.stoppingDecelRate = 0.4 # reach stopping target smoothly + ret.stopAccel = -.7 if candidate in TSS2_CAR: ret.vEgoStopping = 0.25 ret.vEgoStarting = 0.25 From 4598dc5b94942e6e8c7d0b6cb84b646268f72d60 Mon Sep 17 00:00:00 2001 From: ntegan Date: Wed, 22 Feb 2023 18:10:11 -0500 Subject: [PATCH 2/4] initial maneuvering --- selfdrive/car/toyota/carcontroller.py | 2 +- selfdrive/controls/controlsd.py | 19 +++++++++++++++++++ 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 51b730c9925911..c37237ceff9d20 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -55,7 +55,7 @@ def update(self, CC, CS, now_nanos): if self.gac_pressed_frame is not None: # button is held #gac_pressed_duration = (self.frame - self.gac_pressed_frame) * DT_CTRL - manual_accel = -1. + manual_accel = None # gas and brake a = actuators.accel if manual_accel is None else manual_accel diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index bc541548cde89b..d0ece1638a6492 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -3,6 +3,7 @@ import math from typing import SupportsFloat +from selfdrive.controls.lib.drive_helpers import CONTROL_N from cereal import car, log from common.numpy_fast import clip from common.realtime import sec_since_boot, config_realtime_process, Priority, Ratekeeper, DT_CTRL @@ -60,6 +61,8 @@ class Controls: def __init__(self, sm=None, pm=None, can_sock=None, CI=None): + self.maneuvering = False + self.maneuver_begin_frame = None config_realtime_process(4, Priority.CTRL_HIGH) # Ensure the current branch is cached, otherwise the first iteration of controlsd lags @@ -571,6 +574,22 @@ def state_control(self, CS): lat_plan = self.sm['lateralPlan'] long_plan = self.sm['longitudinalPlan'] + for b in CS.buttonEvents: + if b.type == car.CarState.ButtonEvent.Type.gapAdjustCruise: + if b.pressed is True: + self.LoC.reset(v_pid=CS.vEgo) + self.maneuvering = True + self.maneuver_begin_frame = self.sm.frame + elif b.pressed is False: + self.LoC.reset(v_pid=CS.vEgo) + self.maneuvering = False + self.maneuver_begin_frame = None + + if self.maneuvering: + long_plan.speeds = [5. * CV.MPH_TO_MS] * CONTROL_N + long_plan.accels = [-1.] * CONTROL_N + long_plan.jerks = [0.] * CONTROL_N + CC = car.CarControl.new_message() CC.enabled = self.enabled From 4fb855b0cf840ef2fb1fe2f213c1f4a92d69da92 Mon Sep 17 00:00:00 2001 From: ntegan Date: Wed, 22 Feb 2023 18:18:27 -0500 Subject: [PATCH 3/4] do it in longplanner so long_plan logged is the maneuver --- selfdrive/controls/controlsd.py | 5 ----- selfdrive/controls/lib/longitudinal_planner.py | 16 ++++++++++++++++ 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index d0ece1638a6492..701fc997f096d3 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -585,11 +585,6 @@ def state_control(self, CS): self.maneuvering = False self.maneuver_begin_frame = None - if self.maneuvering: - long_plan.speeds = [5. * CV.MPH_TO_MS] * CONTROL_N - long_plan.accels = [-1.] * CONTROL_N - long_plan.jerks = [0.] * CONTROL_N - CC = car.CarControl.new_message() CC.enabled = self.enabled diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 2ef9051122399b..601bc0e3891203 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -3,6 +3,7 @@ import numpy as np from common.numpy_fast import clip, interp +from cereal import car import cereal.messaging as messaging from common.conversions import Conversions as CV from common.filter_simple import FirstOrderFilter @@ -45,6 +46,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0): + self.maneuvering = False self.CP = CP self.mpc = LongitudinalMpc() self.fcw = False @@ -76,6 +78,13 @@ def parse_model(model_msg, model_error): def update(self, sm): self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' + be = sm['carState'].buttonEvents + for b in be: + if b.type == car.CarState.ButtonEvent.Type.gapAdjustCruise: + if b.pressed is True: + self.maneuvering = True + elif b.pressed is False: + self.maneuvering = False v_ego = sm['carState'].vEgo v_cruise_kph = sm['controlsState'].vCruise @@ -153,6 +162,13 @@ def publish(self, sm, pm): longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw + if self.maneuvering: + longitudinalPlan.speeds = [5. * CV.MPH_TO_MS] * CONTROL_N + longitudinalPlan.accels = [-1.] * CONTROL_N + longitudinalPlan.jerks = [0.] * CONTROL_N + longitudinalPlan.hasLead = True + + longitudinalPlan.solverExecutionTime = self.mpc.solve_time pm.send('longitudinalPlan', plan_send) From 9043f33ac81c940ad04f551ab7af085922fb3f09 Mon Sep 17 00:00:00 2001 From: ntegan Date: Wed, 22 Feb 2023 18:18:31 -0500 Subject: [PATCH 4/4] Revert "do it in longplanner so long_plan logged is the maneuver" This reverts commit 4fb855b0cf840ef2fb1fe2f213c1f4a92d69da92. --- selfdrive/controls/controlsd.py | 5 +++++ selfdrive/controls/lib/longitudinal_planner.py | 16 ---------------- 2 files changed, 5 insertions(+), 16 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 701fc997f096d3..d0ece1638a6492 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -585,6 +585,11 @@ def state_control(self, CS): self.maneuvering = False self.maneuver_begin_frame = None + if self.maneuvering: + long_plan.speeds = [5. * CV.MPH_TO_MS] * CONTROL_N + long_plan.accels = [-1.] * CONTROL_N + long_plan.jerks = [0.] * CONTROL_N + CC = car.CarControl.new_message() CC.enabled = self.enabled diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 601bc0e3891203..2ef9051122399b 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -3,7 +3,6 @@ import numpy as np from common.numpy_fast import clip, interp -from cereal import car import cereal.messaging as messaging from common.conversions import Conversions as CV from common.filter_simple import FirstOrderFilter @@ -46,7 +45,6 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0): - self.maneuvering = False self.CP = CP self.mpc = LongitudinalMpc() self.fcw = False @@ -78,13 +76,6 @@ def parse_model(model_msg, model_error): def update(self, sm): self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' - be = sm['carState'].buttonEvents - for b in be: - if b.type == car.CarState.ButtonEvent.Type.gapAdjustCruise: - if b.pressed is True: - self.maneuvering = True - elif b.pressed is False: - self.maneuvering = False v_ego = sm['carState'].vEgo v_cruise_kph = sm['controlsState'].vCruise @@ -162,13 +153,6 @@ def publish(self, sm, pm): longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw - if self.maneuvering: - longitudinalPlan.speeds = [5. * CV.MPH_TO_MS] * CONTROL_N - longitudinalPlan.accels = [-1.] * CONTROL_N - longitudinalPlan.jerks = [0.] * CONTROL_N - longitudinalPlan.hasLead = True - - longitudinalPlan.solverExecutionTime = self.mpc.solve_time pm.send('longitudinalPlan', plan_send)