diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 5c0a004fa6..dda9369dee 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -454,7 +454,8 @@ struct ModelDataV2SP @0xa1680744031fdb2d { } } -struct CustomReserved10 @0xcb9fd56c7057593a { +struct ControllerStateBP @0xcb9fd56c7057593a { + lateralUncertainty @0 :Float32; #BluePilot } struct CustomReserved11 @0xc2243c65e0340384 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 55a47cff0a..c5801a870f 100644 --- a/cereal/log.capnp +++ b/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; diff --git a/cereal/services.py b/cereal/services.py index a2fd5f7dce..0b8558fdf7 100755 --- a/cereal/services.py +++ b/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.), diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 8ab76d3390..896b696b85 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/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 diff --git a/opendbc_repo/opendbc/car/structs.py b/opendbc_repo/opendbc/car/structs.py index 7fd7623382..45b5cc44fe 100644 --- a/opendbc_repo/opendbc/car/structs.py +++ b/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() \ No newline at end of file diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 6758432b68..420301d013 100755 --- a/selfdrive/car/card.py +++ b/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() diff --git a/selfdrive/car/helpers.py b/selfdrive/car/helpers.py index 384152c71b..3c0cb8d35a 100644 --- a/selfdrive/car/helpers.py +++ b/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)}") diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index 7f489ccf98..ab02b9c218 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/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) diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index c8485a3101..bb8bc07d9b 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/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]) diff --git a/selfdrive/ui/sunnypilot/ui_state.py b/selfdrive/ui/sunnypilot/ui_state.py index ca8125512a..d81a7093e3 100644 --- a/selfdrive/ui/sunnypilot/ui_state.py +++ b/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()