update for prius alpha

.gitmodules

@@ -1,21 +1,21 @@
 [submodule "panda"]
   path = panda
-  url = ../../commaai/panda.git
+  url = ../../Edison-CBS/panda.git
 [submodule "opendbc"]
   path = opendbc
-  url = ../../commaai/opendbc.git
+  url = ../../Edison-CBS/opendbc.git
 [submodule "laika_repo"]
   path = laika_repo
-  url = ../../commaai/laika.git
+  url = ../../Edison-CBS/laika.git
 [submodule "cereal"]
   path = cereal
-  url = ../../commaai/cereal.git
+  url = ../../Edison-CBS/cereal.git
 [submodule "rednose_repo"]
   path = rednose_repo
-  url = ../../commaai/rednose.git
+  url = ../../Edison-CBS/rednose.git
 [submodule "body"]
   path = body
-  url = ../../commaai/body.git
+  url = ../../Edison-CBS/body.git
 [submodule "tinygrad"]
   path = tinygrad_repo
-  url = https://github.com/geohot/tinygrad.git
+  url = ../../Edison-CBS/tinygrad.git

common/params.cc

@@ -99,6 +99,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CarParamsCache", CLEAR_ON_MANAGER_START},
     {"CarParamsPersistent", PERSISTENT},
     {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
+    {"Compass", PERSISTENT},
     {"CompletedTrainingVersion", PERSISTENT},
     {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"CurrentBootlog", PERSISTENT},
@@ -112,6 +113,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"DoReboot", CLEAR_ON_MANAGER_START},
     {"DoShutdown", CLEAR_ON_MANAGER_START},
     {"DoUninstall", CLEAR_ON_MANAGER_START},
+    {"DrivingPersonalitiesUIWheel", PERSISTENT},
     {"ExperimentalLongitudinalEnabled", PERSISTENT},
     {"ExperimentalMode", PERSISTENT},
     {"ExperimentalModeConfirmed", PERSISTENT},
@@ -206,6 +208,12 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"Version", PERSISTENT},
     {"VisionRadarToggle", PERSISTENT},
     {"WheeledBody", PERSISTENT},
+    // edison params
+    {"CarBrightnessControl", PERSISTENT},
+    {"LQR", PERSISTENT},
+    {"DisableOpenpilotSounds", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
+    {"ScreenOffTimer", PERSISTENT},
+    {"CruiseSpeedRewrite", PERSISTENT},
 };
 
 } // namespace

common/params.h

@@ -32,6 +32,10 @@ class Params {
   inline bool getBool(const std::string &key, bool block = false) {
     return get(key, block) == "1";
   }
+  inline int getInt(const std::string &key, bool block = false) {
+    std::string value = get(key, block);
+    return value.empty() ? 0 : std::stoi(value);
+  }
   std::map<std::string, std::string> readAll();
 
   // helpers for writing values
@@ -42,6 +46,9 @@ class Params {
   inline int putBool(const std::string &key, bool val) {
     return put(key.c_str(), val ? "1" : "0", 1);
   }
+  inline int putInt(const std::string &key, int val) {
+    return put(key.c_str(), std::to_string(val).c_str(), std::to_string(val).size());
+  }
 
 private:
   std::string params_path;

common/params.py

@@ -1,9 +1,10 @@
-from common.params_pyx import Params, ParamKeyType, UnknownKeyName, put_nonblocking, put_bool_nonblocking # pylint: disable=no-name-in-module, import-error
+from common.params_pyx import Params, ParamKeyType, UnknownKeyName, put_nonblocking, put_bool_nonblocking, put_int_nonblocking # pylint: disable=no-name-in-module, import-error
 assert Params
 assert ParamKeyType
 assert UnknownKeyName
 assert put_nonblocking
 assert put_bool_nonblocking
+assert put_int_nonblocking
 
 if __name__ == "__main__":
   import sys

common/params_pyx.pyx

@@ -17,9 +17,11 @@ cdef extern from "common/params.h":
     c_Params(string) nogil
     string get(string, bool) nogil
     bool getBool(string, bool) nogil
+    int getInt(string, bool) nogil
     int remove(string) nogil
     int put(string, string) nogil
     int putBool(string, bool) nogil
+    int putInt(string, int) nogil
     bool checkKey(string) nogil
     string getParamPath(string) nogil
     void clearAll(ParamKeyType)
@@ -75,6 +77,13 @@ cdef class Params:
       r = self.p.getBool(k, block)
     return r
 
+  def get_int(self, key, bool block=False):
+    cdef string k = self.check_key(key)
+    cdef int r
+    with nogil:
+      r = self.p.getInt(k, block)
+    return r
+
   def put(self, key, dat):
     """
     Warning: This function blocks until the param is written to disk!
@@ -92,6 +101,11 @@ cdef class Params:
     with nogil:
       self.p.putBool(k, val)
 
+  def put_int(self, key, int val):
+    cdef string k = self.check_key(key)
+    with nogil:
+      self.p.putInt(k, val)
+
   def remove(self, key):
     cdef string k = self.check_key(key)
     with nogil:
@@ -109,3 +123,6 @@ def put_nonblocking(key, val, d=""):
 
 def put_bool_nonblocking(key, bool val, d=""):
   threading.Thread(target=lambda: Params(d).put_bool(key, val)).start()
+
+def put_int_nonblocking(key, int val, d=""):
+  threading.Thread(target=lambda: Params(d).put_int(key, val)).start()

release/files_common

@@ -182,6 +182,7 @@ selfdrive/controls/lib/desire_helper.py
 selfdrive/controls/lib/drive_helpers.py
 selfdrive/controls/lib/events.py
 selfdrive/controls/lib/latcontrol_angle.py
+selfdrive/controls/lib/latcontrol_lqr.py
 selfdrive/controls/lib/latcontrol_torque.py
 selfdrive/controls/lib/latcontrol_pid.py
 selfdrive/controls/lib/latcontrol.py

selfdrive/car/tests/test_car_interfaces.py

@@ -73,6 +73,9 @@ def test_car_interfaces(self, car_name, data):
         self.assertTrue(len(tune.pid.kpV) > 0 and len(tune.pid.kpV) == len(tune.pid.kpBP))
         self.assertTrue(len(tune.pid.kiV) > 0 and len(tune.pid.kiV) == len(tune.pid.kiBP))
 
+      elif tune.which() == 'lqr':
+        self.assertTrue(len(tune.lqr.a))
+
       elif tune.which() == 'torque':
         self.assertTrue(not math.isnan(tune.torque.kf) and tune.torque.kf > 0)
         self.assertTrue(not math.isnan(tune.torque.friction) and tune.torque.friction > 0)

selfdrive/car/tests/test_models.py

@@ -134,6 +134,8 @@ def test_car_params(self):
       tuning = self.CP.lateralTuning.which()
       if tuning == 'pid':
         self.assertTrue(len(self.CP.lateralTuning.pid.kpV))
+      elif tuning == 'lqr':
+        self.assertTrue(len(self.CP.lateralTuning.lqr.a))
       elif tuning == 'torque':
         self.assertTrue(self.CP.lateralTuning.torque.kf > 0)
       elif tuning == 'indi':

selfdrive/car/torque_data/params.yaml

@@ -77,6 +77,7 @@ TOYOTA HIGHLANDER HYBRID 2018: [1.752033, 1.6433903296845025, 0.144600]
 TOYOTA HIGHLANDER HYBRID 2020: [1.901174, 2.104015182965606, 0.14447040132184993]
 TOYOTA MIRAI 2021: [2.506899832157829, 1.7417213930750164, 0.20182618449440565]
 TOYOTA PRIUS 2017: [1.60, 1.5023147650693636, 0.151515]
+TOYOTA PRIUS v 2017 : [2.3, 1.9, 0.038]
 TOYOTA PRIUS TSS2 2021: [1.972600, 1.9104337425537743, 0.170968]
 TOYOTA RAV4 2017: [2.085695074355425, 2.2142832316984733, 0.13339165270103975]
 TOYOTA RAV4 2019: [2.331293, 2.0993589721530252, 0.129822]

selfdrive/car/torque_data/substitute.yaml

@@ -6,7 +6,6 @@ MAZDA CX-9: MAZDA CX-9 2021
 
 TOYOTA ALPHARD HYBRID 2021 : TOYOTA SIENNA 2018
 TOYOTA ALPHARD 2020: TOYOTA SIENNA 2018
-TOYOTA PRIUS v 2017 : TOYOTA PRIUS 2017
 TOYOTA RAV4 2022: TOYOTA RAV4 HYBRID 2022
 TOYOTA C-HR HYBRID 2018: TOYOTA C-HR 2018
 TOYOTA C-HR HYBRID 2022: TOYOTA C-HR 2021

selfdrive/car/toyota/carcontroller.py

@@ -132,18 +132,24 @@ def update(self, CC, CS, now_nanos):
 
     self.last_standstill = CS.out.standstill
 
+    # handle UI messages
+    fcw_alert = hud_control.visualAlert == VisualAlert.fcw
+    fcw_alert_acc = (hud_control.visualAlert == VisualAlert.fcw) and not self.CP.enableDsu
+    steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
+
     # we can spam can to cancel the system even if we are using lat only control
     if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
       lead = hud_control.leadVisible or CS.out.vEgo < 12.  # at low speed we always assume the lead is present so ACC can be engaged
+      adjust_distance = CS.distance_btn == 1
 
       # Lexus IS uses a different cancellation message
       if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert_acc))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
@@ -156,9 +162,6 @@ def update(self, CC, CS, now_nanos):
       # ui mesg is at 1Hz but we send asap if:
       # - there is something to display
       # - there is something to stop displaying
-      fcw_alert = hud_control.visualAlert == VisualAlert.fcw
-      steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
-
       send_ui = False
       if ((fcw_alert or steer_alert) and not self.alert_active) or \
          (not (fcw_alert or steer_alert) and self.alert_active):

selfdrive/car/toyota/carstate.py

@@ -4,6 +4,7 @@
 from common.conversions import Conversions as CV
 from common.numpy_fast import mean
 from common.filter_simple import FirstOrderFilter
+from common.params import Params, put_int_nonblocking
 from common.realtime import DT_CTRL
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
@@ -43,6 +44,14 @@ def __init__(self, CP):
     self.acc_type = 1
     self.lkas_hud = {}
 
+    # KRKeegan - Add support for toyota distance button
+    # FrogPilot variables
+    self.params = Params()
+    self.driving_personalities_via_wheel = self.params.get_bool("DrivingPersonalitiesUIWheel")
+    self.distance_btn = 0
+    self.distance_lines = 0
+    self.previous_distance_lines = 0
+
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
 
@@ -106,6 +115,11 @@ def update(self, cp, cp_cam):
     ret.steerFaultTemporary = cp.vl["EPS_STATUS"]["LKA_STATE"] in TEMP_STEER_FAULTS
     ret.steerFaultPermanent = cp.vl["EPS_STATUS"]["LKA_STATE"] in PERM_STEER_FAULTS
 
+    # copy from cydia2020's stuff
+    ret.lightingSystem.parkingLightON = cp.vl["LIGHT_STALK"]['PARKING_LIGHT'] == 1
+    ret.lightingSystem.headlightON = cp.vl["LIGHT_STALK"]['LOW_BEAM'] == 1
+    ret.lightingSystem.meterDimmed = cp.vl["BODY_CONTROL_STATE"]['METER_DIMMED'] == 1
+    ret.lightingSystem.meterLowBrightness = cp.vl["BODY_CONTROL_STATE_2"]["METER_SLIDER_LOW_BRIGHTNESS"] == 1
     if self.CP.steerControlType == SteerControlType.angle:
       ret.steerFaultTemporary = ret.steerFaultTemporary or cp.vl["EPS_STATUS"]["LTA_STATE"] in TEMP_STEER_FAULTS
       ret.steerFaultPermanent = ret.steerFaultPermanent or cp.vl["EPS_STATUS"]["LTA_STATE"] in PERM_STEER_FAULTS
@@ -162,6 +176,24 @@ def update(self, cp, cp_cam):
     if self.CP.carFingerprint != CAR.PRIUS_V:
       self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
 
+    # Driving personalities function
+    if self.driving_personalities_via_wheel:
+      # KRKeegan - Add support for toyota distance button
+      # order must be: RADAR_ACC_CAR --> TSS2_CAR --> smartDsu
+      # cp_acc dynamic according to car carFingerprint
+      if self.CP.carFingerprint in (TSS2_CAR | RADAR_ACC_CAR):
+        if not (self.CP.flags & ToyotaFlags.SMART_DSU.value):
+          self.distance_btn = 1 if cp_acc.vl["ACC_CONTROL"]["DISTANCE"] == 1 else 0
+          # Need to subtract by 1 to comply with the personality profiles of "0", "1", and "2"
+          self.distance_lines = max(cp.vl["PCM_CRUISE_SM"]["DISTANCE_LINES"] - 1, 0) 
+      elif bool(self.CP.flags & ToyotaFlags.SMART_DSU):
+        self.distance_btn = 1 if cp_acc.vl["SDSU"]["FD_BUTTON"] == 1 else 0
+        self.distance_lines = max(cp.vl["PCM_CRUISE_SM"]["DISTANCE_LINES"] - 1, 0)
+
+      if self.distance_lines != self.previous_distance_lines:  
+        put_int_nonblocking('LongitudinalPersonality', self.distance_lines)
+        self.previous_distance_lines = self.distance_lines
+
     return ret
 
   @staticmethod
@@ -197,6 +229,11 @@ def get_can_parser(CP):
       ("TURN_SIGNALS", "BLINKERS_STATE"),
       ("LKA_STATE", "EPS_STATUS"),
       ("AUTO_HIGH_BEAM", "LIGHT_STALK"),
+      # copy from cydia2020's
+      ("PARKING_LIGHT", "LIGHT_STALK"),
+      ("LOW_BEAM", "LIGHT_STALK"),
+      ("METER_DIMMED", "BODY_CONTROL_STATE"),
+      ("METER_SLIDER_LOW_BRIGHTNESS", "BODY_CONTROL_STATE_2"),
     ]
 
     # Check LTA state if using LTA angle control
@@ -254,6 +291,11 @@ def get_can_parser(CP):
       ]
       checks.append(("BSM", 1))
 
+    # KRKeegan - Add support for toyota distance button
+    if bool(CP.flags & ToyotaFlags.SMART_DSU):
+      signals.append(("FD_BUTTON", "SDSU"))
+      checks.append(("SDSU", 0))
+
     if CP.carFingerprint in RADAR_ACC_CAR:
       if not CP.flags & ToyotaFlags.SMART_DSU.value:
         signals += [
@@ -263,9 +305,11 @@ def get_can_parser(CP):
           ("ACC_CONTROL", 33),
         ]
       signals += [
+        ("DISTANCE", 'ACC_CONTROL'),
         ("FCW", "ACC_HUD"),
       ]
       checks += [
+        ("ACC_CONTROL", 0),
         ("ACC_HUD", 1),
       ]
 
@@ -278,6 +322,10 @@ def get_can_parser(CP):
         ("PRE_COLLISION", 33),
       ]
 
+    if CP.carFingerprint in (TSS2_CAR | RADAR_ACC_CAR | NO_STOP_TIMER_CAR):
+      signals.append(("DISTANCE_LINES", "PCM_CRUISE_SM"))
+      checks.append(("PCM_CRUISE_SM", 0))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
 
   @staticmethod
@@ -302,6 +350,7 @@ def get_cam_can_parser(CP):
         ("PRECOLLISION_ACTIVE", "PRE_COLLISION"),
         ("FORCE", "PRE_COLLISION"),
         ("ACC_TYPE", "ACC_CONTROL"),
+        ("DISTANCE", 'ACC_CONTROL'),
         ("FCW", "ACC_HUD"),
       ]
       checks += [

selfdrive/car/toyota/interface.py

@@ -1,7 +1,9 @@
 #!/usr/bin/env python3
 from cereal import car
 from common.conversions import Conversions as CV
+from common.params import Params
 from panda import Panda
+from selfdrive.car.toyota.tunes import LatTunes, set_lat_tune
 from selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
                                         MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
 from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config
@@ -12,6 +14,11 @@
 
 
 class CarInterface(CarInterfaceBase):
+  def __init__(self, CP, CarController, CarState):
+    super().__init__(CP, CarController, CarState)
+
+    self.override_speed = 0.
+
   @staticmethod
   def get_pid_accel_limits(CP, current_speed, cruise_speed):
     return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
@@ -59,9 +66,13 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
     elif candidate == CAR.PRIUS_V:
       stop_and_go = True
       ret.wheelbase = 2.78
-      ret.steerRatio = 17.4
+      ret.steerRatio = 16.8
       tire_stiffness_factor = 0.5533
       ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG
+      if Params().get_bool("LQR"):
+        set_lat_tune(ret.lateralTuning, LatTunes.LQR_PV)
+      else:
+        CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
 
     elif candidate in (CAR.RAV4, CAR.RAV4H):
       stop_and_go = True if (candidate in CAR.RAV4H) else False
@@ -273,6 +284,20 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
   # returns a car.CarState
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
+    params = Params()
+
+    # low speed re-write
+    if ret.cruiseState.enabled and params.get_bool("CruiseSpeedRewrite") and \
+      self.CP.openpilotLongitudinalControl and ret.cruiseState.speed < 45. * CV.KPH_TO_MS:
+      if params.get_bool("CruiseSpeedRewrite"):
+        if self.override_speed == 0.:
+          ret.cruiseState.speed = ret.cruiseState.speedCluster = self.override_speed = max(24. * CV.KPH_TO_MS, ret.vEgo)
+        else:
+          ret.cruiseState.speed = ret.cruiseState.speedCluster = self.override_speed
+      else:
+        ret.cruiseState.speed = ret.cruiseState.speedCluster = 24. * CV.KPH_TO_MS
+    else:
+      self.override_speed = 0.
 
     # events
     events = self.create_common_events(ret)