update for prius v

.github/workflows/selfdrive_tests.yaml

@@ -2,8 +2,8 @@ name: selfdrive
 
 on:
   push:
-    branches:
-      - master
+    branches-ignore:
+      - 'testing-closet*'
   pull_request:
   workflow_dispatch:
 
@@ -142,7 +142,7 @@ jobs:
     - uses: ./.github/workflows/setup-with-retry
     - name: pre-commit
       timeout-minutes: 4
-      run: ${{ env.RUN }} "unset PYTHONWARNINGS && pre-commit run --all"
+      run: ${{ env.RUN }} "unset PYTHONWARNINGS && SKIP=check-added-large-files pre-commit run --all"
 
   valgrind:
     name: valgrind

.github/workflows/tools_tests.yaml

@@ -2,8 +2,8 @@ name: tools
 
 on:
   push:
-    branches:
-      - master
+    branches-ignore:
+      - 'testing-closet*'
   pull_request:
 
 concurrency:

.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

@@ -107,12 +107,14 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"DisablePowerDown", PERSISTENT},
     {"DisableUpdates", PERSISTENT},
+    {"LQR", PERSISTENT},
     {"DisengageOnAccelerator", PERSISTENT},
     {"DmModelInitialized", CLEAR_ON_ONROAD_TRANSITION},
     {"DongleId", PERSISTENT},
     {"DoReboot", CLEAR_ON_MANAGER_START},
     {"DoShutdown", CLEAR_ON_MANAGER_START},
     {"DoUninstall", CLEAR_ON_MANAGER_START},
+    {"DrivingPersonalitiesUIWheel", PERSISTENT},
     {"ExperimentalLongitudinalEnabled", PERSISTENT},
     {"ExperimentalMode", PERSISTENT},
     {"ExperimentalModeConfirmed", PERSISTENT},
@@ -207,6 +209,10 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"Version", PERSISTENT},
     {"VisionRadarToggle", PERSISTENT},
     {"WheeledBody", PERSISTENT},
+    // edison params
+    {"LQR", PERSISTENT},
+    {"ScreenOffTimer", PERSISTENT},
+    {"CruiseSpeedRewrite", PERSISTENT},
 };
 
 } // namespace

common/params.h

@@ -36,6 +36,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
@@ -46,6 +50,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,10 +1,11 @@
 from openpilot.common.params_pyx import Params, ParamKeyType, UnknownKeyName, put_nonblocking, \
-                                        put_bool_nonblocking
+                                        put_bool_nonblocking, put_int_nonblocking
 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) except + 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

@@ -83,6 +83,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

@@ -189,6 +189,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)
       else:

selfdrive/car/torque_data/params.yaml

@@ -70,6 +70,7 @@ TOYOTA HIGHLANDER 2020: [1.9617570834136164, 1.8611643317268927, 0.1451967325611
 TOYOTA HIGHLANDER HYBRID 2018: [1.752033, 1.6433903296845025, 0.144600]
 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.279239424615458, 2.087101966779332, 0.13682208413446817]

selfdrive/car/torque_data/substitute.yaml

@@ -5,7 +5,6 @@ MAZDA CX-5 2022: MAZDA CX-9 2021
 MAZDA CX-9: MAZDA CX-9 2021
 
 TOYOTA ALPHARD 2020: TOYOTA SIENNA 2018
-TOYOTA PRIUS v 2017 : TOYOTA PRIUS 2017
 LEXUS IS 2018: LEXUS NX 2018
 LEXUS CT HYBRID 2018 : LEXUS NX 2018
 LEXUS ES 2018: TOYOTA CAMRY 2018

selfdrive/car/toyota/carcontroller.py

@@ -130,15 +130,18 @@ def update(self, CC, CS, now_nanos):
     # 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(toyotacan.create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert))
+        can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead,
+                                                        CS.acc_type, adjust_distance, fcw_alert))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False))
+        can_sends.append(toyotacan.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.

selfdrive/car/toyota/carstate.py

@@ -4,6 +4,7 @@
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import mean
 from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.params import Params, put_int_nonblocking
 from openpilot.common.realtime import DT_CTRL
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
@@ -44,6 +45,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()
 
@@ -116,7 +125,7 @@ def update(self, cp, cp_cam):
     else:
       ret.accFaulted = cp.vl["PCM_CRUISE_2"]["ACC_FAULTED"] != 0
       ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0
-      ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]["SET_SPEED"] * CV.KPH_TO_MS
+      ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]["SET_SPEED"] * CV.KPH_TO_MS * self.CP.wheelSpeedFactor
       cluster_set_speed = cp.vl["PCM_CRUISE_SM"]["UI_SET_SPEED"]
 
     # UI_SET_SPEED is always non-zero when main is on, hide until first enable
@@ -160,6 +169,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
@@ -193,12 +220,18 @@ def get_can_parser(CP):
     if CP.enableBsm:
       messages.append(("BSM", 1))
 
+    # KRKeegan - Add support for toyota distance button
+    if bool(CP.flags & ToyotaFlags.SMART_DSU):
+      messages.append(("SDSU", 0))
+
     if CP.carFingerprint in RADAR_ACC_CAR and not CP.flags & ToyotaFlags.DISABLE_RADAR.value:
       if not CP.flags & ToyotaFlags.SMART_DSU.value:
         messages += [
           ("ACC_CONTROL", 33),
         ]
       messages += [
+        # try to fix duplicate message check
+        #("ACC_CONTROL", 0),
         ("PCS_HUD", 1),
       ]
 
@@ -207,6 +240,10 @@ def get_can_parser(CP):
         ("PRE_COLLISION", 33),
       ]
 
+#    try to fix duplicate message check
+#    if CP.carFingerprint in (TSS2_CAR | RADAR_ACC_CAR | NO_STOP_TIMER_CAR):
+#      messages.append(("PCM_CRUISE_SM", 0))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod

selfdrive/car/toyota/interface.py

@@ -1,7 +1,9 @@
 from cereal import car
 from openpilot.common.conversions import Conversions as CV
+from openpilot.common.params import Params
 from panda import Panda
 from panda.python import uds
+from openpilot.selfdrive.car.toyota.tunes import LatTunes, set_lat_tune
 from openpilot.selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
                                         MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
 from openpilot.selfdrive.car import get_safety_config
@@ -10,9 +12,14 @@
 
 EventName = car.CarEvent.EventName
 SteerControlType = car.CarParams.SteerControlType
-
+GearShifter = car.CarState.GearShifter
 
 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
@@ -60,9 +67,12 @@ 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
       ret.tireStiffnessFactor = 0.5533
       ret.mass = 3340. * CV.LB_TO_KG
+      ret.wheelSpeedFactor = 1.09
+      if Params().get_bool("LQR"):
+        set_lat_tune(ret.lateralTuning, LatTunes.LQR_PV)
 
     elif candidate in (CAR.RAV4, CAR.RAV4H):
       stop_and_go = True if (candidate in CAR.RAV4H) else False
@@ -278,9 +288,23 @@ def init(CP, logcan, sendcan):
   # 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)
+    events = self.create_common_events(ret, extra_gears=[GearShifter.sport])
 
     # Lane Tracing Assist control is unavailable (EPS_STATUS->LTA_STATE=0) until
     # the more accurate angle sensor signal is initialized

selfdrive/car/toyota/toyotacan.py

@@ -27,12 +27,12 @@ def create_lta_steer_command(packer, steer_angle, steer_req, frame, setme_x64):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance, fcw_alert):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
     "ACC_TYPE": acc_type,
-    "DISTANCE": 0,
+    "DISTANCE": distance,
     "MINI_CAR": lead,
     "PERMIT_BRAKING": 1,
     "RELEASE_STANDSTILL": not standstill_req,