Re enable sentry and swaglog handler for device metadata & crash information

This reverts commit a9f2f54bcc.

CHANGELOGS.md

@@ -5,9 +5,16 @@ sunnypilot - 0.9.8.0 (2024-xx-xx)
 * UPDATED: Synced with commaai's openpilot
   * master commit 4ef757c (July 06, 2024)
 * NEW❗: Default Driving Model: Notre Dame (July 01, 2024)
-* NEW❗: Longitudinal: Acceleration Personality thanks to kegman, rav4kumar, and arne1282!
+* NEW❗: Longitudinal: Acceleration Personality thanks to kegman, rav4kumar, and arne1282! (CTV 2.0: GlideTech)
   * Select from three distinct acceleration personalities: Eco, Normal, and Sport
   * Acceleration personalities are integrated directly into the model's acceleration matrix and can be activated in real-time!
+* NEW❗: Toyota - Drive Mode Selector
+  *  When enabled you can control acceleration personality just with press of button!
+* UPDATED: Dynamic Experimental Control
+  * Switched to weighted moving averages to enhance responsiveness to recent data.
+  * Goal is to improve real-time detection accuracy in dynamic conditions.
+  * Capable of handling the increased complexity that comes with this approach.
+  * Particularly beneficial in environments where recent changes are critical to performance.
 * NEW❗: Longitudinal: Dynamic Personality thanks to rav4kumar!
   * Dynamically adjusts following distance and reaction based on your "Driving Personality" setting
   * Personalities adapt in real-time to your speed and the distance to the lead car
@@ -26,8 +33,8 @@ sunnypilot - 0.9.8.0 (2024-xx-xx)
     * 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)
+* UPDATED: Toyota: TSS2 longitudinal: Custom Tuning (CTV 2.0: GlideTech)
+  * Re-tuned and tested by the community (September 29, 2024)
 * UPDATED: Driving Model Selector v5
   * NEW❗: Driving Model additions
     * Notre Dame (July 01, 2024) - NDv3

cereal/custom.capnp

@@ -35,6 +35,11 @@ enum ModelGeneration {
   five @5;
 }
 
+enum MpcSource {
+  acc @0;
+  blended @1;
+}
+
 struct ControlsStateSP @0x81c2f05a394cf4af {
   lateralState @0 :Text;
   personality @8 :LongitudinalPersonalitySP;
@@ -92,8 +97,10 @@ struct LongitudinalPlanSP @0xaedffd8f31e7b55d {
   desiredTF @13 :Float32;
   notSpeedLimit @14 :Int16;
   e2eX @15 :List(Float32);
-  e2eBlended @18 :Text;
+  e2eBlendedDEPRECATED @18 :Text;
   e2eStatus @22 :Bool;
+  mpcSource @23 :MpcSource;
+  dynamicExperimentalControl @24 :Bool;
 
   distToTurn @7 :Float32;
   turnSpeed @8 :Float32;

common/params.cc

@@ -332,6 +332,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"TorquedOverride", PERSISTENT | BACKUP},
     {"ToyotaAutoLockBySpeed", PERSISTENT | BACKUP},
     {"ToyotaAutoUnlockByShifter", PERSISTENT | BACKUP},
+    {"ToyotaDriveMode", PERSISTENT | BACKUP},
     {"ToyotaEnhancedBsm", PERSISTENT | BACKUP},
     {"ToyotaSnG", PERSISTENT | BACKUP},
     {"ToyotaTSS2Long", PERSISTENT | BACKUP},

selfdrive/car/toyota/carstate.py

@@ -1,18 +1,19 @@
 import copy
 
-from cereal import car
+from cereal import car, custom
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import mean
 from openpilot.common.filter_simple import FirstOrderFilter
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
 from openpilot.selfdrive.car import DT_CTRL
+from openpilot.common.params import Params
 from openpilot.selfdrive.car.interfaces import CarStateBase
 from openpilot.selfdrive.car.toyota.values import ToyotaFlags, ToyotaFlagsSP, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
                                                   TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
 
 SteerControlType = car.CarParams.SteerControlType
-
+AccelPersonality = custom.AccelerationPersonality
 # These steering fault definitions seem to be common across LKA (torque) and LTA (angle):
 # - high steer rate fault: goes to 21 or 25 for 1 frame, then 9 for 2 seconds
 # - lka/lta msg drop out: goes to 9 then 11 for a combined total of 2 seconds, then 3.
@@ -79,6 +80,13 @@ class CarState(CarStateBase):
     self._right_blindspot_d2 = 0
     self._right_blindspot_counter = 0
 
+    self.signals_checked = False
+    self.sport_signal_seen = False
+    self.eco_signal_seen = False
+    self.accel_profile = None
+    self.prev_accel_profile = None
+    self.accel_profile_init = False
+    self.toyota_drive_mode = Params().get_bool('ToyotaDriveMode')
     self.frame = 0
 
   def update(self, cp, cp_cam):
@@ -176,6 +184,51 @@ class CarState(CarStateBase):
     ret.leftBlinker = ret.leftBlinkerOn = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1
     ret.rightBlinker = ret.rightBlinkerOn = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2
 
+    if self.toyota_drive_mode:
+      # Determine sport signal based on car model
+      sport_signal = 'SPORT_ON_2' if self.CP.carFingerprint in (CAR.TOYOTA_RAV4_TSS2, CAR.LEXUS_ES_TSS2, CAR.TOYOTA_HIGHLANDER_TSS2) else 'SPORT_ON'
+
+      # Check signals once
+      if not self.signals_checked:
+        self.signals_checked = True
+
+        # Try to detect sport mode signal, handle missing signal with a fallback
+        try:
+          sport_mode = cp.vl["GEAR_PACKET"][sport_signal]
+          self.sport_signal_seen = True
+        except KeyError:
+          sport_mode = 0
+          self.sport_signal_seen = False
+
+        # Try to detect eco mode signal, handle missing signal with a fallback
+        try:
+          eco_mode = cp.vl["GEAR_PACKET"]['ECON_ON']
+          self.eco_signal_seen = True
+        except KeyError:
+          eco_mode = 0
+          self.eco_signal_seen = False
+      else:
+        # Always re-check the signals to account for mode changes
+        sport_mode = cp.vl["GEAR_PACKET"][sport_signal] if self.sport_signal_seen else 0
+        eco_mode = cp.vl["GEAR_PACKET"]['ECON_ON'] if self.eco_signal_seen else 0
+
+      # Set acceleration profile based on detected modes, with sport mode having higher priority
+      if sport_mode == 1:
+        self.accel_profile = AccelPersonality.sport
+      elif eco_mode == 1:
+        self.accel_profile = AccelPersonality.eco
+      else:
+        self.accel_profile = AccelPersonality.normal
+
+      print(f"Accel profile set to: {self.accel_profile}")
+
+      # If not initialized, sync profile with the current mode on the car
+      if not self.accel_profile_init or self.accel_profile != self.prev_accel_profile:
+        Params().put_nonblocking('AccelPersonality', str(self.accel_profile))
+        self.accel_profile_init = True
+        # Update the previous profile to prevent unnecessary re-syncing
+        self.prev_accel_profile = self.accel_profile
+
     if self.CP.carFingerprint != CAR.TOYOTA_MIRAI:
       ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"]
 

selfdrive/car/toyota/interface.py

@@ -160,31 +160,26 @@ class CarInterface(CarInterfaceBase):
 
     sp_tss2_long_tune = Params().get_bool("ToyotaTSS2Long")
 
-    # hand tuned (August 12, 2024)
-    def custom_tss2_longitudinal_tuning():
+    # Last updated: September 29, 2024
+    def custom_tss2_longitudinal_tuning(): # hand tuned
       ret.vEgoStopping = 0.25
-      ret.vEgoStarting = 0.25
-      ret.stoppingDecelRate = 0.0074
+      ret.vEgoStarting = 0.01
+      ret.stoppingDecelRate = 0.006
 
-    def default_tss2_longitudinal_tuning():
+    def default_tss2_longitudinal_tuning(): # stock comma
       ret.vEgoStopping = 0.25
-      ret.vEgoStarting = 0.25
-      ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
+      ret.vEgoStarting = 0.10
+      ret.stoppingDecelRate = 0.007  # reach stopping target smoothly
 
-    def default_longitudinal_tuning():
+    def default_longitudinal_tuning(): # stock comma
       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.kiBP = [0., 0.1,  3.,  12.,   20.,  26.,  36.,  50]
-        #tune.kiV = [0.34, 0.35, 0.2896, 0.2237, 0.174, 0.10, 0.08, 0.06]
-        #smooth
-        tune.kiBP = [0.,   12.,  20.,  27.,  36.,  50]
-        tune.kiV = [0.333, 0.203, 0.17, 0.12, 0.08, 0.06]
-        #tune.kiBP = [0.,  2.,   5.,  12.,  16,   20.,  27.,  36.,  50]
-        #tune.kiV = [0.27, 0.24, 0.2205, 0.20, 0.18, 0.17, 0.12, 0.08, 0.06]
+        tune.kiBP = [0.,  5.,   12.,  20.,  27.,  36.,  40.]
+        tune.kiV = [0.34, 0.234, 0.20, 0.17, 0.105, 0.09, 0.08]
         custom_tss2_longitudinal_tuning()
       else:
         tune.kpV = [0.0]

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -63,7 +63,7 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
   elif personality==custom.LongitudinalPersonalitySP.standard:
     return 1.0
   elif personality==custom.LongitudinalPersonalitySP.moderate:
-    return 0.85
+    return 0.9
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
     return 0.8
   elif personality==custom.LongitudinalPersonalitySP.overtake:
@@ -86,20 +86,20 @@ def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard):
   else:
     raise NotImplementedError("Longitudinal personality not supported")
 
-
+# Last updated: September 29, 2024
 def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP.standard):
   if personality==custom.LongitudinalPersonalitySP.relaxed:
-    x_vel =  [0,    5.,   5.01,  20.,   27.7]
-    y_dist = [1.0,  1.0,  1.75,  1.75,  1.83]
+    x_vel =  [0,    14.,   27.7]
+    y_dist = [1.75, 1.75,  2.00]
   elif personality==custom.LongitudinalPersonalitySP.standard:
-    x_vel =  [0,    4.,   4.01,  20.,   27.7]
-    y_dist = [1.0,  1.0,  1.75,  1.75,  1.80]
+    x_vel =  [0,    14.,   27.7]
+    y_dist = [1.75, 1.75,  1.70]
   elif personality==custom.LongitudinalPersonalitySP.moderate:
-    x_vel =  [0,    3.,   3.01,  20.,   27.7]
-    y_dist = [1.0,  1.0,  1.45,  1.45,  1.50]
+    x_vel =  [0,    14.,   27.7]
+    y_dist = [1.45, 1.45,  1.48]
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
-    x_vel =  [0,    2.,   2.01,  20.,   27.7]
-    y_dist = [1.0,  1.0,  1.20,  1.20,  1.30]
+    x_vel =  [0,    14.,   27.7]
+    y_dist = [1.25, 1.25,  1.28]
   else:
     raise NotImplementedError("Dynamic personality not supported")
   return np.interp(v_ego, x_vel, y_dist)

selfdrive/controls/lib/longitudinal_planner.py

@@ -36,6 +36,7 @@ _A_TOTAL_MAX_V = [1.7, 3.2]
 _A_TOTAL_MAX_BP = [20., 40.]
 
 
+MpcSource = custom.MpcSource
 EventName = car.CarEvent.EventName
 
 
@@ -131,10 +132,13 @@ class LongitudinalPlanner:
       self.read_param()
     self.param_read_counter += 1
     if self.dynamic_experimental_controller.is_enabled() and sm['controlsState'].experimentalMode:
-      self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState'], sm['navInstruction'].maneuverDistance)
+      self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt)
+      self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState'], sm['navInstruction'].maneuverDistance)
+      self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode()
     else:
       self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
 
+
     v_ego = sm['carState'].vEgo
     v_cruise_kph = min(sm['controlsState'].vCruise, V_CRUISE_MAX)
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
@@ -267,7 +271,8 @@ class LongitudinalPlanner:
     longitudinalPlanSP.turnSpeedControlState = self.turn_speed_controller.state
     longitudinalPlanSP.turnSpeed = float(self.turn_speed_controller.v_target)
 
-    longitudinalPlanSP.e2eBlended = self.mpc.mode
+    longitudinalPlanSP.mpcSource = MpcSource.blended if self.mpc.mode == 'blended' else MpcSource.acc
+    longitudinalPlanSP.dynamicExperimentalControl = self.dynamic_experimental_controller.is_enabled()
 
     pm.send('longitudinalPlanSP', plan_sp_send)
 

selfdrive/controls/lib/sunnypilot/accel_controller.py

@@ -21,22 +21,22 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 # THE SOFTWARE.
 
-# Last updated: August 12, 2024
+# Last updated: September 29, 2024
 
 from cereal import custom
 from openpilot.common.numpy_fast import interp
 
 AccelPersonality = custom.AccelerationPersonality
 
-# accel personality by @arne182 modified by cgw
-_DP_CRUISE_MIN_V =       [-0.05, -0.05, -0.088, -0.088, -0.25, -0.25, -0.39, -0.39,  -0.75]
-_DP_CRUISE_MIN_V_ECO =   [-0.04, -0.04, -0.087, -0.087, -0.24, -0.24, -0.38, -0.38,  -0.65]
-_DP_CRUISE_MIN_V_SPORT = [-0.06, -0.06, -0.089, -0.089, -0.26, -0.26, -0.40, -0.40,  -0.85]
-_DP_CRUISE_MIN_BP =      [0.,    5.0,   5.01,   11.1,   11.11, 20.,   20.01, 30.,    30.01]
+# accel personality by @arne182 modified by cgw and kumar
+_DP_CRUISE_MIN_V =       [-1.0, -1.0,  -0.88]
+_DP_CRUISE_MIN_V_ECO =   [-1.0, -1.0,  -0.76]
+_DP_CRUISE_MIN_V_SPORT = [-1.0, -1.0,  -1.0]
+_DP_CRUISE_MIN_BP =      [0.,   11.1,  20.]
 
-_DP_CRUISE_MAX_V =       [2.0, 2.0, 1.9, 1.60, 1.11, .75,  .55,  .42,  .2]
-_DP_CRUISE_MAX_V_ECO =   [2.0, 2.0, 1.8, 1.35, 0.86, .53,  .42,  .31,  .085]
-_DP_CRUISE_MAX_V_SPORT = [2.0, 2.0, 2.0, 2.00, 1.34, .96,  .78,  .60,  .4]
+_DP_CRUISE_MAX_V =       [2.0, 2.0, 2.0, 1.75, 1.03, .72,  .53,  .42,  .13]
+_DP_CRUISE_MAX_V_ECO =   [2.0, 2.0, 2.0, 1.50, 0.92, .54,  .43,  .32,  .088]
+_DP_CRUISE_MAX_V_SPORT = [2.0, 2.0, 2.0, 2.00, 1.25, .96,  .78,  .60,  .4]
 _DP_CRUISE_MAX_BP =      [0.,  1.,  6.,  8.,   11.,  20.,  25.,  30.,  55.]
 
 

selfdrive/controls/lib/sunnypilot/dynamic_experimental_controller.py

@@ -21,33 +21,39 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 # THE SOFTWARE.
 #
-# Version = 2024-1-29
+# Version = 2024-9-28
 from common.numpy_fast import interp
-from openpilot.selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE
+import numpy as np
 
-LEAD_WINDOW_SIZE = 5
+# d-e2e, from modeldata.h
+TRAJECTORY_SIZE = 33
+
+LEAD_WINDOW_SIZE = 4
 LEAD_PROB = 0.6
 
-SLOW_DOWN_WINDOW_SIZE = 5
+SLOW_DOWN_WINDOW_SIZE = 4
 SLOW_DOWN_PROB = 0.6
-SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
-SLOW_DOWN_DIST = [20, 30., 50., 70., 80., 90., 105., 120.]
 
-SLOWNESS_WINDOW_SIZE = 20
-SLOWNESS_PROB = 0.6
+SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
+SLOW_DOWN_DIST = [25., 38., 55., 75., 95., 115., 130., 150.]
+
+SLOWNESS_WINDOW_SIZE = 12
+SLOWNESS_PROB = 0.5
 SLOWNESS_CRUISE_OFFSET = 1.05
 
-DANGEROUS_TTC_WINDOW_SIZE = 5
-DANGEROUS_TTC = 2.0
+DANGEROUS_TTC_WINDOW_SIZE = 3
+DANGEROUS_TTC = 2.3
 
-HIGHWAY_CRUISE_KPH = 75
+HIGHWAY_CRUISE_KPH = 70
 
 STOP_AND_GO_FRAME = 60
 
 SET_MODE_TIMEOUT = 10
 
-MPC_FCW_WINDOW_SIZE = 5
-MPC_FCW_PROB = 0.6
+MPC_FCW_WINDOW_SIZE = 10
+MPC_FCW_PROB = 0.5
+
+V_ACC_MIN = 9.72
 
 
 class SNG_State:
@@ -77,29 +83,51 @@ class GenericMovingAverageCalculator:
     self.data = []
     self.total = 0
 
+class WeightedMovingAverageCalculator:
+  def __init__(self, window_size):
+    self.window_size = window_size
+    self.data = []
+    self.weights = np.linspace(1, 3, window_size)  # Linear weights, adjust as needed
+
+  def add_data(self, value):
+    if len(self.data) == self.window_size:
+      self.data.pop(0)
+    self.data.append(value)
+
+  def get_weighted_average(self):
+    if len(self.data) == 0:
+      return None
+    weighted_sum = np.dot(self.data, self.weights[-len(self.data):])
+    weight_total = np.sum(self.weights[-len(self.data):])
+    return weighted_sum / weight_total
+
+  def reset_data(self):
+    self.data = []
 
 class DynamicExperimentalController:
   def __init__(self):
     self._is_enabled = False
     self._mode = 'acc'
     self._mode_prev = 'acc'
+    self._mode_changed = False
     self._frame = 0
 
-    self._lead_gmac = GenericMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE)
+    # Use weighted moving average for filtering leads
+    self._lead_gmac = WeightedMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE)
     self._has_lead_filtered = False
     self._has_lead_filtered_prev = False
 
-    self._slow_down_gmac = GenericMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE)
+    self._slow_down_gmac = WeightedMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE)
     self._has_slow_down = False
 
     self._has_blinkers = False
 
-    self._slowness_gmac = GenericMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE)
+    self._slowness_gmac = WeightedMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE)
     self._has_slowness = False
 
     self._has_nav_instruction = False
 
-    self._dangerous_ttc_gmac = GenericMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE)
+    self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE)
     self._has_dangerous_ttc = False
 
     self._v_ego_kph = 0.
@@ -113,13 +141,50 @@ class DynamicExperimentalController:
     self._sng_transit_frame = 0
     self._sng_state = SNG_State.off
 
-    self._mpc_fcw_gmac = GenericMovingAverageCalculator(window_size=MPC_FCW_WINDOW_SIZE)
+    self._mpc_fcw_gmac = WeightedMovingAverageCalculator(window_size=MPC_FCW_WINDOW_SIZE)
     self._has_mpc_fcw = False
     self._mpc_fcw_crash_cnt = 0
 
     self._set_mode_timeout = 0
     pass
 
+
+  def _adaptive_slowdown_threshold(self):
+    """
+    Adapts the slow down threshold based on vehicle speed and recent behavior.
+    """
+    return interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.03 * np.log(1 + len(self._slow_down_gmac.data)))
+
+  def _anomaly_detection(self, recent_data, threshold=2.0, context_check=True):
+    """
+    Basic anomaly detection using standard deviation.
+    """
+    if len(recent_data) < 5:
+      return False
+    mean = np.mean(recent_data)
+    std_dev = np.std(recent_data)
+    anomaly = recent_data[-1] > mean + threshold * std_dev
+
+    # Context check to ensure repeated anomaly
+    if context_check:
+      return np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1
+    return anomaly
+
+  def _smoothed_lead_detection(self, lead_prob, smoothing_factor=0.2):
+    """
+    Smoothing the lead detection to avoid erratic behavior.
+    """
+    self._has_lead_filtered = (1 - smoothing_factor) * self._has_lead_filtered + smoothing_factor * lead_prob
+    return self._has_lead_filtered > LEAD_PROB
+
+  def _adaptive_lead_prob_threshold(self):
+    """
+    Adapts lead probability threshold based on driving conditions.
+    """
+    if self._v_ego_kph > HIGHWAY_CRUISE_KPH:
+      return LEAD_PROB + 0.1  # Increase the threshold on highways
+    return LEAD_PROB
+
   def _update(self, car_state, lead_one, md, controls_state, maneuver_distance):
     self._v_ego_kph = car_state.vEgo * 3.6
     self._v_cruise_kph = controls_state.vCruise
@@ -128,18 +193,27 @@ class DynamicExperimentalController:
 
     # fcw detection
     self._mpc_fcw_gmac.add_data(self._mpc_fcw_crash_cnt > 0)
-    self._has_mpc_fcw = self._mpc_fcw_gmac.get_moving_average() >= MPC_FCW_PROB
+    self._has_mpc_fcw = self._mpc_fcw_gmac.get_weighted_average() > MPC_FCW_PROB
 
     # nav enable detection
     self._has_nav_instruction = md.navEnabledDEPRECATED and maneuver_distance / max(car_state.vEgo, 1) < 13
 
-    # lead detection
+    # lead detection with smoothing
     self._lead_gmac.add_data(lead_one.status)
-    self._has_lead_filtered = self._lead_gmac.get_moving_average() >= LEAD_PROB
+    self._has_lead_filtered = self._lead_gmac.get_weighted_average() > LEAD_PROB
+    #lead_prob = self._lead_gmac.get_weighted_average() or 0
+    #self._has_lead_filtered = self._smoothed_lead_detection(lead_prob)
 
-    # slow down detection
-    self._slow_down_gmac.add_data(len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST))
-    self._has_slow_down = self._slow_down_gmac.get_moving_average() >= SLOW_DOWN_PROB
+    # adaptive slow down detection
+    adaptive_threshold = self._adaptive_slowdown_threshold()
+    slow_down_trigger = len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < adaptive_threshold
+    self._slow_down_gmac.add_data(slow_down_trigger)
+    self._has_slow_down = self._slow_down_gmac.get_weighted_average() > SLOW_DOWN_PROB
+
+    # anomaly detection for slow down events
+    if self._anomaly_detection(self._slow_down_gmac.data):
+      # Handle anomaly: potentially log it, adjust behavior, or issue a warning
+      self._has_slow_down = False  # Reset slow down if anomaly detected
 
     # blinker detection
     self._has_blinkers = car_state.leftBlinker or car_state.rightBlinker
@@ -161,7 +235,7 @@ class DynamicExperimentalController:
     # slowness detection
     if not self._has_standstill:
       self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph*SLOWNESS_CRUISE_OFFSET))
-      self._has_slowness = self._slowness_gmac.get_moving_average() >= SLOWNESS_PROB
+      self._has_slowness = self._slowness_gmac.get_weighted_average() > SLOWNESS_PROB
 
     # dangerous TTC detection
     if not self._has_lead_filtered and self._has_lead_filtered_prev:
@@ -171,14 +245,14 @@ class DynamicExperimentalController:
     if self._has_lead and car_state.vEgo >= 0.01:
       self._dangerous_ttc_gmac.add_data(lead_one.dRel/car_state.vEgo)
 
-    self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_moving_average() is not None and self._dangerous_ttc_gmac.get_moving_average() <= DANGEROUS_TTC
+    self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_weighted_average() is not None and self._dangerous_ttc_gmac.get_weighted_average() <= DANGEROUS_TTC
 
     # keep prev values
     self._has_standstill_prev = self._has_standstill
     self._has_lead_filtered_prev = self._has_lead_filtered
     self._frame += 1
 
-  def _blended_priority_mode(self):
+  def _radarless_mode(self):
     # when mpc fcw crash prob is high
     # use blended to slow down quickly
     if self._has_mpc_fcw:
@@ -190,21 +264,21 @@ class DynamicExperimentalController:
       self._set_mode('blended')
       return
 
-    # when blinker is on and speed is driving below highway cruise speed: blended
+    # when blinker is on and speed is driving below V_ACC_MIN: blended
     # we dont want it to switch mode at higher speed, blended may trigger hard brake
-    if self._has_blinkers and self._v_ego_kph < HIGHWAY_CRUISE_KPH:
-      self._set_mode('blended')
-      return
+    #if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
 
     # when at highway cruise and SNG: blended
     # ensuring blended mode is used because acc is bad at catching SNG lead car
     # especially those who accel very fast and then brake very hard.
-    if self._sng_state == SNG_State.going and self._v_cruise_kph >= HIGHWAY_CRUISE_KPH:
-      self._set_mode('blended')
-      return
+    #if self._sng_state == SNG_State.going and self._v_cruise_kph >= V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
 
     # when standstill: blended
-    # in case of lead car suddenly move away under traffic light, acc mode wont brake at traffic light.
+    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
     if self._has_standstill:
       self._set_mode('blended')
       return
@@ -226,9 +300,9 @@ class DynamicExperimentalController:
       self._set_mode('acc')
       return
 
-    self._set_mode('blended')
+    self._set_mode('acc')
 
-  def _acc_priority_mode(self):
+  def _radar_mode(self):
     # when mpc fcw crash prob is high
     # use blended to slow down quickly
     if self._has_mpc_fcw:
@@ -236,18 +310,18 @@ class DynamicExperimentalController:
       return
 
     # If there is a filtered lead, the vehicle is not in standstill, and the lead vehicle's yRel meets the condition,
-    #if self._has_lead_filtered and not self._has_standstill:
-    #  self._set_mode('acc')
-    #  return
-
-    # when blinker is on and speed is driving below highway cruise speed: blended
-    # we dont want it to switch mode at higher speed, blended may trigger hard brake
-    if self._has_blinkers and self._v_ego_kph < HIGHWAY_CRUISE_KPH:
-      self._set_mode('blended')
+    if self._has_lead_filtered and not self._has_standstill:
+      self._set_mode('acc')
       return
 
+    # when blinker is on and speed is driving below V_ACC_MIN: blended
+    # we dont want it to switch mode at higher speed, blended may trigger hard brake
+    #if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
+
     # when standstill: blended
-    # in case of lead car suddenly move away under traffic light, acc mode wont brake at traffic light.
+    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
     if self._has_standstill:
       self._set_mode('blended')
       return
@@ -270,17 +344,22 @@ class DynamicExperimentalController:
 
     self._set_mode('acc')
 
-  def get_mpc_mode(self, radar_unavailable, car_state, lead_one, md, controls_state, maneuver_distance):
+  def update(self, radar_unavailable, car_state, lead_one, md, controls_state, maneuver_distance):
     if self._is_enabled:
       self._update(car_state, lead_one, md, controls_state, maneuver_distance)
       if radar_unavailable:
-        self._blended_priority_mode()
+        self._radarless_mode()
       else:
-        self._acc_priority_mode()
-
+        self._radar_mode()
+    self._mode_changed = self._mode != self._mode_prev
     self._mode_prev = self._mode
+
+  def get_mpc_mode(self):
     return self._mode
 
+  def has_changed(self):
+    return self._mode_changed
+
   def set_enabled(self, enabled):
     self._is_enabled = enabled
 
@@ -297,4 +376,4 @@ class DynamicExperimentalController:
         self._set_mode_timeout = SET_MODE_TIMEOUT
 
     if self._set_mode_timeout > 0:
-      self._set_mode_timeout -= 1
+      self._set_mode_timeout -= 1

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

@@ -191,6 +191,14 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
   toyotaAutoUnlock->setConfirmation(true, false);
   addItem(toyotaAutoUnlock);
 
+  auto toyotaDriveMode = new ParamControlSP(
+    "ToyotaDriveMode",
+    tr("Enable Toyota Drive Mode Button"),
+    tr("Sunnypilot will link the Acceleration Personality to the car's physical drive mode selector.\nReboot Required."),
+    "../assets/offroad/icon_blank.png");
+  toyotaDriveMode->setConfirmation(true, false);
+  addItem(toyotaDriveMode);
+
   // Volkswagen
   addItem(new LabelControlSP(tr("Volkswagen")));
   auto volkswagenCCOnly = new ParamControlSP(

selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.cc

@@ -320,8 +320,8 @@ void AnnotatedCameraWidgetSP::updateState(const UIStateSP &s) {
   // TODO: Add toggle variables to cereal, and parse from cereal
   longitudinalPersonality = s.scene.longitudinal_personality;
   dynamicLaneProfile = s.scene.dynamic_lane_profile;
-  mpcMode = QString::fromStdString(lp_sp.getE2eBlended());
-  mpcMode = (mpcMode == "blended") ? mpcMode.replace(0, 1, mpcMode[0].toUpper()) : mpcMode.toUpper();
+  const auto mpc_source = lp_sp.getMpcSource();
+  mpcSource = mpc_source == cereal::MpcSource::BLENDED ? QString(tr("blended")) : QString(tr("acc"));
 
   static int reverse_delay = 0;
   bool reverse_allowed = false;
@@ -1061,7 +1061,7 @@ void AnnotatedCameraWidgetSP::drawFeatureStatusText(QPainter &p, int x, int y) {
     p.setBrush(dec_color);
     p.drawEllipse(dec_btn);
     QString dec_status_text;
-    dec_status_text.sprintf("DEC: %s\n", dynamicExperimentalControlToggle ? (experimentalMode ? QString(mpcMode).toStdString().c_str() : QString("Inactive").toStdString().c_str()) : "OFF");
+    dec_status_text.sprintf("DEC: %s\n", dynamicExperimentalControlToggle ? (experimentalMode ? QString(mpcSource).toStdString().c_str() : QString("Inactive").toStdString().c_str()) : "OFF");
     p.setPen(QPen(shadow_color, 2));
     p.drawText(x + drop_shadow_size, y + drop_shadow_size, dec_status_text);
     p.setPen(QPen(text_color, 2));
@@ -1177,6 +1177,10 @@ void AnnotatedCameraWidgetSP::drawLaneLines(QPainter &painter, const UIStateSP *
 
   // paint path
   QLinearGradient bg(0, height(), 0, 0);
+  const auto long_plan_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP();
+  bool exp_mode_path = (long_plan_sp.getDynamicExperimentalControl() && long_plan_sp.getMpcSource() == cereal::MpcSource::BLENDED) ||
+                       (!long_plan_sp.getDynamicExperimentalControl() && sm["controlsState"].getControlsState().getExperimentalMode());
+
   if (madsEnabled || car_state.getCruiseState().getEnabled()) {
     if (steerOverride && latActive) {
       bg.setColorAt(0.0, QColor::fromHslF(20 / 360., 0.94, 0.51, 0.17));
@@ -1185,7 +1189,7 @@ void AnnotatedCameraWidgetSP::drawLaneLines(QPainter &painter, const UIStateSP *
     } else if (!(latActive || car_state.getCruiseState().getEnabled())) {
       bg.setColorAt(0, whiteColor());
       bg.setColorAt(1, whiteColor(0));
-    } else if (sm["controlsState"].getControlsState().getExperimentalMode()) {
+    } else if (exp_mode_path) {
       // The first half of track_vertices are the points for the right side of the path
       const auto &acceleration = sm["modelV2"].getModelV2().getAcceleration().getX();
       const int max_len = std::min<int>(scene.track_vertices.length() / 2, acceleration.size());
@@ -1286,46 +1290,25 @@ void AnnotatedCameraWidgetSP::drawDriverState(QPainter &painter, const UIStateSP
 }
 
 void AnnotatedCameraWidgetSP::rocketFuel(QPainter &p) {
+  UIState *s = uiState();
+  float accel = (*s->sm)["carControl"].getCarControl().getActuators().getAccel();
+  int widgetHeight = rect().height();
+  float halfHeightAbs = std::abs(accel) * widgetHeight / 2.0f;
+  const float scannerWidth = 15;
+  QRect scannerRect;
 
-  static const int ct_n = 1;
-  static float ct;
-
-  int rect_w = rect().width();
-  int rect_h = rect().height();
-
-  const int n = 15 + 1; //Add one off screen due to timing issues
-  static float t[n];
-  int dim_n = (sin(ct / 5) + 1) * (n - 0.01);
-  t[dim_n] = 1.0;
-  t[(int)(ct/ct_n)] = 1.0;
-
-  UIStateSP *s = uiStateSP();
-  float vc_accel0 = (*s->sm)["carState"].getCarState().getAEgo();
-  static float vc_accel;
-  vc_accel = vc_accel + (vc_accel0 - vc_accel) / 5;
-  float hha = 0;
-  if (vc_accel > 0) {
-    hha = 0.85 - 0.1 / vc_accel;  // only extend up to 85%
+  if (accel > 0) {
     p.setBrush(QColor(0, 245, 0, 200));
-  }
-  if (vc_accel < 0) {
-    hha = 0.85 + 0.1 / vc_accel; // only extend up to 85%
-    p.setBrush(QColor(245, 0, 0, 200));
-  }
-  if (hha < 0) {
-    hha = 0;
-  }
-  hha = hha * rect_h;
-  float wp = 28;
-  if (vc_accel > 0) {
-    QRect ra = QRect(rect_w - wp, rect_h / 2 - hha / 2, wp, hha / 2);
-    p.drawRect(ra);
+    scannerRect = QRect(0, widgetHeight / 2 - halfHeightAbs, scannerWidth, halfHeightAbs);
   } else {
-    QRect ra = QRect(rect_w - wp, rect_h / 2, wp, hha / 2);
-    p.drawRect(ra);
+    p.setBrush(QColor(245, 0, 0, 200));
+    scannerRect = QRect(0, widgetHeight / 2, scannerWidth, halfHeightAbs);
   }
+
+  p.drawRect(scannerRect);
 }
 
+
 void AnnotatedCameraWidgetSP::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd,
                                      int num, const cereal::CarState::Reader &car_data, int chevron_data) {
   painter.save();

selfdrive/ui/sunnypilot/qt/onroad/annotated_camera.h

@@ -192,7 +192,7 @@ private:
   cereal::LongitudinalPlanSP::SpeedLimitControlState slcState;
   int longitudinalPersonality;
   int dynamicLaneProfile;
-  QString mpcMode;
+  QString mpcSource;
 
   int speed_limit_frame;
   bool slcShowSign = true;

selfdrive/ui/ui.cc

@@ -326,7 +326,7 @@ void Device::resetInteractiveTimeout(int timeout) {
 
 void Device::updateBrightness(const UIState &s) {
   clipped_brightness = offroad_brightness;
-  if (s.scene.started && s.scene.light_sensor > 0) {
+  if (s.scene.started && s.scene.light_sensor >= 0) {
     clipped_brightness = s.scene.light_sensor;
 
     // CIE 1931 - https://www.photonstophotos.net/GeneralTopics/Exposure/Psychometric_Lightness_and_Gamma.htm
@@ -337,7 +337,7 @@ void Device::updateBrightness(const UIState &s) {
     }
 
     // Scale back to 10% to 100%
-    clipped_brightness = std::clamp(100.0f * clipped_brightness, 10.0f, 100.0f);
+    clipped_brightness = std::clamp(100.0f * clipped_brightness, 5.0f, 100.0f);
   }
   RETURN_IF_SUNNYPILOT
   
@@ -377,4 +377,4 @@ Device *device() {
   static Device _device;
   return &_device;
 }
-#endif
+#endif

system/athena/sunnylinkd.py

@@ -45,7 +45,7 @@ def handle_long_poll(ws: WebSocket, exit_event: threading.Event | None) -> None:
     threading.Thread(target=ws_ping, args=(ws, end_event), name='ws_ping'),
     threading.Thread(target=ws_queue, args=(end_event,), name='ws_queue'),
     # threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler'),
-    # threading.Thread(target=sunny_log_handler, args=(end_event, comma_prime_cellular_end_event), name='log_handler'),
+    threading.Thread(target=sunny_log_handler, args=(end_event, comma_prime_cellular_end_event), name='log_handler'),
     # threading.Thread(target=stat_handler, args=(end_event,), name='stat_handler'),
   ] + [
     threading.Thread(target=jsonrpc_handler, args=(end_event,), name=f'worker_{x}')

system/manager/manager.py

@@ -106,6 +106,7 @@ def manager_init() -> None:
     ("TorqueMaxLatAccel", "250"),
     ("ToyotaAutoLockBySpeed", "0"),
     ("ToyotaAutoUnlockByShifter", "0"),
+    ("ToyotaDriveMode", "0"),
     ("ToyotaEnhancedBsm", "0"),
     ("TrueVEgoUi", "0"),
     ("TurnSpeedControl", "0"),

system/sentry.py

@@ -19,9 +19,9 @@ from openpilot.system.version import get_build_metadata, get_version
 
 class SentryProject(Enum):
   # python project
-  SELFDRIVE = "https://7e3be9bfcfe04c9abe58bd25fe290d1a@o1138119.ingest.sentry.io/6191481"
+  SELFDRIVE = "https://3e25e235efe389b74cde72fb0e115aef@o1138119.ingest.us.sentry.io/4509950385979397"
   # native project
-  SELFDRIVE_NATIVE = "https://7e3be9bfcfe04c9abe58bd25fe290d1a@o1138119.ingest.sentry.io/6191481"
+  SELFDRIVE_NATIVE = "https://3e25e235efe389b74cde72fb0e115aef@o1138119.ingest.us.sentry.io/4509950385979397"
 
 
 CRASHES_DIR = Paths.community_crash_root()