diff --git a/cereal b/cereal index ecb9c5eb8554fa..03bc465ba1d739 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit ecb9c5eb8554fae8f40698df80eeadf3fc1131e7 +Subproject commit 03bc465ba1d7394838830e7818a2b295366e23fb diff --git a/opendbc b/opendbc index 2bab99fd861786..75c8dd4d0df665 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 2bab99fd861786312bde676823e21d80eeeb01fa +Subproject commit 75c8dd4d0df66546c568682bf54b90083add6138 diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 743546c5debd08..49e24d226f160f 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -8,7 +8,7 @@ from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from selfdrive.config import Conversions as CV -from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR +from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, SPORT_ECO_CAR class CarState(CarStateBase): @@ -87,6 +87,9 @@ def update(self, cp, cp_cam): can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + if self.CP.carFingerprint in SPORT_ECO_CAR: + ret.sportOn = bool(cp.vl["GEAR_PACKET"]["SPORT_ON"]) + ret.econOn = bool(cp.vl["GEAR_PACKET"]["ECON_ON"]) ret.leftBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1 ret.rightBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2 @@ -235,6 +238,12 @@ def get_can_parser(CP): signals.append(("FD_BUTTON", "SDSU", 0)) checks.append(("SDSU", 33)) + if CP.carFingerprint in SPORT_ECO_CAR: + signals += [ + ("SPORT_ON", "GEAR_PACKET", 0), + ("ECON_ON", "GEAR_PACKET", 0), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @staticmethod diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 8be6073003ec06..7c30e24ea6e884 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -16,6 +16,7 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed): + # as well as here (pid output is clipped to this) return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX @staticmethod diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 9361b10f74f420..b519d4f3346dcc 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -10,7 +10,7 @@ class CarControllerParams: - ACCEL_MAX = 1.6 # m/s2, lower than allowed 2.0 m/s2 for tuning reasons + ACCEL_MAX = 1.0 # m/s2, lower than allowed 2.0 m/s2 for tuning reasons ACCEL_MIN = -3.5 # m/s2 STEER_MAX = 1500 @@ -1644,6 +1644,7 @@ class CAR: CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2} NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH} +SPORT_ECO_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY} # CamryH for some reason has special dbc # no resume button press required NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH} diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1233baddacef5a..bbd2d551498a03 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -585,7 +585,7 @@ def state_control(self, CS): 'live_tracks': self.sm_smiskol['liveTracks'], 'has_lead': long_plan.hasLead} # accel PID loop - pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS) + pid_accel_limits = list(self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS)) actuators.accel = self.LoC.update(self.active, CS, self.CP, long_plan, pid_accel_limits, extras_loc) # interpolate lat plan to 100hz diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index b8eb4af3ce6f9c..712ed50c17d765 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -4,6 +4,7 @@ from selfdrive.controls.lib.pid import PIDController from selfdrive.controls.lib.drive_helpers import CONTROL_N from selfdrive.modeld.constants import T_IDXS +from selfdrive.controls.lib.dynamic_gas import DynamicGas from common.op_params import opParams LongCtrlState = car.CarControl.Actuators.LongControlState @@ -53,6 +54,9 @@ def __init__(self, CP): self.v_pid = 0.0 self.last_output_accel = 0.0 + # self.op_params = opParams() + # self.dynamic_gas = DynamicGas(CP, candidate) + def reset(self, v_pid): """Reset PID controller and change setpoint""" self.pid.reset() @@ -79,8 +83,18 @@ def update(self, active, CS, CP, long_plan, accel_limits, extras): a_target = clip(a_target, ACCEL_MIN_ISO, ACCEL_MAX_ISO) self.pid.neg_limit = accel_limits[0] + # if CS.sportOn: + # pass # already max accel from CarControllerParams + # elif CS.econOn: + # accel_limits[1] = 1.3 + # else: + # accel_limits[1] = 1.5 + accel_limits[1] = 1.5 self.pid.pos_limit = accel_limits[1] + # if self.op_params.get('dynamic_gas'): + # gas_max = self.dynamic_gas.update(CS, extras) + # 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, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 4af37a2307f199..29058e11cb754c 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,16 +16,29 @@ LON_MPC_STEP = 0.2 # first step is 0.2s AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted A_CRUISE_MIN = -1.2 -A_CRUISE_MAX_VALS = [1.5, 1.2, 0.8, 0.6] -A_CRUISE_MAX_BP = [0., 15., 25., 40.] +# TODO: tune from DATA! +A_CRUISE_MAX_VALS = [1.6, 1.5, 0.6, 0.4] +A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.] # 0., 14., 50.3, 90 mph + +A_CRUISE_MAX_VALS_SPORT = [1.0, 0.656] +A_CRUISE_MAX_BP_SPORT = [11.3876, 29.238] + +A_CRUISE_MAX_VALS_ECON = [1.0, 0.64, 0.5, 0.36] +A_CRUISE_MAX_BP_ECON = [3., 16.6332, 22.4933, 30.2597] # Lookup table for turns _A_TOTAL_MAX_V = [2.5, 3.8] _A_TOTAL_MAX_BP = [15., 40.] -def get_max_accel(v_ego): - return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) +def get_max_accel(v_ego, CS): + # change A_CRUISE_MAX_VALS live + if CS.sportOn: + return interp(v_ego, A_CRUISE_MAX_BP_SPORT, A_CRUISE_MAX_VALS_SPORT) + elif CS.econOn: + return interp(v_ego, A_CRUISE_MAX_BP_ECON, A_CRUISE_MAX_VALS_ECON) + else: + return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): @@ -78,7 +91,7 @@ def update(self, sm): self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * v_ego self.v_desired = max(0.0, self.v_desired) - accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] + accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego, sm['carState'])] accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration