Toyota: add eco, normal, sport profiles

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

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

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}

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

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,

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