curve/torque bar for configured curature based cars

cereal/custom.capnp

@@ -454,7 +454,8 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
   }
 }
 
-struct CustomReserved10 @0xcb9fd56c7057593a {
+struct ControllerStateBP @0xcb9fd56c7057593a {
+  lateralUncertainty @0 :Float32; #BluePilot
 }
 
 struct CustomReserved11 @0xc2243c65e0340384 {

cereal/log.capnp

@@ -2635,7 +2635,7 @@ struct Event {
     carStateSP @114 :Custom.CarStateSP;
     liveMapDataSP @115 :Custom.LiveMapDataSP;
     modelDataV2SP @116 :Custom.ModelDataV2SP;
-    customReserved10 @136 :Custom.CustomReserved10;
+    controllerStateBP @136 :Custom.ControllerStateBP;
     customReserved11 @137 :Custom.CustomReserved11;
     customReserved12 @138 :Custom.CustomReserved12;
     customReserved13 @139 :Custom.CustomReserved13;

cereal/services.py

@@ -102,6 +102,9 @@ def __init__(self, should_log: bool, frequency: float, decimation: Optional[int]
   "modelDataV2SP": (True, 20., None, QueueSize.BIG),
   "liveLocationKalman": (True, 20.),
 
+  # bluepilot
+  "controllerStateBP": (True, 100., 10),
+
   # debug
   "uiDebug": (True, 0., 1),
   "testJoystick": (True, 0.),

opendbc_repo/opendbc/car/ford/carcontroller.py

@@ -52,22 +52,31 @@ def anti_overshoot(apply_curvature, apply_curvature_last, v_ego):
   return float(np.interp(v_ego, [5, 10], [apply_curvature, output_curvature]))
 
 def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw, steering_angle, lat_active, CP):
+  max_curvature = 1 # large initial value
   # No blending at low speed due to lack of torque wind-up and inaccurate current curvature
   if v_ego_raw > 9:
     apply_curvature = np.clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR,
                               current_curvature + CarControllerParams.CURVATURE_ERROR)
+    max_curvature = abs(current_curvature) + CarControllerParams.CURVATURE_ERROR
 
   # Curvature rate limit after driver torque limit
   apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS)
 
+  steer_up = apply_curvature_last * apply_curvature >= 0. and abs(apply_curvature) > abs(apply_curvature_last)
+  rate_limits = CarControllerParams.ANGLE_LIMITS.ANGLE_RATE_LIMIT_UP if steer_up else CarControllerParams.ANGLE_LIMITS.ANGLE_RATE_LIMIT_DOWN
+  std_steer_angle_rate_limit = np.interp(v_ego_raw, rate_limits[0], rate_limits[1])
+  std_steer_angle_limit = abs(apply_curvature_last) + abs(std_steer_angle_rate_limit)
+  max_curvature = np.minimum(max_curvature, std_steer_angle_limit)
+
   # Ford Q4/CAN FD has more torque available compared to Q3/CAN so we limit it based on lateral acceleration.
   # Safety is not aware of the road roll so we subtract a conservative amount at all times
   if CP.flags & FordFlags.CANFD:
     # Limit curvature to conservative max lateral acceleration
     curvature_accel_limit = MAX_LATERAL_ACCEL / (max(v_ego_raw, 1) ** 2)
     apply_curvature = float(np.clip(apply_curvature, -curvature_accel_limit, curvature_accel_limit))
+    max_curvature = np.minimum(max_curvature, abs(curvature_accel_limit))
 
-  return apply_curvature
+  return apply_curvature, max_curvature
 
 
 def apply_creep_compensation(accel: float, v_ego: float) -> float:
@@ -104,9 +113,9 @@ def __init__(self, dbc_names, CP, CP_SP):
     self.accel_pitch_compensated = 0.0
     self.steering_wheel_delta_adjusted = 0.0
     self.last_button_frame = 0  # Track last ICBM button press frame
+    self.lateralUncertainty = 0.0
 
-   ################################## lateral control parameters ##############################################
-
+    ################################## lateral control parameters ##############################################
 
     # Toggles
     self.enable_human_turn_detection = True
@@ -306,6 +315,10 @@ def handle_post_lane_change_transition(self, path_angle, path_offset, desired_cu
 
     return (path_angle, path_offset, desired_curvature_rate)
 
+  def calculate_lateral_uncertainty(self, requested_curvature, apply_curvature, max_curvature):
+    max_curvature = np.clip(max_curvature, apply_curvature, self.curvature_max)  # ensure max_curvature is within reasonable bounds
+    return float(requested_curvature / max_curvature)
+
   def update(self, CC, CC_SP, CS, now_nanos):
     can_sends = []
     self.sm.update(0)
@@ -471,13 +484,14 @@ def update(self, CC, CC_SP, CS, now_nanos):
           requested_curvature = 0.0
 
         # apply curvature limits
-        apply_curvature = apply_ford_curvature_limits(requested_curvature,
+        apply_curvature, max_curvature = apply_ford_curvature_limits(requested_curvature,
                                                                 self.apply_curvature_last,
                                                                 current_curvature,
                                                                 CS.out.vEgoRaw,
                                                                 0,
                                                                 CC.latActive,
                                                                 self.CP)
+        lateralUncertainty = self.calculate_lateral_uncertainty(requested_curvature, apply_curvature, max_curvature)
 
         #if reset_steering is 1, set apply_curvature to 0
         if reset_steering == 1:
@@ -660,12 +674,14 @@ def update(self, CC, CC_SP, CS, now_nanos):
 
           current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
 
-          self.apply_curvature_last = apply_ford_curvature_limits(apply_curvature, self.apply_curvature_last, current_curvature,
+          self.apply_curvature_last, max_curvature = apply_ford_curvature_limits(apply_curvature, self.apply_curvature_last, current_curvature,
                                                               CS.out.vEgoRaw, 0., CC.latActive, self.CP)
 
           #rem bluepilot sends apply_curvature, and at some point openpilot swapped to sending apply_curvature_last.
           apply_curvature = self.apply_curvature_last
 
+          lateralUncertainty = self.calculate_lateral_uncertainty(requested_curvature, apply_curvature, max_curvature)
+
         # reset steering by setting all values to 0 and ramp_type to immediate
         if reset_steering == 1:
           ramp_type = 3
@@ -683,7 +699,9 @@ def update(self, CC, CC_SP, CS, now_nanos):
         self.HC_PID_controller.reset()
         self.LC_PID_controller.reset()
         ramp_type = 0
+        lateralUncertainty = 0.0
 
+      self.lateralUncertainty = lateralUncertainty
       self.apply_curvature_last = apply_curvature
       self.curvature_rate_last = desired_curvature_rate
       self.path_offset_last = path_offset

opendbc_repo/opendbc/car/structs.py

@@ -170,3 +170,7 @@ class ParamType(StrEnum):
 @auto_dataclass
 class CarStateSP:
   speedLimit: float = auto_field()
+
+@auto_dataclass
+class ControllerStateBP:
+  lateralUncertainty: float = auto_field()

selfdrive/car/card.py

@@ -72,7 +72,7 @@ class Car:
   def __init__(self, CI=None, RI=None) -> None:
     self.can_sock = messaging.sub_sock('can', timeout=20)
     self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP', 'longitudinalPlanSP'])
-    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP', 'carStateSP'])
+    self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP', 'carStateSP', 'controllerStateBP'])
 
     self.can_rcv_cum_timeout_counter = 0
 
@@ -289,6 +289,16 @@ def controls_update(self, CS: car.CarState, CC: car.CarControl, CC_SP: custom.Ca
 
       self.CC_prev = CC
 
+    if hasattr(self.CI.CC, "lateralUncertainty"):
+      cs_bp = structs.ControllerStateBP()
+      cs_bp.lateralUncertainty = self.CI.CC.lateralUncertainty
+      cs_bp_capnp = convert_to_capnp(cs_bp)
+      cs_bp_send = messaging.new_message('controllerStateBP')
+      cs_bp_send.valid = True
+      cs_bp_send.controllerStateBP = cs_bp_capnp
+      self.pm.send('controllerStateBP', cs_bp_send)
+
+
   def step(self):
     CS, CS_SP, RD = self.state_update()
 

selfdrive/car/helpers.py

@@ -35,13 +35,15 @@ def asdictref(obj) -> dict[str, Any]:
   return _asdictref_inner(obj)
 
 
-def convert_to_capnp(struct: structs.CarParamsSP | structs.CarStateSP) -> capnp.lib.capnp._DynamicStructBuilder:
+def convert_to_capnp(struct: structs.CarParamsSP | structs.CarStateSP | structs.ControllerStateBP) -> capnp.lib.capnp._DynamicStructBuilder:
   struct_dict = asdictref(struct)
 
   if isinstance(struct, structs.CarParamsSP):
     struct_capnp = custom.CarParamsSP.new_message(**struct_dict)
   elif isinstance(struct, structs.CarStateSP):
     struct_capnp = custom.CarStateSP.new_message(**struct_dict)
+  elif isinstance(struct, structs.ControllerStateBP):
+    struct_capnp = custom.ControllerStateBP.new_message(**struct_dict)
   else:
     raise ValueError(f"Unsupported struct type: {type(struct)}")
 

selfdrive/ui/mici/onroad/hud_renderer.py

@@ -172,8 +172,7 @@ def _update_state(self) -> None:
   def _render(self, rect: rl.Rectangle) -> None:
     """Render HUD elements to the screen."""
 
-    if ui_state.sm['controlsState'].lateralControlState.which() != 'angleState':
-      self._torque_bar.render(rect)
+    self._torque_bar.render(rect)
 
     if self.is_cruise_set:
       self._draw_set_speed(rect)

selfdrive/ui/mici/onroad/torque_bar.py

@@ -2,6 +2,7 @@
 import time
 from functools import wraps
 from collections import OrderedDict
+import cereal.messaging as messaging
 
 import numpy as np
 import pyray as rl
@@ -18,7 +19,6 @@
 
 DEBUG = False
 
-
 def quantized_lru_cache(maxsize=128):
   def decorator(func):
     cache = OrderedDict()
@@ -162,23 +162,32 @@ def _update_state(self):
 
     # torque line
     if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState':
-      controls_state = ui_state.sm['controlsState']
-      car_state = ui_state.sm['carState']
-      live_parameters = ui_state.sm['liveParameters']
-      lateral_acceleration = controls_state.curvature * car_state.vEgo ** 2 - live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY
-      # TODO: pull from carparams
-      max_lateral_acceleration = 3
-
-      # from selfdrived
-      actual_lateral_accel = controls_state.curvature * car_state.vEgo ** 2
-      desired_lateral_accel = controls_state.desiredCurvature * car_state.vEgo ** 2
-      accel_diff = (desired_lateral_accel - actual_lateral_accel)
-
-      self._torque_filter.update(min(max(lateral_acceleration / max_lateral_acceleration + accel_diff, -1), 1))
+      if ui_state.sm.updated["controllerStateBP"]:
+        ctrlr_state = ui_state.sm['controllerStateBP']
+        self._torque_filter.update(min(max(ctrlr_state.lateralUncertainty, -1.2), 1.2))
+      else:
+        #incomplete implementation?
+        controls_state = ui_state.sm['controlsState']
+        car_state = ui_state.sm['carState']
+        live_parameters = ui_state.sm['liveParameters']
+        lateral_acceleration = controls_state.curvature * car_state.vEgo ** 2 - live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY
+        # TODO: pull from carparams
+        max_lateral_acceleration = 3
+
+        # from selfdrived
+        actual_lateral_accel = controls_state.curvature * car_state.vEgo ** 2
+        desired_lateral_accel = controls_state.desiredCurvature * car_state.vEgo ** 2
+        accel_diff = (desired_lateral_accel - actual_lateral_accel)
+
+        self._torque_filter.update(min(max(lateral_acceleration / max_lateral_acceleration + accel_diff, -1), 1))
+
     else:
       self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque)
 
   def _render(self, rect: rl.Rectangle) -> None:
+    if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState' and not ui_state.sm.updated["controllerStateBP"]:
+      return
+
     # adjust y pos with torque
     torque_line_offset = np.interp(abs(self._torque_filter.x), [0.5, 1], [22, 26])
     torque_line_height = np.interp(abs(self._torque_filter.x), [0.5, 1], [14, 56])

selfdrive/ui/sunnypilot/ui_state.py

@@ -17,7 +17,7 @@ def __init__(self):
     self.params = Params()
     self.sm_services_ext = [
       "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
-      "gpsLocation", "liveTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "liveDelay"
+      "gpsLocation", "liveTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "liveDelay", "controllerStateBP"
     ]
 
     self.sunnylink_state = SunnylinkState()