Merge branch 'master-priv' into accel-profile

CHANGELOGS.md

@@ -8,6 +8,17 @@ sunnypilot - 0.9.8.0 (2024-xx-xx)
 ************************
 * UPDATED: Synced with commaai's openpilot
   * master commit b45caf4 (June 14, 2024)
+* NEW❗: Toyota - Enhanced Blind Spot Monitor (BSM) thanks to arne182, rav4kumar, and eFiniLan!
+  * Enables Blind Spot Monitor (BSM) signals parsing in sunnypilot using the factory Blind Spot Monitor (BSM)
+  * sunnypilot will use debugging CAN messages to receive unfiltered BSM signals, allowing detection of more objects
+  * Supported platforms
+    * RAV4 TSS1, equipped with factory Blind Spot Monitoring (BSM)
+    * Lexus LSS1, equipped with factory Blind Spot Monitoring (BSM)
+    * Toyota TSS1/1.5, equipped with factory Blind Spot Monitoring (BSM)
+    * Prius TSS2, equipped with factory Blind Spot Monitoring (BSM)
+  * NOTE: Only enable this feature if your Toyota/Lexus vehicle has factory Blind Spot Monitor equipped, and mentioned in the supported platforms list
+* UPDATED: Toyota: TSS2 longitudinal: Custom Tuning
+  * Re-tuned and tested by the community (July 1, 2024)
 * Kia Ceed Plug-in Hybrid Non-SCC 2022 support thanks to TerminatorNL!
 
 sunnypilot - 0.9.7.1 (2024-06-13)

common/params.cc

@@ -318,6 +318,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"TorqueFriction", PERSISTENT | BACKUP},
     {"TorqueMaxLatAccel", PERSISTENT | BACKUP},
     {"TorquedOverride", PERSISTENT | BACKUP},
+    {"ToyotaEnhancedBsm", PERSISTENT | BACKUP},
     {"ToyotaSnG", PERSISTENT | BACKUP},
     {"ToyotaTSS2Long", PERSISTENT | BACKUP},
     {"TrueVEgoUi", PERSISTENT | BACKUP},

selfdrive/car/toyota/carcontroller.py

@@ -6,7 +6,7 @@ from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_st
 from openpilot.selfdrive.car.interfaces import CarControllerBase
 from openpilot.selfdrive.car.toyota import toyotacan
 from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
-                                        MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \
+                                        MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, ToyotaFlagsSP, \
                                         UNSUPPORTED_DSU_CAR
 from opendbc.can.packer import CANPacker
 
@@ -26,6 +26,9 @@ MAX_USER_TORQUE = 500
 MAX_LTA_ANGLE = 94.9461  # deg
 MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150  # slightly above steering pressed allows some resistance when changing lanes
 
+LEFT_BLINDSPOT = b"\x41"
+RIGHT_BLINDSPOT = b"\x42"
+
 
 class CarController(CarControllerBase):
   def __init__(self, dbc_name, CP, VM):
@@ -48,6 +51,10 @@ class CarController(CarControllerBase):
     self._reverse_acc_change = self.param_s.get_bool("ReverseAccChange")
     self._sng_hack = self.param_s.get_bool("ToyotaSnG")
 
+    self.left_blindspot_debug_enabled = False
+    self.right_blindspot_debug_enabled = False
+    self.last_blindspot_frame = 0
+
   def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
     hud_control = CC.hudControl
@@ -205,6 +212,9 @@ class CarController(CarControllerBase):
     if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
       can_sends.append([0x750, 0, b"\x0F\x02\x3E\x00\x00\x00\x00\x00", 0])
 
+    if self.CP.spFlags & ToyotaFlagsSP.SP_ENHANCED_BSM and self.frame > 200:
+      can_sends.extend(self.create_enhanced_bsm_messages(CS, 20, True))
+
     new_actuators = actuators.as_builder()
     new_actuators.steer = apply_steer / self.params.STEER_MAX
     new_actuators.steerOutputCan = apply_steer
@@ -214,3 +224,45 @@ class CarController(CarControllerBase):
 
     self.frame += 1
     return new_actuators, can_sends
+
+  # Enhanced BSM (@arne182, @rav4kumar)
+  def create_enhanced_bsm_messages(self, CS: car.CarState, e_bsm_rate: int = 20, always_on: bool = True):
+    # let's keep all the commented out code for easy debug purpose for future.
+    can_sends = []
+
+    # left bsm
+    if not self.left_blindspot_debug_enabled:
+      if always_on or CS.out.vEgo > 6:  # eagle eye camera will stop working if left bsm is switched on under 6m/s
+        can_sends.append(toyotacan.create_set_bsm_debug_mode(LEFT_BLINDSPOT, True))
+        self.left_blindspot_debug_enabled = True
+        # print("bsm debug left, on")
+    else:
+      if not always_on and self.frame - self.last_blindspot_frame > 50:
+        can_sends.append(toyotacan.create_set_bsm_debug_mode(LEFT_BLINDSPOT, False))
+        self.left_blindspot_debug_enabled = False
+        # print("bsm debug left, off")
+      if self.frame % e_bsm_rate == 0:
+        can_sends.append(toyotacan.create_bsm_polling_status(LEFT_BLINDSPOT))
+        # if CS.out.leftBlinker:
+        self.last_blindspot_frame = self.frame
+        # print(self.last_blindspot_frame)
+        # print("bsm poll left")
+    # right bsm
+    if not self.right_blindspot_debug_enabled:
+      if always_on or CS.out.vEgo > 6:  # eagle eye camera will stop working if right bsm is switched on under 6m/s
+        can_sends.append(toyotacan.create_set_bsm_debug_mode(RIGHT_BLINDSPOT, True))
+        self.right_blindspot_debug_enabled = True
+        # print("bsm debug right, on")
+    else:
+      if not always_on and self.frame - self.last_blindspot_frame > 50:
+        can_sends.append(toyotacan.create_set_bsm_debug_mode(RIGHT_BLINDSPOT, False))
+        self.right_blindspot_debug_enabled = False
+        # print("bsm debug right, off")
+      if self.frame % e_bsm_rate == e_bsm_rate / 2:
+        can_sends.append(toyotacan.create_bsm_polling_status(RIGHT_BLINDSPOT))
+        # if CS.out.rightBlinker:
+        self.last_blindspot_frame = self.frame
+        # print(self.last_blindspot_frame)
+        # print("bsm poll right")
+
+    return can_sends

selfdrive/car/toyota/carstate.py

@@ -69,6 +69,18 @@ class CarState(CarStateBase):
       self.zss_angle_offset = 0.
       self.zss_threshold_count = 0
 
+    self._left_blindspot = False
+    self._left_blindspot_d1 = 0
+    self._left_blindspot_d2 = 0
+    self._left_blindspot_counter = 0
+
+    self._right_blindspot = False
+    self._right_blindspot_d1 = 0
+    self._right_blindspot_d2 = 0
+    self._right_blindspot_counter = 0
+
+    self.frame = 0
+
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
 
@@ -245,8 +257,12 @@ class CarState(CarStateBase):
       else:
         self.distance_button = cp.vl["SDSU"]["FD_BUTTON"]
 
+    if self.CP.spFlags & ToyotaFlagsSP.SP_ENHANCED_BSM and self.frame > 199:
+      ret.leftBlindspot, ret.rightBlindspot = self.sp_get_enhanced_bsm(cp)
+
     self._update_traffic_signals(cp_cam)
     ret.cruiseState.speedLimit = self._calculate_speed_limit()
+    self.frame += 1
 
     return ret
 
@@ -328,6 +344,43 @@ class CarState(CarStateBase):
       return self._spdval1 * CV.MPH_TO_MS
     return 0
 
+  # Enhanced BSM (@arne182, @rav4kumar)
+  def sp_get_enhanced_bsm(self, cp):
+    # Let's keep all the commented out code for easy debug purposes in the future.
+    distance_1 = cp.vl["DEBUG"].get('BLINDSPOTD1')
+    distance_2 = cp.vl["DEBUG"].get('BLINDSPOTD2')
+    side = cp.vl["DEBUG"].get('BLINDSPOTSIDE')
+
+    if all(val is not None for val in [distance_1, distance_2, side]):
+      if side == 65:  # left blind spot
+        if distance_1 != self._left_blindspot_d1 or distance_2 != self._left_blindspot_d2:
+          self._left_blindspot_d1 = distance_1
+          self._left_blindspot_d2 = distance_2
+          self._left_blindspot_counter = 100
+        self._left_blindspot = distance_1 > 10 or distance_2 > 10
+
+      elif side == 66:  # right blind spot
+        if distance_1 != self._right_blindspot_d1 or distance_2 != self._right_blindspot_d2:
+          self._right_blindspot_d1 = distance_1
+          self._right_blindspot_d2 = distance_2
+          self._right_blindspot_counter = 100
+        self._right_blindspot = distance_1 > 10 or distance_2 > 10
+
+      # update counters
+      self._left_blindspot_counter = max(0, self._left_blindspot_counter - 1)
+      self._right_blindspot_counter = max(0, self._right_blindspot_counter - 1)
+
+      # reset blind spot status if counter reaches 0
+      if self._left_blindspot_counter == 0:
+        self._left_blindspot = False
+        self._left_blindspot_d1 = self._left_blindspot_d2 = 0
+
+      if self._right_blindspot_counter == 0:
+        self._right_blindspot = False
+        self._right_blindspot_d1 = self._right_blindspot_d2 = 0
+
+    return self._left_blindspot, self._right_blindspot
+
   @staticmethod
   def get_can_parser(CP):
     messages = [
@@ -384,6 +437,9 @@ class CarState(CarStateBase):
     if CP.spFlags & ToyotaFlagsSP.SP_ZSS:
       messages.append(("SECONDARY_STEER_ANGLE", 0))
 
+    if CP.spFlags & ToyotaFlagsSP.SP_ENHANCED_BSM:
+      messages.append(("DEBUG", 65))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod

selfdrive/car/toyota/interface.py

@@ -160,23 +160,40 @@ class CarInterface(CarInterfaceBase):
 
     sp_tss2_long_tune = Params().get_bool("ToyotaTSS2Long")
 
+    # hand tuned (July 1, 2024)
+    def custom_tss2_longitudinal_tuning():
+      ret.vEgoStopping = 0.01
+      ret.vEgoStarting = 0.01
+      ret.stoppingDecelRate = 0.35
+
+    def default_tss2_longitudinal_tuning():
+      ret.vEgoStopping = 0.25
+      ret.vEgoStarting = 0.25
+      ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
+
+    def default_longitudinal_tuning():
+      tune.kiBP = [0., 5., 35.]
+      tune.kiV = [3.6, 2.4, 1.5]
+
     tune = ret.longitudinalTuning
     if candidate in TSS2_CAR or ret.enableGasInterceptorDEPRECATED:
       if sp_tss2_long_tune:
-        tune.kpBP = [0., 5., 20., 30.]
-        tune.kpV = [1.3, 1.0, 0.7, 0.1]
-        tune.kiBP = [0.,   1.,    2.,    3.,   4.,   5.,    12.,  20.,  27., 40.]
-        tune.kiV = [.348, .3361, .3168, .2831, .2571, .226, .198, .17,  .10, .01]
+        tune.kiBP = [0., 5., 12., 20., 27., 36., 50]
+        tune.kiV = [0.35, 0.23, 0.20, 0.17, 0.10, 0.07, 0.01]
+        custom_tss2_longitudinal_tuning()
       else:
         tune.kpV = [0.0]
         tune.kiV = [0.5]
       if candidate in TSS2_CAR:
-        ret.vEgoStopping = 0.25
-        ret.vEgoStarting = 0.25
-        ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
+        default_tss2_longitudinal_tuning()
     else:
-      tune.kiBP = [0., 5., 35.]
-      tune.kiV = [3.6, 2.4, 1.5]
+      default_longitudinal_tuning()
+
+    if Params().get_bool("ToyotaEnhancedBsm"):
+      ret.spFlags |= ToyotaFlagsSP.SP_ENHANCED_BSM.value
+
+    if candidate == CAR.TOYOTA_PRIUS_TSS2:
+      ret.spFlags |= ToyotaFlagsSP.SP_NEED_DEBUG_BSM.value
 
     return ret
 

selfdrive/car/toyota/toyotacan.py

@@ -1,4 +1,5 @@
 from cereal import car
+from openpilot.selfdrive.car import make_can_msg
 
 SteerControlType = car.CarParams.SteerControlType
 
@@ -118,3 +119,14 @@ def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_dep
     ]})
 
   return packer.make_can_msg("LKAS_HUD", 0, values)
+
+
+def create_set_bsm_debug_mode(lr_blindspot, enabled):
+  dat = b"\x02\x10\x60\x00\x00\x00\x00" if enabled else b"\x02\x10\x01\x00\x00\x00\x00"
+  dat = lr_blindspot + dat
+
+  return make_can_msg(0x750, dat, 0)
+
+
+def create_bsm_polling_status(lr_blindspot):
+  return make_can_msg(0x750, lr_blindspot + b"\x02\x21\x69\x00\x00\x00\x00", 0)

selfdrive/car/toyota/values.py

@@ -62,6 +62,8 @@ class ToyotaFlags(IntFlag):
 
 class ToyotaFlagsSP(IntFlag):
   SP_ZSS = 1
+  SP_ENHANCED_BSM = 2
+  SP_NEED_DEBUG_BSM = 4
 
 
 class Footnote(Enum):

selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc

@@ -124,6 +124,15 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
   toyotaTss2LongTune->setConfirmation(true, false);
   addItem(toyotaTss2LongTune);
 
+  toyotaEnhancedBsm = new ParamControl(
+    "ToyotaEnhancedBsm",
+    tr("Enable Enhanced Blind Spot Monitor"),
+    "",
+    "../assets/offroad/icon_blank.png"
+  );
+  toyotaEnhancedBsm->setConfirmation(true, false);
+  addItem(toyotaEnhancedBsm);
+
   auto toyotaSngHack = new ParamControl(
     "ToyotaSnG",
     tr("Enable Toyota Stop and Go Hack"),
@@ -146,9 +155,69 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
 
   // trigger offroadTransition when going onroad/offroad
   connect(uiState(), &UIState::offroadTransition, [=](bool offroad) {
+    is_onroad = !offroad;
     hkgSmoothStop->setEnabled(offroad);
     toyotaTss2LongTune->setEnabled(offroad);
+    toyotaEnhancedBsm->setEnabled(offroad);
     toyotaSngHack->setEnabled(offroad);
     volkswagenCCOnly->setEnabled(offroad);
+    updateToggles();
   });
 }
+
+void SPVehiclesTogglesPanel::showEvent(QShowEvent *event) {
+  updateToggles();
+}
+
+void SPVehiclesTogglesPanel::updateToggles() {
+  if (!isVisible()) {
+    return;
+  }
+
+  // Toyota: Enhanced Blind Spot Monitor
+  QString ebsm_init = "<font color='yellow'>⚠️ " + tr("Start the car to check car compatibility") + "</font>";
+  QString ebsm_not_required = "<font color=#00ff00>✅ " + tr("This platform is already supported, therefore no need to enable this toggle") + "</font>";
+  QString ebsm_not_supported = "<font color='yellow'>⚠️ " + tr("This platform is not supported") + "</font>";
+  QString ebsm_supported = "<font color=#00ff00>✅ " + tr("This platform can be supported") + "</font>";
+
+  auto cp_bytes = params.get("CarParamsPersistent");
+  if (!cp_bytes.empty()) {
+    AlignedBuffer aligned_buf;
+    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
+    cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
+
+    // Toyota: Enhanced Blind Spot Monitor
+    {
+      if (CP.getCarName() == "toyota") {
+        if (CP.getEnableBsm() && !(CP.getSpFlags() & 4)) {  // 4 = ToyotaFlagsSP.SP_NEED_DEBUG_BSM
+          toyotaEnhancedBsm->setDescription(toyotaEnhancedBsmDesciptionBuilder(ebsm_not_required));
+          toyotaEnhancedBsm->showDescription();
+          toyotaEnhancedBsm->setEnabled(false);
+          params.remove("ToyotaEnhancedBsm");
+        } else {
+          toyotaEnhancedBsm->setDescription(toyotaEnhancedBsmDesciptionBuilder(ebsm_supported));
+          toyotaEnhancedBsm->showDescription();
+          toyotaEnhancedBsm->setEnabled(true);
+        }
+      } else {
+        toyotaEnhancedBsm->setDescription(toyotaEnhancedBsmDesciptionBuilder(ebsm_not_supported));
+        toyotaEnhancedBsm->showDescription();
+        toyotaEnhancedBsm->setEnabled(false);
+        params.remove("ToyotaEnhancedBsm");
+      }
+    }
+
+    toyotaEnhancedBsm->refresh();
+  } else {
+    toyotaEnhancedBsm->setEnabled(false);
+    params.remove("ToyotaEnhancedBsm");
+
+    toyotaEnhancedBsm->setDescription(toyotaEnhancedBsmDesciptionBuilder(ebsm_init));
+    toyotaEnhancedBsm->showDescription();
+  }
+
+  // override toggle state when onroad/offroad
+  if (is_onroad) {
+    toyotaEnhancedBsm->setEnabled(false);
+  }
+}

selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.h

@@ -35,9 +35,25 @@ class SPVehiclesTogglesPanel : public ListWidget {
   Q_OBJECT
 public:
   explicit SPVehiclesTogglesPanel(VehiclePanel *parent);
+  void showEvent(QShowEvent *event) override;
+
+public slots:
+  void updateToggles();
 
 private:
   Params params;
+  bool is_onroad = false;
 
   ParamControl *stockLongToyota;
+  ParamControl *toyotaEnhancedBsm;
+
+  const QString toyotaEnhancedBsmDescription = QString("%1<br><br>"
+                                                       "%2")
+                                               .arg(tr("sunnypilot will use debugging CAN messages to receive unfiltered BSM signals, allowing detection of more objects."))
+                                               .arg(tr("Tested on RAV4 TSS1, Lexus LSS1, Toyota TSS1/1.5, and Prius TSS2."));
+
+  QString toyotaEnhancedBsmDesciptionBuilder(const QString &custom_description) {
+    QString description = "<b>" + custom_description + "</b><br><br>" + toyotaEnhancedBsmDescription;
+    return description;
+  }
 };

system/manager/manager.py

@@ -97,6 +97,7 @@ def manager_init() -> None:
     ("TorqueDeadzoneDeg", "0"),
     ("TorqueFriction", "1"),
     ("TorqueMaxLatAccel", "250"),
+    ("ToyotaEnhancedBsm", "0"),
     ("TrueVEgoUi", "0"),
     ("TurnSpeedControl", "0"),
     ("TurnVisionControl", "0"),