diff --git a/common/params_keys.h b/common/params_keys.h index d2adbc03b..c7c2e2005 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -547,6 +547,9 @@ inline static std::unordered_map keys = { {"RelaxedJerkSpeed", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, {"RelaxedJerkSpeedDecrease", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, {"ReverseCruise", {PERSISTENT, BOOL, "0", "0", 1}}, + {"RivianAngleControl", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}}, + {"RivianAngleSaturated", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL, "0", "0"}}, + {"RivianToiRecoveryFailed", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL, "0", "0"}}, {"RecoveryPower", {PERSISTENT, FLOAT, "1.0", "1.0", 2}}, {"RoadEdgesWidth", {PERSISTENT, FLOAT, "2.0", "2.0", 2, SETTINGS_SIMPLE}}, {"RoadNameUI", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}}, diff --git a/opendbc_repo/opendbc/car/car.capnp b/opendbc_repo/opendbc/car/car.capnp index d2b8d758a..6ea415bb9 100644 --- a/opendbc_repo/opendbc/car/car.capnp +++ b/opendbc_repo/opendbc/car/car.capnp @@ -374,6 +374,7 @@ struct CarControl { brake @1: Float32; # [0.0, 1.0] torqueOutputCan @8: Float32; # value sent over can to the car speed @6: Float32; # m/s + lateralControlMode @9: LateralControlMode; enum LongControlState @0xe40f3a917d908282{ off @0; @@ -381,6 +382,13 @@ struct CarControl { stopping @2; starting @3; } + + enum LateralControlMode { + inactive @0; + torque @1; + angle @2; + torqueRecovering @3; + } } struct CruiseControl { @@ -510,6 +518,7 @@ struct CarParams { startingState @70 :Bool; # Does this car make use of special starting state steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds + lateralSmoothSeconds @78 :Float32; # Speed-scheduled curvature smoothing used by select angle-control platforms longitudinalActuatorDelay @58 :Float32; # Gas/Brake actuator delay in seconds openpilotLongitudinalControl @37 :Bool; # is openpilot doing the longitudinal control? carVin @38 :Text; # VIN number queried during fingerprinting diff --git a/opendbc_repo/opendbc/car/docs_definitions.py b/opendbc_repo/opendbc/car/docs_definitions.py index 53d0c3191..e7ec748cd 100644 --- a/opendbc_repo/opendbc/car/docs_definitions.py +++ b/opendbc_repo/opendbc/car/docs_definitions.py @@ -142,6 +142,7 @@ class CarHarness(EnumBase): ford_q3 = BaseCarHarness("Ford Q3 connector") ford_q4 = BaseCarHarness("Ford Q4 connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler]) rivian = BaseCarHarness("Rivian A connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler]) + rivian_b = BaseCarHarness("Rivian B connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler]) tesla_a = BaseCarHarness("Tesla A connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler]) tesla_b = BaseCarHarness("Tesla B connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler]) psa_a = BaseCarHarness("PSA A connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler]) diff --git a/opendbc_repo/opendbc/car/rivian/carcontroller.py b/opendbc_repo/opendbc/car/rivian/carcontroller.py index 41d9cce9c..c1b1c1bd7 100644 --- a/opendbc_repo/opendbc/car/rivian/carcontroller.py +++ b/opendbc_repo/opendbc/car/rivian/carcontroller.py @@ -1,10 +1,28 @@ import numpy as np from opendbc.can import CANPacker -from opendbc.car import Bus +from opendbc.car import Bus, structs from opendbc.car.lateral import apply_driver_steer_torque_limits from opendbc.car.interfaces import CarControllerBase -from opendbc.car.rivian.riviancan import create_lka_steering, create_longitudinal, create_wheel_touch, create_adas_status -from opendbc.car.rivian.values import CarControllerParams +from opendbc.car.rivian.ext_controller import HIGH_ANGLE_CAP_FRAC, HIGH_ANGLE_THRESHOLD_DEG, ExternalController +from opendbc.car.rivian.riviancan import (create_acm_status, create_adas_status, create_angle_steering, + create_lka_steering, create_longitudinal, create_wheel_touch) +from opendbc.car.rivian.toi_controller import TOI_MAX_ANGLE_DEG, ToiController +from opendbc.car.rivian.values import CarControllerParams, RivianFlags + +GearShifter = structs.CarState.GearShifter +LateralControlMode = structs.CarControl.Actuators.LateralControlMode + + +def get_longitudinal_accel(requested_accel: float, gas_pressed: bool, long_active: bool = False, + v_ego: float = 0.0) -> float: + # Keep Rivian's command stream continuous when Panda's gas safety check becomes active. + if gas_pressed: + return 0.0 + + accel = requested_accel + if long_active: + accel += float(np.interp(v_ego, CarControllerParams.ACCEL_FF_DRAG_BP, CarControllerParams.ACCEL_FF_DRAG_V)) + return float(np.clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)) class CarController(CarControllerBase): @@ -14,29 +32,104 @@ class CarController(CarControllerBase): self.packer = CANPacker(dbc_names[Bus.pt]) self.cancel_frames = 0 + self.toi_controller = ToiController() + self.angle_harness = bool(CP.flags & RivianFlags.ANGLE_HARNESS) + self.ext_controller = ExternalController(CP) if self.angle_harness else None + self.angle_saturation_last = None + self.angle_saturation_params = None + self.toi_recovery_failed_last = None + self.toi_recovery_params = None + try: + # Keep opendbc importable standalone while exposing controller-only state + # to selfdrived's existing car-specific warning bridge. + from openpilot.common.params import Params + status_params = Params(memory=True) + self.toi_recovery_params = status_params + if self.angle_harness: + self.angle_saturation_params = status_params + except Exception: + pass + + def _publish_angle_saturation(self) -> None: + params = getattr(self, "angle_saturation_params", None) + if params is None: + return + + saturated = bool(self.ext_controller.angle_saturated) + if saturated != getattr(self, "angle_saturation_last", None): + put_bool = getattr(params, "put_bool_nonblocking", None) or params.put_bool + put_bool("RivianAngleSaturated", saturated) + self.angle_saturation_last = saturated + + def _publish_toi_recovery_failed(self) -> None: + params = getattr(self, "toi_recovery_params", None) + if params is None: + return + + toi_controller = self.ext_controller.toi_controller if self.angle_harness else self.toi_controller + failed = bool(toi_controller.recovery_failed) + if failed != getattr(self, "toi_recovery_failed_last", None): + put_bool = getattr(params, "put_bool_nonblocking", None) or params.put_bool + put_bool("RivianToiRecoveryFailed", failed) + self.toi_recovery_failed_last = failed + + def update_live_params(self, roll, angle_offset_deg, stiffness_factor, steer_ratio): + if self.ext_controller is not None: + self.ext_controller.roll = roll + self.ext_controller.angle_offset_deg = angle_offset_deg + self.ext_controller.VM.update_params(max(stiffness_factor, 0.1), max(steer_ratio, 0.1)) def update(self, CC, CS, now_nanos, starpilot_toggles): actuators = CC.actuators can_sends = [] + lat_active = CC.latActive and CS.out.gearShifter == GearShifter.drive apply_torque = 0 + torque_request = False steer_max = round(float(np.interp(CS.out.vEgoRaw, CarControllerParams.STEER_MAX_LOOKUP[0], CarControllerParams.STEER_MAX_LOOKUP[1]))) - if CC.latActive: - new_torque = int(round(CC.actuators.torque * steer_max)) - apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, - CS.out.steeringTorque, CarControllerParams, steer_max) + if self.angle_harness: + # The Panda permission is capability-gated at fingerprint time; this + # runtime setting only selects which already-safe channel is active. + self.ext_controller.force_torque = not bool(getattr(starpilot_toggles, "rivian_angle_control", False)) + self.ext_controller.update(CS, lat_active, actuators) + self._publish_angle_saturation() + apply_torque = self.ext_controller.torque_cmd + torque_request = self.ext_controller.toi_act_cmd + else: + torque_request, torque_allowed = self.toi_controller.update( + lat_active, + abs(CS.out.steeringAngleDeg) >= TOI_MAX_ANGLE_DEG, + bool(getattr(CS, "toi_fault", False)), + bool(getattr(CS, "toi_active", False)), + bool(getattr(CS, "toi_unavailable", False)), + ) + + if lat_active and torque_allowed: + new_torque = int(round(CC.actuators.torque * steer_max)) + apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, + CS.out.steeringTorque, CarControllerParams, steer_max) + if abs(CS.out.steeringAngleDeg) > HIGH_ANGLE_THRESHOLD_DEG: + cap = int(round(steer_max * HIGH_ANGLE_CAP_FRAC)) + apply_torque = int(np.clip(apply_torque, -cap, cap)) - # send steering command self.apply_torque_last = apply_torque - can_sends.append(create_lka_steering(self.packer, self.frame, CS.acm_lka_hba_cmd, apply_torque, CC.enabled, CC.latActive)) + self._publish_toi_recovery_failed() + can_sends.append(create_lka_steering(self.packer, self.frame, CS.acm_lka_hba_cmd, + apply_torque, CC.enabled, torque_request)) - if self.frame % 5 == 0: - can_sends.append(create_wheel_touch(self.packer, CS.sccm_wheel_touch, CC.enabled)) + if self.angle_harness: + can_sends.append(create_angle_steering(self.packer, self.frame, self.ext_controller.apply_angle_last, + self.ext_controller.angle_active)) + feature_status = (1 if self.ext_controller.torque_active else 2) if lat_active else 0 + can_sends.append(create_acm_status(self.packer, self.frame, feature_status)) + + if self.frame % 5 == 0 and not (self.CP.flags & RivianFlags.GEN2): + can_sends.append(create_wheel_touch(self.packer, CS.sccm_wheel_touch, lat_active if self.angle_harness else CC.enabled)) # Longitudinal control if self.CP.openpilotLongitudinalControl: - accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)) + accel = get_longitudinal_accel(actuators.accel, CS.out.gasPressed, CC.longActive, CS.out.vEgo) can_sends.append(create_longitudinal(self.packer, self.frame, accel, CC.enabled)) else: interface_status = None @@ -48,11 +141,27 @@ class CarController(CarControllerBase): else: self.cancel_frames = 0 - can_sends.append(create_adas_status(self.packer, CS.vdm_adas_status, interface_status)) + for msg in CS.vdm_adas_status: + can_sends.append(create_adas_status(self.packer, msg, interface_status)) new_actuators = actuators.as_builder() + # Always report the actual applied torque. In angle mode this is zero, which + # deliberately freezes the torque PID integrator instead of letting it wind + # up and dump saturated torque into the first cooperative handoff. new_actuators.torque = apply_torque / steer_max new_actuators.torqueOutputCan = apply_torque + lateral_mode = LateralControlMode.inactive + if self.angle_harness: + new_actuators.steeringAngleDeg = self.ext_controller.apply_angle_last + if lat_active and self.ext_controller.toi_controller.recovering: + lateral_mode = LateralControlMode.torqueRecovering + elif lat_active and (self.ext_controller.torque_active or self.ext_controller.torque_prearm): + lateral_mode = LateralControlMode.torque + elif lat_active and self.ext_controller.angle_active: + lateral_mode = LateralControlMode.angle + elif lat_active: + lateral_mode = LateralControlMode.torqueRecovering if self.toi_controller.recovering else LateralControlMode.torque + new_actuators.lateralControlMode = lateral_mode self.frame += 1 return new_actuators, can_sends diff --git a/opendbc_repo/opendbc/car/rivian/carstate.py b/opendbc_repo/opendbc/car/rivian/carstate.py index b3ba0e547..41f21d38b 100644 --- a/opendbc_repo/opendbc/car/rivian/carstate.py +++ b/opendbc_repo/opendbc/car/rivian/carstate.py @@ -3,20 +3,36 @@ from cereal import custom from opendbc.can import CANParser from opendbc.car import Bus, structs from opendbc.car.interfaces import CarStateBase -from opendbc.car.rivian.values import DBC, GEAR_MAP +from opendbc.car.rivian.carstate_ext import RivianLongitudinalState +from opendbc.car.rivian.faults import TOI_FAULT_ALERT_FRAMES, get_steering_faults +from opendbc.car.rivian.values import DBC, GEAR_MAP, RivianFlags from opendbc.car.common.conversions import Conversions as CV GearShifter = structs.CarState.GearShifter +def get_cruise_available(flags: int, acm_feature_status: int) -> bool: + # VDM reports unavailable until after stock ACC activates, so it cannot gate + # the PCM enable edge. The ACM reports standby before activation and ACC once active. + return bool(flags & RivianFlags.LONGITUDINAL_HARNESS) or acm_feature_status in (0, 1) + + class CarState(CarStateBase): def __init__(self, CP, FPCP): super().__init__(CP, FPCP) self.last_speed = 30 + self.longitudinal_state = RivianLongitudinalState(CP) - self.acm_lka_hba_cmd = None - self.sccm_wheel_touch = None - self.vdm_adas_status = None + self.acm_lka_hba_cmd: dict | None = None + self.sccm_wheel_touch: dict | None = None + self.vdm_adas_status: list[dict] = [] + self.hands_on_level = 0 + self.eac_status = 0 + self.eac_error_code = 0 + self.toi_fault = False + self.toi_active = False + self.toi_unavailable = False + self.toi_fault_frames = 0 def update(self, can_parsers, starpilot_toggles) -> structs.CarState: cp = can_parsers[Bus.pt] @@ -44,17 +60,30 @@ class CarState(CarStateBase): ret.steeringTorque = cp.vl["EPAS_SystemStatus"]["EPAS_TorsionBarTorque"] ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > 1.0, 5) - ret.steerFaultTemporary = cp.vl["EPAS_AdasStatus"]["EPAS_EacErrorCode"] != 0 + self.eac_error_code = int(cp.vl["EPAS_AdasStatus"]["EPAS_EacErrorCode"]) + self.eac_status = int(cp.vl["EPAS_AdasStatus"]["EPAS_EacStatus"]) + self.hands_on_level = int(cp.vl["EPAS_SystemStatus"]["EPAS_HandsOnLevel"]) + self.toi_fault = cp.vl["EPAS_SystemStatus"]["H_CAN_EPSS_ToiFlt"] != 0 + self.toi_active = cp.vl["EPAS_SystemStatus"]["H_CAN_EPSS_ToiActive"] != 0 + self.toi_unavailable = cp.vl["EPAS_SystemStatus"]["H_CAN_EPS_ToiUnavailable"] != 0 + toi_fault = self.toi_fault or self.toi_unavailable + self.toi_fault_frames = self.toi_fault_frames + 1 if toi_fault else 0 + toi_fault_persistent = self.toi_fault_frames >= TOI_FAULT_ALERT_FRAMES + ret.steerFaultPermanent, ret.steerFaultTemporary, ret.steeringDisengage = get_steering_faults( + bool(self.CP.flags & RivianFlags.ANGLE_HARNESS), toi_fault, toi_fault_persistent, + self.eac_status, self.eac_error_code, + ) # Cruise state speed = min(int(cp_adas.vl["ACM_tsrCmd"]["ACM_tsrSpdDisClsMain"]), 85) self.last_speed = speed if speed != 0 else self.last_speed - ret.cruiseState.enabled = cp_cam.vl["ACM_Status"]["ACM_FeatureStatus"] == 1 + acm_feature_status = int(cp_cam.vl["ACM_Status"]["ACM_FeatureStatus"]) + ret.cruiseState.enabled = acm_feature_status == 1 # TODO: find cruise set speed on CAN ret.cruiseState.speed = self.last_speed * CV.MPH_TO_MS # detected speed limit if not self.CP.openpilotLongitudinalControl: ret.cruiseState.speed = -1 - ret.cruiseState.available = cp.vl["VDM_AdasSts"]["VDM_AdasInterfaceStatus"] in (1, 2) + ret.cruiseState.available = get_cruise_available(self.CP.flags, acm_feature_status) ret.cruiseState.standstill = cp.vl["VDM_AdasSts"]["VDM_AdasVehicleHoldStatus"] == 1 # ACM_Status->ACM_FaultSupervisorState normally 1, appears to go to 3 when either: @@ -71,36 +100,47 @@ class CarState(CarStateBase): # Gear ret.gearShifter = GEAR_MAP.get(int(cp.vl["VDM_PropStatus"]["VDM_Prndl_Status"]), GearShifter.unknown) - # Doors - ret.doorOpen = any(cp_adas.vl["IndicatorLights"][door] != 2 for door in ("RearDriverDoor", "FrontPassengerDoor", "DriverDoor", "RearPassengerDoor")) + # Gen 2 does not publish these signals. Stock ACC handles their disengage + # behavior at standstill, and the doors cannot be opened while driving. + if not (self.CP.flags & RivianFlags.GEN2): + ret.doorOpen = any(cp_adas.vl["IndicatorLights"][door] != 2 for door in ("RearDriverDoor", "FrontPassengerDoor", "DriverDoor", "RearPassengerDoor")) + ret.seatbeltUnlatched = cp.vl["RCM_Status"]["RCM_Status_IND_WARN_BELT_DRIVER"] != 0 # Blinkers ret.leftBlinker = cp_adas.vl["IndicatorLights"]["TurnLightLeft"] in (1, 2) ret.rightBlinker = cp_adas.vl["IndicatorLights"]["TurnLightRight"] in (1, 2) - # Seatbelt - ret.seatbeltUnlatched = cp.vl["RCM_Status"]["RCM_Status_IND_WARN_BELT_DRIVER"] != 0 - - # Blindspot - # ret.leftBlindspot = False - # ret.rightBlindspot = False - # AEB ret.stockAeb = cp_cam.vl["ACM_AebRequest"]["ACM_EnableRequest"] != 0 # Messages needed by carcontroller self.acm_lka_hba_cmd = copy.copy(cp_cam.vl["ACM_lkaHbaCmd"]) - self.sccm_wheel_touch = copy.copy(cp.vl["SCCM_WheelTouch"]) - self.vdm_adas_status = copy.copy(cp.vl["VDM_AdasSts"]) + if not (self.CP.flags & RivianFlags.GEN2): + self.sccm_wheel_touch = copy.copy(cp.vl["SCCM_WheelTouch"]) + # This message can lag and deliver two samples in one parser cycle. Forward + # every sample so cancelling stock ACC remains reliable. + adas_status_msgs = cp.vl_all["VDM_AdasSts"] + self.vdm_adas_status = [dict(zip(adas_status_msgs, vals, strict=True)) + for vals in zip(*adas_status_msgs.values(), strict=True)] + if not self.vdm_adas_status: + self.vdm_adas_status = [copy.copy(cp.vl["VDM_AdasSts"])] + + self.longitudinal_state.update(ret, can_parsers) fp_ret = custom.StarPilotCarState.new_message() return ret, fp_ret + def set_cruise_speed(self, speed: float) -> float: + return self.longitudinal_state.set_cruise_speed(speed) + @staticmethod def get_can_parsers(CP): - return { + parsers = { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 0), Bus.adas: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 1), Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2), } + if CP.flags & RivianFlags.LONGITUDINAL_HARNESS: + parsers[Bus.alt] = CANParser(DBC[CP.carFingerprint][Bus.alt], [], 1) + return parsers diff --git a/opendbc_repo/opendbc/car/rivian/carstate_ext.py b/opendbc_repo/opendbc/car/rivian/carstate_ext.py new file mode 100644 index 000000000..9a16fd4af --- /dev/null +++ b/opendbc_repo/opendbc/car/rivian/carstate_ext.py @@ -0,0 +1,104 @@ +"""Rivian Extreme-harness state support.""" + +import math + +from opendbc.car import Bus, structs +from opendbc.car.common.conversions import Conversions as CV +from opendbc.car.rivian.values import RivianFlags + +ButtonType = structs.CarState.ButtonEvent.Type + +MAX_SET_SPEED = 85 * CV.MPH_TO_MS +MIN_SET_SPEED = 20 * CV.MPH_TO_MS + + +class RivianLongitudinalState: + """State provided by the Extreme harness park-assist CAN bridge.""" + + def __init__(self, CP): + self.CP = CP + self.set_speed = 10 + self.increase_button = False + self.decrease_button = False + self.distance_button = 0 + self.scroll_click_pressed = False + self.increase_counter = 0 + self.decrease_counter = 0 + self.stalk_down_counter = 0 + + def set_cruise_speed(self, speed: float) -> float: + if self.CP.openpilotLongitudinalControl: + self.set_speed = max(MIN_SET_SPEED, min(float(speed), MAX_SET_SPEED)) + return self.set_speed + + def update_longitudinal_upgrade(self, ret: structs.CarState, can_parsers) -> list: + cp_park = can_parsers[Bus.alt] + cp_adas = can_parsers[Bus.adas] + cp = can_parsers[Bus.pt] + button_events = [] + + prev_increase_button = self.increase_button + prev_decrease_button = self.decrease_button + prev_scroll_click_pressed = self.scroll_click_pressed + + if self.CP.openpilotLongitudinalControl: + right_scroll = int(cp_park.vl["WheelButtons_Fwd"]["RightButton_Scroll"]) + if right_scroll != 255: + if self.distance_button != right_scroll: + # Rivian's rotary value changes once per detent. A release-only gap + # event preserves the existing three-profile personality cycling. + button_events.append(structs.CarState.ButtonEvent(pressed=False, type=ButtonType.gapAdjustCruise)) + self.distance_button = right_scroll + + # Scroll-click is a separate two-bit signal from rotary movement. Treat + # it as a held distance button so the existing short/long/very-long + # mappings apply, including Traffic Mode on a very-long hold. + self.scroll_click_pressed = cp_park.vl["WheelButtons_Fwd"]["RightButton_ScrollClick"] == 2 + if self.scroll_click_pressed != prev_scroll_click_pressed: + button_events.append(structs.CarState.ButtonEvent(pressed=self.scroll_click_pressed, type=ButtonType.gapAdjustCruise)) + + self.increase_button = cp_park.vl["WheelButtons_Fwd"]["RightButton_RightClick"] == 2 + self.decrease_button = cp_park.vl["WheelButtons_Fwd"]["RightButton_LeftClick"] == 2 + self.increase_counter = self.increase_counter + 1 if self.increase_button else 0 + self.decrease_counter = self.decrease_counter + 1 if self.decrease_button else 0 + + metric = cp_adas.vl["Cluster"]["Cluster_Unit"] == 0 + conversion = CV.KPH_TO_MS if metric else CV.MPH_TO_MS + long_press_step = 10.0 if metric else 5.0 + set_speed_display = self.set_speed * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH) + + if self.increase_button: + if self.increase_counter % 66 == 0: + self.set_speed = math.ceil((set_speed_display + 1) / long_press_step) * long_press_step * conversion + elif not prev_increase_button: + self.set_speed += conversion + + if self.decrease_button: + if self.decrease_counter % 66 == 0: + self.set_speed = math.floor((set_speed_display - 1) / long_press_step) * long_press_step * conversion + elif not prev_decrease_button: + self.set_speed -= conversion + + if not ret.cruiseState.enabled: + self.set_speed = ret.vEgoCluster + + stalk_down = int(cp.vl["VDM_AdasSts"]["VDM_UserAdasRequest"]) in (3, 4) + self.stalk_down_counter = self.stalk_down_counter + 1 if stalk_down else 0 + if self.stalk_down_counter == 50: + self.set_speed = max(self.set_speed, ret.vEgoCluster) + + self.set_speed = max(MIN_SET_SPEED, min(self.set_speed, MAX_SET_SPEED)) + ret.cruiseState.speed = self.set_speed + + ret.leftBlindspot = cp_park.vl["BSM_BlindSpotIndicator_Fwd"]["BSM_BlindSpotIndicator_Left"] != 0 + ret.rightBlindspot = cp_park.vl["BSM_BlindSpotIndicator_Fwd"]["BSM_BlindSpotIndicator_Right"] != 0 + + return button_events + + def update(self, ret: structs.CarState, can_parsers) -> None: + button_events = [] + + if self.CP.flags & RivianFlags.LONGITUDINAL_HARNESS: + button_events.extend(self.update_longitudinal_upgrade(ret, can_parsers)) + + ret.buttonEvents = button_events diff --git a/opendbc_repo/opendbc/car/rivian/ext_controller.py b/opendbc_repo/opendbc/car/rivian/ext_controller.py new file mode 100644 index 000000000..d7389eeea --- /dev/null +++ b/opendbc_repo/opendbc/car/rivian/ext_controller.py @@ -0,0 +1,432 @@ +"""Rivian Gen 1 hybrid steering for the Extreme harness.""" + +import math +from collections import deque + +import numpy as np + +from opendbc.car import rate_limit +from opendbc.car.common.filter_simple import FirstOrderFilter +from opendbc.car.lateral import apply_driver_steer_torque_limits, get_max_angle_delta_vm, get_max_angle_vm +from opendbc.car.rivian.toi_controller import TOI_MAX_ANGLE_DEG, TOI_REARM_ANGLE_DEG, ToiController +from opendbc.car.rivian.values import CAR, CarControllerParams as CCP, RivianFlags +from opendbc.car.vehicle_model import VehicleModel + +# Limits observed in the Gen 1 EPAS firmware. Margins keep commands inside the +# firmware's absolute-angle and sliding-window rate checks. +EPAS_FW_MAX_ANGLE_BP = [0.0, 2.78, 5.56, 8.33, 12.50, 16.67, 22.22, 27.78] +EPAS_FW_MAX_ANGLE_V = [500, 500, 250, 150, 85, 56, 40, 25] +EPAS_FW_RATE_BP = [5.56, 8.33, 12.50, 16.67] +EPAS_FW_RATE_V = [4.50, 1.50, 0.60, 0.18] +EPAS_FW_ANGLE_MARGIN = 0.98 +EPAS_FW_RATE_MARGIN = 0.94 +PANDA_STEP_MARGIN = 0.9 + +MIN_TORQUE_FRAMES = 50 +HANDOFF_EXIT_DEG = 15.0 +UNWIND_HANDOFF_RATE = 40.0 +HANDOFF_MAX_ANGLE_DEG = 25.0 +DRIVER_HANDS_OFF_EXIT_FRAMES = 100 +EAC_RECOVER_FRAMES = 15 + +EAC_REARM_RELEASE_FRAMES = 25 + +# The wheel's inertia loads the torsion bar while the column is swinging. Reject +# those transients so they cannot be mistaken for a driver override. +TORSION_RATE_WINDOW = 25 +TORSION_MAX_RATE_SWING = 90.0 +DRIVER_OVERRIDE_TORQUE = 6.0 +DRIVER_OVERRIDE_FRAMES = 5 + +# Sustained light torsion bridges capacitive-sensor dropouts while the driver's +# hands slide on the wheel. This only delays recovery from a driver override. +PRESENCE_LPF_RC = 0.032 +PRESENCE_TORQUE_THRESHOLD = 1.5 +PRESENCE_MIN_FRAMES = 30 +PRESENCE_HOLD_FRAMES = 100 + +# Above this angle the rack spends most of its time saturated. Keeping a small +# amount of headroom helps the torque controller recover as geometry unwinds. +HIGH_ANGLE_THRESHOLD_DEG = 90 +HIGH_ANGLE_CAP_FRAC = 0.95 + +# A live angle->torque selection cannot release the EPAS angle servo before the +# rate-limited torque channel is ready to carry the curve. Ramp torque under the +# still-active angle command, then release angle once sufficient hold torque is +# available. These values come from AdventurePilot's road-tested handoff. +TORQUE_PREARM_EXIT_FRAC = 0.85 +TORQUE_PREARM_MIN_HOLD = 20 +TORQUE_PREARM_MAX_FRAMES = 150 +TORQUE_PREARM_STALL_FRAMES = 12 +TORQUE_PREARM_ABORT_LOCKOUT = 50 + +# The torque controller is deliberately frozen while angle steering is active, +# so angle saturation needs its own debounced envelope check for the stock +# "turn exceeds steering limit" warning. +ANGLE_SAT_MIN_LAT_ACCEL = 1.0 +ANGLE_SAT_FRAMES = 30 + + +def apply_rivian_steer_angle_limits_vm(apply_angle: float, apply_angle_last: float, v_ego_raw: float, steering_angle: float, + lat_active: bool, limits, VM: VehicleModel) -> float: + """Apply Rivian's jerk, accel, and safety constraints to its angle channel.""" + v_ego_raw = max(v_ego_raw, 1) + + # When the speed-scheduled angle envelope shrinks, Rivian must unwind toward + # it through the jerk limit instead of snapping directly to the new bound. + max_angle = get_max_angle_vm(v_ego_raw, VM, limits) + new_apply_angle = np.clip(apply_angle, -max_angle, max_angle) + + max_angle_delta = get_max_angle_delta_vm(v_ego_raw, VM, limits) + max_angle_delta = min(max_angle_delta, limits.ANGLE_LIMITS.MAX_ANGLE_RATE) + new_apply_angle = rate_limit(new_apply_angle, apply_angle_last, -max_angle_delta, max_angle_delta) + + if not lat_active: + new_apply_angle = steering_angle + + return float(np.clip(new_apply_angle, -limits.ANGLE_LIMITS.STEER_ANGLE_MAX, limits.ANGLE_LIMITS.STEER_ANGLE_MAX)) + + +class _RateBudget: + WINDOW_USER_FRAMES = 16 + WINDOW_TIME_S = 0.16 + + def __init__(self): + self.history = deque([0.0] * self.WINDOW_USER_FRAMES, maxlen=self.WINDOW_USER_FRAMES) + + def push(self, sent_angle: float) -> None: + self.history.append(round(sent_angle * 10) / 10) + + def bounds(self, threshold_dps: float, margin: float) -> tuple[float, float]: + cmd_oldest = self.history[0] + budget = threshold_dps * self.WINDOW_TIME_S * margin + return cmd_oldest - budget, cmd_oldest + budget + + +def get_safety_CP(): + from opendbc.car.rivian.interface import CarInterface + return CarInterface.get_non_essential_params(CAR.RIVIAN_R1_GEN1) + + +class ExternalController: + """Hybrid Gen 1 steering: EPAS angle control with cooperative torque fallback.""" + + def __init__(self, CP): + self.VM = VehicleModel(CP) + self.VM_safety = VehicleModel(get_safety_CP()) + self.gen2 = bool(CP.flags & RivianFlags.GEN2) + + self.wheel_touch_cnt = 0 + self.torsion_cnt = 0 + self.torsion_sign = 0 + self.driver_override_cnt = 0 + self.rate_hist = deque([0.0] * TORSION_RATE_WINDOW, maxlen=TORSION_RATE_WINDOW) + self.hands_on = False + self.torsion_lpf = FirstOrderFilter(0.0, PRESENCE_LPF_RC, 0.01) + self.presence_cnt = 0 + self.presence_sign = 0 + self.presence_hold = 0 + self.hands_off_frames = 0 + + self.torque_active = False + self.torque_active_frames = 0 + # Handback protections depend on why torque mode was entered. Driver + # recovery gets the release timer; both driver and Galaxy recovery get the + # low-angle gate. + self.driver_override_recovery = False + self.galaxy_torque_recovery = False + self.lat_active_last = False + self.eac_dead_frames = 0 + self.eac_rearm_release_frames = 0 + self.eac_rearm_attempted = False + + # Galaxy can switch an angle-capable truck to torque while driving. The + # prearm state makes that transition make-before-break. + self.force_torque = False + self.torque_prearm = False + self.prearm_frames = 0 + self.prearm_torque_peak = 0 + self.prearm_stall_frames = 0 + self.prearm_abort_lockout = 0 + self.prearm_last_outcome = "" + self.prearm_last_hold = 0 + self.prearm_last_frames = 0 + self.prearm_last_peak = 0 + + self.apply_angle_last = 0.0 + self.angle_active = False + self.angle_saturated = False + self.angle_sat_frames = 0 + self.rate_budget = _RateBudget() + self.roll = 0.0 + self.angle_offset_deg = 0.0 + + self.apply_torque_last = 0 + self.torque_cmd = 0 + self.toi_controller = ToiController() + self.toi_act_cmd = False + + def update(self, CS, lat_active: bool, actuators) -> None: + self._update_hands_on(CS) + desired_angle = math.degrees(self.VM.get_steer_from_curvature( + -float(actuators.curvature), CS.out.vEgo, self.roll)) + self.angle_offset_deg + self._update_torque_active(CS, lat_active, desired_angle, actuators) + desired_lat_accel = float(actuators.curvature) * CS.out.vEgo ** 2 + self._update_angle(CS, lat_active, desired_angle, desired_lat_accel) + self._update_torque(CS, actuators) + + def _update_wheel_touched(self, wheel_touched: bool, minimum_count: int) -> bool: + self.wheel_touch_cnt += 1 if wheel_touched else -1 + self.wheel_touch_cnt = int(np.clip(self.wheel_touch_cnt, 0, minimum_count * 2 + 1)) + return self.wheel_touch_cnt > minimum_count + + def _update_torsion(self, torque: float, steering_rate: float, threshold: float, minimum_count: int) -> bool: + self.rate_hist.append(steering_rate) + if max(self.rate_hist) - min(self.rate_hist) > TORSION_MAX_RATE_SWING: + self.torsion_cnt = 0 + return False + + abs_torque = abs(torque) + pressed = abs_torque > threshold + sign = int(np.sign(torque)) + if pressed and self.torsion_sign and sign != self.torsion_sign: + self.torsion_cnt = 0 + else: + self.torsion_cnt += max(1, math.ceil(abs_torque / threshold)) if pressed else -1 + self.torsion_cnt = int(np.clip(self.torsion_cnt, 0, minimum_count * 2 + 1)) + if pressed: + self.torsion_sign = sign + return self.torsion_cnt > minimum_count + + def _update_torsion_presence(self, torque: float) -> bool: + filtered = self.torsion_lpf.update(torque) + sign = 1 if filtered > PRESENCE_TORQUE_THRESHOLD else -1 if filtered < -PRESENCE_TORQUE_THRESHOLD else 0 + self.presence_cnt = self.presence_cnt + 1 if sign != 0 and sign == self.presence_sign else int(sign != 0) + self.presence_sign = sign + if self.presence_cnt >= PRESENCE_MIN_FRAMES: + self.presence_hold = PRESENCE_HOLD_FRAMES + elif self.presence_hold > 0: + self.presence_hold -= 1 + return self.presence_hold > 0 + + def _update_hands_on(self, CS) -> None: + wheel_touch = False + if not self.gen2: + calibration = CS.sccm_wheel_touch["SETME_X52"] + capacitive = CS.sccm_wheel_touch["SCCM_WheelTouch_CapacitiveValue"] > calibration * 0.9 + wheel_touch = self._update_wheel_touched(capacitive, 25) + torsion = self._update_torsion(CS.out.steeringTorque, CS.out.steeringRateDeg, 4.0, 9) + # Keep rejecting ordinary inertial torsion, but do not mask a sustained, + # high-effort driver override just because the wheel is already moving fast. + strong_override = abs(CS.out.steeringTorque) >= DRIVER_OVERRIDE_TORQUE and CS.out.steeringPressed + self.driver_override_cnt += 1 if strong_override else -1 + self.driver_override_cnt = int(np.clip(self.driver_override_cnt, 0, DRIVER_OVERRIDE_FRAMES)) + presence = self._update_torsion_presence(CS.out.steeringTorque) + self.hands_on = wheel_touch or torsion or self.driver_override_cnt >= DRIVER_OVERRIDE_FRAMES or CS.hands_on_level > 1 + self.hands_off_frames = 0 if self.hands_on or presence else self.hands_off_frames + 1 + + def _reset_prearm(self) -> None: + self.torque_prearm = False + self.prearm_frames = 0 + self.prearm_torque_peak = 0 + self.prearm_stall_frames = 0 + + def _end_prearm(self, outcome: str, hold_target: int) -> None: + self.prearm_last_outcome = outcome + self.prearm_last_hold = hold_target + self.prearm_last_frames = self.prearm_frames + self.prearm_last_peak = self.prearm_torque_peak + self._reset_prearm() + + def _update_torque_active(self, CS, lat_active: bool, desired_angle: float, actuators) -> None: + self.torque_active_frames = self.torque_active_frames + 1 if self.torque_active else 0 + epas_ready = CS.eac_status == 1 and CS.eac_error_code == 0 + eac_active = CS.eac_status == 2 + epas_inhibited = CS.eac_status == 0 and CS.eac_error_code == 0 + gap = abs(desired_angle - CS.out.steeringAngleDeg) + + if not lat_active: + self.torque_active = False + self.driver_override_recovery = False + self.galaxy_torque_recovery = False + self.eac_dead_frames = 0 + self.eac_rearm_release_frames = 0 + self.eac_rearm_attempted = False + self.prearm_abort_lockout = 0 + self._reset_prearm() + self.lat_active_last = False + return + + if self.force_torque: + # A stale angle-recovery count must not trigger after switching modes. + self.eac_dead_frames = 0 + self.eac_rearm_release_frames = 0 + self.eac_rearm_attempted = False + self.driver_override_recovery = False + self.lat_active_last = True + + if self.torque_active: + self.galaxy_torque_recovery = True + self._reset_prearm() + return + + if self.prearm_abort_lockout > 0: + self.prearm_abort_lockout -= 1 + self._reset_prearm() + return + + steer_max = round(float(np.interp(CS.out.vEgoRaw, CCP.STEER_MAX_LOOKUP[0], CCP.STEER_MAX_LOOKUP[1]))) + hold_target = abs(int(round(float(actuators.torque) * steer_max))) + epas_holding = eac_active + driver_took_over = self.hands_on and CS.out.steeringPressed + if not epas_holding or driver_took_over or hold_target < TORQUE_PREARM_MIN_HOLD: + self.torque_active = True + self.galaxy_torque_recovery = True + self._reset_prearm() + return + + # Keep angle active while the independently rate-limited torque channel + # ramps underneath it. Progress is evaluated from the prior frame. + self.torque_prearm = True + self.prearm_frames += 1 + if abs(self.apply_torque_last) > self.prearm_torque_peak: + self.prearm_torque_peak = abs(self.apply_torque_last) + self.prearm_stall_frames = 0 + else: + self.prearm_stall_frames += 1 + + reached = abs(self.apply_torque_last) >= TORQUE_PREARM_EXIT_FRAC * hold_target + stalled = self.prearm_stall_frames >= TORQUE_PREARM_STALL_FRAMES + if reached: + self.torque_active = True + self.galaxy_torque_recovery = True + self._end_prearm("reached", hold_target) + elif self.prearm_frames >= TORQUE_PREARM_MAX_FRAMES: + if stalled: + self._end_prearm("abort", hold_target) + self.prearm_abort_lockout = TORQUE_PREARM_ABORT_LOCKOUT + else: + self.torque_active = True + self.galaxy_torque_recovery = True + self._end_prearm("backstop", hold_target) + return + + # Cancelling a force request during prearm immediately returns to the + # already-active angle channel. A completed torque handoff uses the normal + # cooperative release logic below to return to angle safely. + self.prearm_abort_lockout = 0 + self._reset_prearm() + if not self.torque_active: + self.galaxy_torque_recovery = False + + if (self.torque_active and epas_inhibited and + not self.hands_on and not CS.out.steeringPressed): + self.eac_rearm_release_frames = min(self.eac_rearm_release_frames + 1, EAC_REARM_RELEASE_FRAMES) + else: + self.eac_rearm_release_frames = 0 + + if self.hands_on and CS.out.steeringPressed: + self.torque_active = True + self.driver_override_recovery = True + self.galaxy_torque_recovery = False + self.eac_rearm_attempted = False + elif self.eac_dead_frames >= EAC_RECOVER_FRAMES: + self.torque_active = True + self.driver_override_recovery = False + self.galaxy_torque_recovery = False + elif not self.lat_active_last and not epas_ready: + self.torque_active = True + self.driver_override_recovery = False + self.galaxy_torque_recovery = False + self.eac_rearm_attempted = False + elif self.torque_active and self.torque_active_frames >= MIN_TORQUE_FRAMES and not self.hands_on: + fw_max = float(np.interp(CS.out.vEgoRaw, EPAS_FW_MAX_ANGLE_BP, EPAS_FW_MAX_ANGLE_V)) * EPAS_FW_ANGLE_MARGIN + in_envelope = abs(CS.out.steeringAngleDeg) < fw_max + threshold_dps = float(np.interp(CS.out.vEgoRaw, EPAS_FW_RATE_BP, EPAS_FW_RATE_V)) * 100.0 + lower, upper = self.rate_budget.bounds(threshold_dps, EPAS_FW_RATE_MARGIN) + rate_settled = lower <= CS.out.steeringAngleDeg <= upper and abs(CS.out.steeringRateDeg) < UNWIND_HANDOFF_RATE + + rearm_ready = (epas_inhibited and not self.eac_rearm_attempted and + self.eac_rearm_release_frames >= EAC_REARM_RELEASE_FRAMES and + not CS.out.steeringPressed and not CS.out.steerFaultTemporary and + not CS.out.steerFaultPermanent) + handoff_angle_ready = (not (self.driver_override_recovery or self.galaxy_torque_recovery) or + abs(CS.out.steeringAngleDeg) < HANDOFF_MAX_ANGLE_DEG) + driver_handoff_ready = (not self.driver_override_recovery or + self.hands_off_frames >= DRIVER_HANDS_OFF_EXIT_FRAMES) + if ((epas_ready or rearm_ready) and in_envelope and rate_settled and + gap < HANDOFF_EXIT_DEG and handoff_angle_ready and driver_handoff_ready): + self.torque_active = False + self.driver_override_recovery = False + self.galaxy_torque_recovery = False + self.eac_rearm_attempted = rearm_ready + + if eac_active: + self.eac_rearm_attempted = False + + if not self.torque_active and not eac_active: + self.eac_dead_frames += 1 + else: + self.eac_dead_frames = 0 + self.lat_active_last = True + + def _update_angle(self, CS, lat_active: bool, desired_angle: float, desired_lat_accel: float) -> None: + self.angle_active = lat_active and not self.torque_active + v_lookahead = max(CS.out.vEgoRaw + max(CS.out.aEgo, 0.0), 1.0) + apply_angle = apply_rivian_steer_angle_limits_vm(desired_angle, self.apply_angle_last, v_lookahead, + CS.out.steeringAngleDeg, self.angle_active, CCP, self.VM_safety) + + saturated = False + if self.angle_active: + fw_max = float(np.interp(CS.out.vEgoRaw, EPAS_FW_MAX_ANGLE_BP, EPAS_FW_MAX_ANGLE_V)) * EPAS_FW_ANGLE_MARGIN + safety_max = get_max_angle_vm(max(CS.out.vEgoRaw, 1.0), self.VM_safety, CCP) + deliverable_angle = min(fw_max, safety_max) + saturated = abs(desired_lat_accel) > ANGLE_SAT_MIN_LAT_ACCEL and abs(desired_angle) > deliverable_angle + apply_angle = float(np.clip(apply_angle, -fw_max, fw_max)) + threshold_dps = float(np.interp(CS.out.vEgoRaw, EPAS_FW_RATE_BP, EPAS_FW_RATE_V)) * 100.0 + lower, upper = self.rate_budget.bounds(threshold_dps, EPAS_FW_RATE_MARGIN) + apply_angle = float(np.clip(apply_angle, lower, upper)) + step = get_max_angle_delta_vm(max(CS.out.vEgoRaw, 1.0), self.VM_safety, CCP) * PANDA_STEP_MARGIN + apply_angle = float(np.clip(apply_angle, self.apply_angle_last - step, self.apply_angle_last + step)) + + self.angle_sat_frames = self.angle_sat_frames + 1 if saturated else 0 + self.angle_saturated = self.angle_sat_frames >= ANGLE_SAT_FRAMES + self.apply_angle_last = apply_angle + self.rate_budget.push(apply_angle) + + def _update_torque(self, CS, actuators) -> None: + torque_requested = self.torque_active or self.torque_prearm + self.toi_act_cmd, torque_allowed = self.toi_controller.update( + torque_requested, + abs(CS.out.steeringAngleDeg) >= TOI_MAX_ANGLE_DEG, + bool(getattr(CS, "toi_fault", False)), + bool(getattr(CS, "toi_active", False)), + bool(getattr(CS, "toi_unavailable", False)), + prearming=self.torque_prearm, + high_angle_rearm=abs(CS.out.steeringAngleDeg) <= TOI_REARM_ANGLE_DEG, + hold_high_angle_release=self.driver_override_recovery, + ) + + if not torque_requested: + self.apply_torque_last = 0 + self.torque_cmd = 0 + return + + if not torque_allowed: + # The EPAS has either not acknowledged the request yet or is completing a + # high-angle release. Reset the limiter to the torque actually sent. + self.apply_torque_last = 0 + self.torque_cmd = 0 + return + + steer_max = round(float(np.interp(CS.out.vEgoRaw, CCP.STEER_MAX_LOOKUP[0], CCP.STEER_MAX_LOOKUP[1]))) + requested_torque = int(round(float(actuators.torque) * steer_max)) + torque_cmd = apply_driver_steer_torque_limits(requested_torque, self.apply_torque_last, + CS.out.steeringTorque, CCP, steer_max) + + if abs(CS.out.steeringAngleDeg) > HIGH_ANGLE_THRESHOLD_DEG: + cap = int(round(steer_max * HIGH_ANGLE_CAP_FRAC)) + torque_cmd = int(np.clip(torque_cmd, -cap, cap)) + + self.apply_torque_last = torque_cmd + self.torque_cmd = torque_cmd diff --git a/opendbc_repo/opendbc/car/rivian/faults.py b/opendbc_repo/opendbc/car/rivian/faults.py new file mode 100644 index 000000000..cc7de40a0 --- /dev/null +++ b/opendbc_repo/opendbc/car/rivian/faults.py @@ -0,0 +1,12 @@ +TOI_FAULT_ALERT_FRAMES = 25 + + +def get_steering_faults(angle_harness: bool, toi_fault: bool, toi_fault_persistent: bool, + eac_status: int, eac_error_code: int) -> tuple[bool, bool, bool]: + if angle_harness: + # The torque controller handles the short ToiFlt release/rearm handshake + # internally. Escalate only if EPAS does not recover inside that window. + temporary = toi_fault_persistent or (eac_status == 2 and eac_error_code != 0) + return eac_status == 4, temporary, eac_status == 2 and eac_error_code == 12 + + return False, toi_fault or eac_error_code != 0, False diff --git a/opendbc_repo/opendbc/car/rivian/interface.py b/opendbc_repo/opendbc/car/rivian/interface.py index f1108e081..34883264d 100644 --- a/opendbc_repo/opendbc/car/rivian/interface.py +++ b/opendbc_repo/opendbc/car/rivian/interface.py @@ -3,7 +3,7 @@ from opendbc.car.interfaces import CarInterfaceBase from opendbc.car.rivian.carcontroller import CarController from opendbc.car.rivian.carstate import CarState from opendbc.car.rivian.radar_interface import RadarInterface -from opendbc.car.rivian.values import RivianSafetyFlags +from opendbc.car.rivian.values import RivianFlags, RivianSafetyFlags class CarInterface(CarInterfaceBase): @@ -11,27 +11,58 @@ class CarInterface(CarInterfaceBase): CarController = CarController RadarInterface = RadarInterface + @staticmethod + def _apply_angle_caps(ret: structs.CarParams) -> None: + """Enable capabilities that are safe only with the xnor Extreme angle box.""" + ret.flags |= RivianFlags.ANGLE_HARNESS.value + ret.safetyConfigs[0].safetyParam |= RivianSafetyFlags.ANGLE_CONTROL.value + ret.steerActuatorDelay = 0.1 + ret.lateralSmoothSeconds = 0.4 + ret.steerAtStandstill = True + @staticmethod def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams: ret.brand = "rivian" ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.rivian)] - ret.steerActuatorDelay = 0.15 + # Gen 2 (2025+) does not publish SCCM_WheelTouch on the powertrain bus. + if 0x321 not in fingerprint[0]: + ret.flags |= RivianFlags.GEN2.value + + angle_harness = 0x1310 in fingerprint[1] + longitudinal_harness = 0x131A in fingerprint[1] + + if angle_harness: + CarInterface._apply_angle_caps(ret) + + if longitudinal_harness: + ret.flags |= RivianFlags.LONGITUDINAL_HARNESS.value + + # A base comma harness and the xnor longitudinal harness are valid torque + # configurations. Angle-only caps remain gated to the detected Extreme box. + if not angle_harness: + ret.steerActuatorDelay = 0.15 + ret.lateralSmoothSeconds = 0.0 + ret.steerAtStandstill = False ret.steerLimitTimer = 0.4 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) ret.steerControlType = structs.CarParams.SteerControlType.torque - ret.radarUnavailable = True + ret.radarUnavailable = not longitudinal_harness + ret.enableBsm = longitudinal_harness - # TODO: pending finding/handling missing set speed and fixing up radar parser - ret.alphaLongitudinalAvailable = False - if alpha_long: + ret.alphaLongitudinalAvailable = longitudinal_harness + if alpha_long and ret.alphaLongitudinalAvailable: ret.openpilotLongitudinalControl = True ret.safetyConfigs[0].safetyParam |= RivianSafetyFlags.LONG_CONTROL.value - ret.longitudinalActuatorDelay = 0.35 + # AdventurePilot road data measured roughly 0.26-0.38 s command-to-aEgo + # lag. 0.3 s puts the planner near the center of the observed plant delay. + ret.longitudinalActuatorDelay = 0.3 ret.vEgoStopping = 0.25 - ret.stopAccel = 0 + ret.stopAccel = -0.2 + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.2] return ret diff --git a/opendbc_repo/opendbc/car/rivian/radar_interface.py b/opendbc_repo/opendbc/car/rivian/radar_interface.py index f74acafe9..4c4d9669f 100644 --- a/opendbc_repo/opendbc/car/rivian/radar_interface.py +++ b/opendbc_repo/opendbc/car/rivian/radar_interface.py @@ -50,22 +50,26 @@ class RadarInterface(RadarInterfaceBase): for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT): msg = self.rcp.vl[f"RADAR_TRACK_{addr:x}"] - if addr not in self.pts: - self.pts[addr] = structs.RadarData.RadarPoint() - self.pts[addr].trackId = self.track_id - self.track_id += 1 - - valid = msg['STATE'] in (3, 4) and msg['STATE_2'] == 1 + # STATE: 1=New, 2=New_updated, 3=Updated, 4=Coasting, 7=New_coasting + valid = msg['STATE'] in (1, 2, 3, 4, 7) + # Ignore short-range-only objects, which include close stationary roadside + # objects that can otherwise lead to phantom braking. + valid = valid and msg['MODE'] in (2, 3) if valid: + if addr not in self.pts or msg['STATE'] in (1, 2, 7): + self.pts[addr] = structs.RadarData.RadarPoint() + self.pts[addr].trackId = self.track_id + self.track_id += 1 + azimuth = math.radians(msg['AZIMUTH']) - self.pts[addr].measured = True + self.pts[addr].measured = msg['STATE'] in (2, 3) self.pts[addr].dRel = math.cos(azimuth) * msg['LONG_DIST'] - self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST'] + self.pts[addr].yRel = -math.sin(azimuth) * msg['LONG_DIST'] self.pts[addr].vRel = msg['REL_SPEED'] self.pts[addr].aRel = float('nan') self.pts[addr].yvRel = float('nan') - else: + elif addr in self.pts: del self.pts[addr] ret.points = list(self.pts.values()) diff --git a/opendbc_repo/opendbc/car/rivian/riviancan.py b/opendbc_repo/opendbc/car/rivian/riviancan.py index bbe679c20..c507bf710 100644 --- a/opendbc_repo/opendbc/car/rivian/riviancan.py +++ b/opendbc_repo/opendbc/car/rivian/riviancan.py @@ -43,6 +43,33 @@ def create_lka_steering(packer, frame, acm_lka_hba_cmd, apply_torque, enabled, a return packer.make_can_msg("ACM_lkaHbaCmd", 0, values) +def create_angle_steering(packer, frame, angle, active): + values = { + "ACM_SteeringControl_Counter": frame % 15, + "ACM_SteeringAngleRequest": angle, + "ACM_EacEnabled": active, + "ACM_HapticRequired": 0, + } + + data = packer.make_can_msg("ACM_SteeringControl", 0, values)[1] + values["ACM_SteeringControl_Checksum"] = checksum(data[1:], 0x1D, 0x41) + return packer.make_can_msg("ACM_SteeringControl", 0, values) + + +def create_acm_status(packer, frame, feature_status): + values = { + "ACM_Status_Counter": frame % 15, + "ACM_FeatureStatus": feature_status, + "ACM_FaultStatus": 0, + "ACM_FaultSupervisorState": 0, + "ACM_Unkown1": 0, + } + + data = packer.make_can_msg("ACM_Status", 0, values)[1] + values["ACM_Status_Checksum"] = checksum(data[1:], 0x1D, 0x5F) + return packer.make_can_msg("ACM_Status", 0, values) + + def create_wheel_touch(packer, sccm_wheel_touch, enabled): values = {s: sccm_wheel_touch[s] for s in ( "SCCM_WheelTouch_Counter", @@ -92,6 +119,8 @@ def create_adas_status(packer, vdm_adas_status, interface_status): )} if interface_status is not None: + if interface_status == 1: + values["VDM_UserAdasRequest"] = 1 values["VDM_AdasInterfaceStatus"] = interface_status data = packer.make_can_msg("VDM_AdasSts", 2, values)[1] diff --git a/opendbc_repo/opendbc/car/rivian/tests/test_rivian.py b/opendbc_repo/opendbc/car/rivian/tests/test_rivian.py index 90dcfcd5d..93769abf4 100644 --- a/opendbc_repo/opendbc/car/rivian/tests/test_rivian.py +++ b/opendbc_repo/opendbc/car/rivian/tests/test_rivian.py @@ -1,8 +1,487 @@ +from types import SimpleNamespace + +from opendbc.car import Bus, lateral as lateral_helpers, structs +from opendbc.car.common.conversions import Conversions as CV +from opendbc.car.docs_definitions import CarHarness +from opendbc.car.rivian import carcontroller as rivian_carcontroller +from opendbc.car.rivian import ext_controller +from opendbc.car.rivian.carcontroller import CarController, get_longitudinal_accel +from opendbc.car.rivian.carstate import get_cruise_available +from opendbc.car.rivian.carstate_ext import RivianLongitudinalState +from opendbc.car.rivian.ext_controller import ExternalController +from opendbc.car.rivian.faults import get_steering_faults from opendbc.car.rivian.fingerprints import FW_VERSIONS -from opendbc.car.rivian.values import CAR, FW_QUERY_CONFIG, WMI, ModelLine, ModelYear +from opendbc.car.rivian.interface import CarInterface +from opendbc.car.rivian.toi_controller import (TOI_ACK_FRAMES, TOI_MAX_ANGLE_FRAMES, + TOI_RECOVERY_TIMEOUT_FRAMES, ToiController, ToiState) +from opendbc.car.rivian.values import CAR, FW_QUERY_CONFIG, WMI, ModelLine, ModelYear, RivianFlags, RivianSafetyFlags class TestRivian: + @staticmethod + def _car_params(bus_one_messages=(), alpha_long=False, gen2=False): + fingerprint = {bus: {} for bus in range(8)} + if not gen2: + fingerprint[0][0x321] = 8 + fingerprint[1] = {address: 8 for address in bus_one_messages} + return CarInterface.get_params(CAR.RIVIAN_R1_GEN1, fingerprint, [], alpha_long, False, False, SimpleNamespace()) + + def test_standard_harness_uses_torque_control(self): + params = self._car_params() + + assert not params.dashcamOnly + assert params.steerControlType == structs.CarParams.SteerControlType.torque + assert not params.flags & RivianFlags.ANGLE_HARNESS + assert not params.safetyConfigs[0].safetyParam & RivianSafetyFlags.ANGLE_CONTROL + assert not params.openpilotLongitudinalControl + + def test_longitudinal_harness_uses_torque_and_openpilot_long(self): + params = self._car_params((0x131A,), alpha_long=True) + + assert not params.dashcamOnly + assert params.flags & RivianFlags.LONGITUDINAL_HARNESS + assert not params.flags & RivianFlags.ANGLE_HARNESS + assert params.openpilotLongitudinalControl + assert params.safetyConfigs[0].safetyParam & RivianSafetyFlags.LONG_CONTROL + + def test_extreme_harness_enables_angle_control(self): + params = self._car_params((0x1310,)) + + assert not params.dashcamOnly + assert params.flags & RivianFlags.ANGLE_HARNESS + assert params.safetyConfigs[0].safetyParam & RivianSafetyFlags.ANGLE_CONTROL + + def test_extreme_and_longitudinal_harnesses_enable_both_paths(self): + params = self._car_params((0x1310, 0x131A), alpha_long=True) + + assert not params.dashcamOnly + assert params.flags & RivianFlags.ANGLE_HARNESS + assert params.flags & RivianFlags.LONGITUDINAL_HARNESS + assert params.safetyConfigs[0].safetyParam & RivianSafetyFlags.ANGLE_CONTROL + assert params.safetyConfigs[0].safetyParam & RivianSafetyFlags.LONG_CONTROL + assert params.openpilotLongitudinalControl + + def test_gen2_is_detected_without_enabling_harness_capabilities(self): + params = self._car_params(gen2=True) + + assert params.flags & RivianFlags.GEN2 + assert not params.flags & RivianFlags.ANGLE_HARNESS + assert not params.flags & RivianFlags.LONGITUDINAL_HARNESS + + def test_gen2_detection_preserves_explicit_harness_capabilities(self): + params = self._car_params((0x1310, 0x131A), alpha_long=True, gen2=True) + + assert params.flags & RivianFlags.GEN2 + assert params.flags & RivianFlags.ANGLE_HARNESS + assert params.flags & RivianFlags.LONGITUDINAL_HARNESS + assert params.safetyConfigs[0].safetyParam & RivianSafetyFlags.ANGLE_CONTROL + assert params.safetyConfigs[0].safetyParam & RivianSafetyFlags.LONG_CONTROL + + def test_gen2_docs_use_rivian_b_harness(self): + gen2_docs = [doc for doc in CAR.RIVIAN_R1_GEN1.config.car_docs if "2025" in doc.name] + + assert len(gen2_docs) == 2 + assert all(CarHarness.rivian_b in doc.car_parts.parts for doc in gen2_docs) + + def test_standard_harness_uses_acm_state_for_stock_acc_availability(self): + params = self._car_params() + + assert get_cruise_available(params.flags, 0) # standby + assert get_cruise_available(params.flags, 1) # ACC active before VDM responds + assert not get_cruise_available(params.flags, 2) # Highway Assist + assert not get_cruise_available(params.flags, 3) # unavailable + assert not get_cruise_available(params.flags, 4) # faulted + + def test_longitudinal_harness_does_not_depend_on_stock_acm_state(self): + params = self._car_params((0x131A,), alpha_long=True) + + assert all(get_cruise_available(params.flags, feature_status) for feature_status in range(5)) + + def test_gas_pedal_zeroes_stale_longitudinal_acceleration(self): + assert get_longitudinal_accel(0.07, gas_pressed=True) == 0.0 + assert get_longitudinal_accel(-2.44, gas_pressed=True) == 0.0 + assert get_longitudinal_accel(0.07, gas_pressed=False) == 0.07 + + def test_longitudinal_drag_feedforward_is_active_only_while_controlling(self): + assert get_longitudinal_accel(0.0, gas_pressed=False, long_active=False, v_ego=8.0) == 0.0 + assert get_longitudinal_accel(0.0, gas_pressed=False, long_active=True, v_ego=8.0) == 0.17 + assert get_longitudinal_accel(0.0, gas_pressed=True, long_active=True, v_ego=8.0) == 0.0 + + def test_live_params_update_rx_dev_command_model(self): + updates = [] + controller = CarController.__new__(CarController) + controller.ext_controller = SimpleNamespace( + roll=0.0, + angle_offset_deg=0.0, + VM=SimpleNamespace(update_params=lambda stiffness, ratio: updates.append((stiffness, ratio))), + ) + + controller.update_live_params(0.05, 1.25, 0.8, 16.0) + + assert controller.ext_controller.roll == 0.05 + assert controller.ext_controller.angle_offset_deg == 1.25 + assert updates == [(0.8, 16.0)] + + def test_rivian_angle_limit_does_not_change_shared_angle_car_behavior(self, monkeypatch): + limits = SimpleNamespace(ANGLE_LIMITS=SimpleNamespace(MAX_ANGLE_RATE=5.0, STEER_ANGLE_MAX=500.0)) + vehicle_model = SimpleNamespace() + + monkeypatch.setattr(ext_controller, "get_max_angle_vm", lambda *args: 10.0) + monkeypatch.setattr(ext_controller, "get_max_angle_delta_vm", lambda *args: 1.0) + monkeypatch.setattr(lateral_helpers, "get_max_angle_vm", lambda *args: 10.0) + monkeypatch.setattr(lateral_helpers, "get_max_angle_delta_vm", lambda *args: 1.0) + + rivian_angle = ext_controller.apply_rivian_steer_angle_limits_vm( + 20.0, 15.0, 10.0, 0.0, True, limits, vehicle_model, + ) + shared_angle = lateral_helpers.apply_steer_angle_limits_vm( + 20.0, 15.0, 10.0, 0.0, True, limits, vehicle_model, + ) + + assert rivian_angle == 14.0 + assert shared_angle == 10.0 + + def test_angle_saturation_is_debounced_and_clears_outside_angle_mode(self): + controller = ExternalController(self._car_params((0x1310,))) + car_state = SimpleNamespace(out=SimpleNamespace( + vEgoRaw=15.0, + aEgo=0.0, + steeringAngleDeg=0.0, + )) + + for _ in range(ext_controller.ANGLE_SAT_FRAMES - 1): + controller._update_angle(car_state, True, desired_angle=200.0, desired_lat_accel=2.0) + assert not controller.angle_saturated + + controller._update_angle(car_state, True, desired_angle=200.0, desired_lat_accel=2.0) + assert controller.angle_saturated + + controller.torque_active = True + controller._update_angle(car_state, True, desired_angle=200.0, desired_lat_accel=2.0) + assert not controller.angle_saturated + + def test_angle_saturation_ignores_parking_speed_turns(self): + controller = ExternalController(self._car_params((0x1310,))) + car_state = SimpleNamespace(out=SimpleNamespace( + vEgoRaw=2.0, + aEgo=0.0, + steeringAngleDeg=0.0, + )) + + for _ in range(ext_controller.ANGLE_SAT_FRAMES + 5): + controller._update_angle(car_state, True, desired_angle=400.0, desired_lat_accel=0.8) + + assert not controller.angle_saturated + + def test_angle_saturation_param_is_seeded_and_edge_written(self): + writes = [] + controller = CarController.__new__(CarController) + controller.ext_controller = SimpleNamespace(angle_saturated=False) + controller.angle_saturation_last = None + controller.angle_saturation_params = SimpleNamespace( + put_bool=lambda key, value: writes.append((key, value)), + ) + + controller._publish_angle_saturation() + controller._publish_angle_saturation() + controller.ext_controller.angle_saturated = True + controller._publish_angle_saturation() + + assert writes == [ + ("RivianAngleSaturated", False), + ("RivianAngleSaturated", True), + ] + + def test_toi_recovery_failure_param_is_edge_written(self): + writes = [] + controller = CarController.__new__(CarController) + controller.angle_harness = False + controller.toi_controller = SimpleNamespace(recovery_failed=False) + controller.toi_recovery_failed_last = None + controller.toi_recovery_params = SimpleNamespace( + put_bool=lambda key, value: writes.append((key, value)), + ) + + controller._publish_toi_recovery_failed() + controller._publish_toi_recovery_failed() + controller.toi_controller.recovery_failed = True + controller._publish_toi_recovery_failed() + + assert writes == [ + ("RivianToiRecoveryFailed", False), + ("RivianToiRecoveryFailed", True), + ] + + @staticmethod + def _controller_actuators(monkeypatch, *, angle_harness, torque_active, requested_torque, applied_torque, + angle_control=True, toi_recovering=False, gen2=False, frame=1): + monkeypatch.setattr(rivian_carcontroller, "create_lka_steering", lambda *args: None) + monkeypatch.setattr(rivian_carcontroller, "create_angle_steering", lambda *args: None) + monkeypatch.setattr(rivian_carcontroller, "create_acm_status", lambda *args: None) + monkeypatch.setattr(rivian_carcontroller, "apply_driver_steer_torque_limits", lambda *args: applied_torque) + controller = CarController.__new__(CarController) + flags = RivianFlags.GEN2 if gen2 else RivianFlags(0) + controller.CP = SimpleNamespace(flags=flags, openpilotLongitudinalControl=False) + controller.packer = None + controller.frame = frame + controller.apply_torque_last = 0 + controller.cancel_frames = 0 + controller.toi_controller = SimpleNamespace( + update=lambda *args: (True, True), + recovering=toi_recovering, + recovery_failed=False, + ) + controller.angle_harness = angle_harness + controller.ext_controller = None + if angle_harness: + controller.ext_controller = SimpleNamespace( + update=lambda *args: None, + force_torque=False, + torque_cmd=applied_torque, + toi_act_cmd=True, + apply_angle_last=12.0, + angle_active=not torque_active, + torque_active=torque_active, + torque_prearm=False, + toi_controller=SimpleNamespace(recovering=toi_recovering, recovery_failed=False), + ) + + output = SimpleNamespace() + actuators = SimpleNamespace(torque=requested_torque, accel=0.0, as_builder=lambda: output) + car_control = SimpleNamespace( + actuators=actuators, + latActive=True, + longActive=False, + enabled=True, + cruiseControl=SimpleNamespace(cancel=False), + ) + car_state = SimpleNamespace( + out=SimpleNamespace( + gearShifter=structs.CarState.GearShifter.drive, + vEgo=10.0, + vEgoRaw=10.0, + gasPressed=False, + steeringTorque=0.0, + steeringAngleDeg=0.0, + ), + acm_lka_hba_cmd={}, + sccm_wheel_touch={}, + vdm_adas_status=(), + ) + + result = controller.update(car_control, car_state, 0, SimpleNamespace(rivian_angle_control=angle_control))[0] + result.force_torque = controller.ext_controller.force_torque if angle_harness else None + return result + + def test_angle_toggle_selects_controller_live(self, monkeypatch): + angle_output = self._controller_actuators( + monkeypatch, + angle_harness=True, + torque_active=False, + requested_torque=0.0, + applied_torque=0, + angle_control=True, + ) + torque_output = self._controller_actuators( + monkeypatch, + angle_harness=True, + torque_active=False, + requested_torque=0.0, + applied_torque=0, + angle_control=False, + ) + + assert not angle_output.force_torque + assert torque_output.force_torque + + def test_angle_channel_reports_actual_zero_torque(self, monkeypatch): + output = self._controller_actuators( + monkeypatch, + angle_harness=True, + torque_active=False, + requested_torque=0.42, + applied_torque=0, + ) + + assert output.torque == 0.0 + assert output.torqueOutputCan == 0 + assert output.lateralControlMode == structs.CarControl.Actuators.LateralControlMode.angle + + def test_torque_fallback_reports_applied_can_torque(self, monkeypatch): + output = self._controller_actuators( + monkeypatch, + angle_harness=True, + torque_active=True, + requested_torque=0.42, + applied_torque=100, + ) + steer_max = round(float(rivian_carcontroller.np.interp( + 10.0, + rivian_carcontroller.CarControllerParams.STEER_MAX_LOOKUP[0], + rivian_carcontroller.CarControllerParams.STEER_MAX_LOOKUP[1], + ))) + + assert output.torque == 100 / steer_max + assert output.torqueOutputCan == 100 + assert output.lateralControlMode == structs.CarControl.Actuators.LateralControlMode.torque + + def test_torque_harness_reporting_is_unchanged(self, monkeypatch): + output = self._controller_actuators( + monkeypatch, + angle_harness=False, + torque_active=False, + requested_torque=0.42, + applied_torque=80, + ) + steer_max = round(float(rivian_carcontroller.np.interp( + 10.0, + rivian_carcontroller.CarControllerParams.STEER_MAX_LOOKUP[0], + rivian_carcontroller.CarControllerParams.STEER_MAX_LOOKUP[1], + ))) + + assert output.torque == 80 / steer_max + assert output.torqueOutputCan == 80 + assert output.lateralControlMode == structs.CarControl.Actuators.LateralControlMode.torque + + def test_torque_recovery_is_reported_explicitly(self, monkeypatch): + output = self._controller_actuators( + monkeypatch, + angle_harness=True, + torque_active=True, + requested_torque=0.42, + applied_torque=0, + toi_recovering=True, + ) + + assert output.lateralControlMode == structs.CarControl.Actuators.LateralControlMode.torqueRecovering + + def test_gen2_does_not_transmit_missing_wheel_touch_message(self, monkeypatch): + monkeypatch.setattr( + rivian_carcontroller, + "create_wheel_touch", + lambda *args: (_ for _ in ()).throw(AssertionError("Gen 2 has no SCCM wheel-touch message")), + ) + + self._controller_actuators( + monkeypatch, + angle_harness=False, + torque_active=False, + requested_torque=0.0, + applied_torque=0, + gen2=True, + frame=0, + ) + + def test_gen1_continues_transmitting_wheel_touch_message(self, monkeypatch): + calls = [] + monkeypatch.setattr(rivian_carcontroller, "create_wheel_touch", lambda *args: calls.append(args)) + + self._controller_actuators( + monkeypatch, + angle_harness=False, + torque_active=False, + requested_torque=0.0, + applied_torque=0, + frame=0, + ) + + assert len(calls) == 1 + + def test_software_cruise_speed_request_is_clamped_to_rivian_bounds(self): + state = RivianLongitudinalState(SimpleNamespace(openpilotLongitudinalControl=True)) + + assert state.set_cruise_speed(45 * CV.MPH_TO_MS) == 45 * CV.MPH_TO_MS + assert state.set_cruise_speed(10 * CV.MPH_TO_MS) == 20 * CV.MPH_TO_MS + assert state.set_cruise_speed(100 * CV.MPH_TO_MS) == 85 * CV.MPH_TO_MS + + @staticmethod + def _longitudinal_parsers(scroll=0, scroll_click=0): + return { + Bus.alt: SimpleNamespace(vl={ + "WheelButtons_Fwd": { + "RightButton_Scroll": scroll, + "RightButton_ScrollClick": scroll_click, + "RightButton_RightClick": 0, + "RightButton_LeftClick": 0, + }, + "BSM_BlindSpotIndicator_Fwd": { + "BSM_BlindSpotIndicator_Left": 0, + "BSM_BlindSpotIndicator_Right": 0, + }, + }), + Bus.adas: SimpleNamespace(vl={"Cluster": {"Cluster_Unit": 1}}), + Bus.pt: SimpleNamespace(vl={"VDM_AdasSts": {"VDM_UserAdasRequest": 0}}), + } + + @staticmethod + def _longitudinal_ret(): + return SimpleNamespace( + buttonEvents=[], + cruiseState=SimpleNamespace(enabled=True, speed=0.0), + vEgoCluster=10.0, + leftBlindspot=False, + rightBlindspot=False, + ) + + def test_scroll_rotation_emits_one_personality_event_per_detent(self): + state = RivianLongitudinalState(SimpleNamespace(openpilotLongitudinalControl=True)) + ret = self._longitudinal_ret() + + assert state.update_longitudinal_upgrade(ret, self._longitudinal_parsers(scroll=0)) == [] + events = state.update_longitudinal_upgrade(ret, self._longitudinal_parsers(scroll=1)) + assert [(event.type, event.pressed) for event in events] == [(structs.CarState.ButtonEvent.Type.gapAdjustCruise, False)] + assert state.update_longitudinal_upgrade(ret, self._longitudinal_parsers(scroll=1)) == [] + assert state.update_longitudinal_upgrade(ret, self._longitudinal_parsers(scroll=255)) == [] + events = state.update_longitudinal_upgrade(ret, self._longitudinal_parsers(scroll=2)) + assert [(event.type, event.pressed) for event in events] == [(structs.CarState.ButtonEvent.Type.gapAdjustCruise, False)] + + def test_scroll_click_emits_held_distance_button_edges(self): + state = RivianLongitudinalState(SimpleNamespace(openpilotLongitudinalControl=True)) + ret = self._longitudinal_ret() + + assert state.update_longitudinal_upgrade(ret, self._longitudinal_parsers()) == [] + events = state.update_longitudinal_upgrade(ret, self._longitudinal_parsers(scroll_click=2)) + assert [(event.type, event.pressed) for event in events] == [(structs.CarState.ButtonEvent.Type.gapAdjustCruise, True)] + assert state.update_longitudinal_upgrade(ret, self._longitudinal_parsers(scroll_click=2)) == [] + events = state.update_longitudinal_upgrade(ret, self._longitudinal_parsers(scroll_click=0)) + assert [(event.type, event.pressed) for event in events] == [(structs.CarState.ButtonEvent.Type.gapAdjustCruise, False)] + + def test_scroll_controls_are_ignored_without_openpilot_longitudinal(self): + state = RivianLongitudinalState(SimpleNamespace(openpilotLongitudinalControl=False)) + events = state.update_longitudinal_upgrade( + self._longitudinal_ret(), + self._longitudinal_parsers(scroll=1, scroll_click=2), + ) + assert events == [] + + def test_angle_harness_ignores_toi_fault(self): + permanent, temporary, disengage = get_steering_faults(True, True, False, 1, 0) + + assert not permanent + assert not temporary + assert not disengage + + def test_angle_harness_reports_active_eac_fault(self): + permanent, temporary, disengage = get_steering_faults(True, False, False, 2, 12) + + assert not permanent + assert temporary + assert disengage + + def test_torque_harness_reports_toi_fault(self): + permanent, temporary, disengage = get_steering_faults(False, True, False, 1, 0) + + assert not permanent + assert temporary + assert not disengage + + def test_angle_harness_reports_persistent_toi_fault(self): + permanent, temporary, disengage = get_steering_faults(True, True, True, 1, 0) + + assert not permanent + assert temporary + assert not disengage + def test_custom_fuzzy_fingerprinting(self, subtests): for platform in CAR: with subtests.test(platform=platform.name): @@ -19,5 +498,572 @@ class TestRivian: vin = "".join(vin) matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy({}, vin, FW_VERSIONS) - should_match = year != ModelYear.S_2025 and not bad + should_match = year != ModelYear.T_2026 and not bad assert (matches == {platform}) == should_match, "Bad match" + + def test_toi_engagement_waits_for_epas_acknowledgement(self): + controller = ToiController() + + assert controller.update(True, False, False, False, False) == (True, False) + assert controller.state == ToiState.REARMING + + for _ in range(TOI_ACK_FRAMES): + assert controller.update(True, False, False, True, False) == (True, False) + + assert controller.state == ToiState.TORQUE + assert controller.update(True, False, False, True, False) == (True, True) + + def test_toi_delayed_acknowledgement_does_not_report_recovery_failure(self): + controller = ToiController() + + for _ in range(31): + assert controller.update(True, False, False, False, False) == (True, False) + + assert controller.state == ToiState.REARMING + assert not controller.recovery_failed + + for _ in range(TOI_ACK_FRAMES): + assert controller.update(True, False, False, True, False) == (True, False) + + assert controller.state == ToiState.TORQUE + assert not controller.recovery_failed + + def test_toi_prearm_allows_torque_before_epas_acknowledgement(self): + controller = ToiController() + + assert controller.update(True, False, False, False, False, prearming=True) == (True, True) + assert controller.state == ToiState.PREARMING + assert not controller.recovering + + assert controller.update(True, False, False, False, False) == (True, True) + assert controller.state == ToiState.ACTIVATING + + for _ in range(TOI_ACK_FRAMES): + assert controller.update(True, False, False, True, False) == (True, True) + + assert controller.state == ToiState.TORQUE + assert not controller.recovery_failed + + def test_toi_prearm_resumes_after_high_angle_release(self): + controller = ToiController() + + for _ in range(TOI_MAX_ANGLE_FRAMES): + assert controller.update(True, True, False, False, False, prearming=True) == (True, True) + + assert controller.update(True, True, False, False, False, prearming=True) == (False, False) + assert controller.state == ToiState.RELEASING + + for _ in range(TOI_ACK_FRAMES): + assert controller.update(True, True, False, False, False, prearming=True) == (False, False) + + assert controller.state == ToiState.PREARMING + assert controller.update(True, True, False, False, False, prearming=True) == (True, True) + + def test_toi_prearmed_activation_timeout_releases_request(self): + controller = ToiController() + + assert controller.update(True, False, False, False, False, prearming=True) == (True, True) + for _ in range(TOI_RECOVERY_TIMEOUT_FRAMES - 1): + assert controller.update(True, False, False, False, False) == (True, True) + + assert controller.update(True, False, False, False, False) == (False, False) + assert controller.state == ToiState.RELEASING + assert controller.recovery_failed + + def test_high_angle_toi_release_waits_for_feedback_before_rearming(self): + controller = ToiController() + controller.state = ToiState.TORQUE + + for _ in range(TOI_MAX_ANGLE_FRAMES): + assert controller.update(True, True, False, True, False) == (True, True) + + assert controller.update(True, True, False, True, False) == (False, False) + assert controller.state == ToiState.RELEASING + + # Unlike the old two-frame blip, the request stays low for as long as the + # EPAS continues reporting that torque overlay is active. + for _ in range(5): + assert controller.update(True, True, False, True, False) == (False, False) + + for _ in range(TOI_ACK_FRAMES): + assert controller.update(True, True, False, False, False) == (False, False) + + assert controller.state == ToiState.REARMING + assert controller.update(True, True, False, False, False) == (True, False) + + def test_driver_override_recovery_waits_for_lower_angle_before_rearming(self): + controller = ToiController() + controller.state = ToiState.RELEASING + + for _ in range(TOI_ACK_FRAMES): + assert controller.update(True, True, False, False, False, high_angle_rearm=False, + hold_high_angle_release=True) == (False, False) + + assert controller.state == ToiState.HIGH_ANGLE_LOCKOUT + for _ in range(TOI_RECOVERY_TIMEOUT_FRAMES + 1): + assert controller.update(True, True, False, False, False, high_angle_rearm=False, + hold_high_angle_release=True) == (False, False) + assert not controller.recovery_failed + + # The high-angle threshold has hysteresis: dropping below the 90 degree + # release point is not sufficient; rearming waits until 80 degrees. + assert controller.update(True, False, False, False, False, high_angle_rearm=False, + hold_high_angle_release=True) == (False, False) + assert controller.state == ToiState.HIGH_ANGLE_LOCKOUT + assert controller.update(True, False, False, False, False, high_angle_rearm=True, + hold_high_angle_release=True) == (False, False) + assert controller.state == ToiState.REARMING + assert controller.update(True, False, False, False, False, high_angle_rearm=True, + hold_high_angle_release=True) == (True, False) + for _ in range(TOI_ACK_FRAMES): + assert controller.update(True, False, False, True, False, high_angle_rearm=True, + hold_high_angle_release=True) == (True, False) + + assert controller.state == ToiState.TORQUE + assert controller.update(True, False, False, True, False, high_angle_rearm=True, + hold_high_angle_release=True) == (True, True) + + def test_toi_fault_forces_release_at_any_angle(self): + controller = ToiController() + controller.state = ToiState.TORQUE + + assert controller.update(True, False, True, False, False) == (False, False) + assert controller.state == ToiState.RELEASING + + def test_toi_recovery_timeout_is_reported_and_request_stays_released(self): + controller = ToiController() + controller.state = ToiState.RELEASING + + for _ in range(TOI_RECOVERY_TIMEOUT_FRAMES): + assert controller.update(True, False, True, False, False) == (False, False) + + assert controller.recovery_failed + assert controller.state == ToiState.RELEASING + + def test_toi_release_resets_external_torque_limiter(self): + controller = ExternalController.__new__(ExternalController) + controller.torque_active = True + controller.torque_prearm = False + controller.driver_override_recovery = False + controller.apply_torque_last = 100 + controller.torque_cmd = 100 + controller.toi_controller = SimpleNamespace( + update=lambda *args, **kwargs: (False, False), + recovering=True, + recovery_failed=False, + ) + + car_state = SimpleNamespace( + out=SimpleNamespace(vEgoRaw=10.0, steeringTorque=0.0, steeringAngleDeg=100.0), + toi_fault=True, + toi_active=False, + toi_unavailable=False, + ) + controller._update_torque(car_state, SimpleNamespace(torque=1.0)) + + assert controller.apply_torque_last == 0 + assert controller.torque_cmd == 0 + assert not controller.toi_act_cmd + + @staticmethod + def _handoff_controller(torque_active=False, lat_active_last=True): + controller = ExternalController.__new__(ExternalController) + controller.hands_on = False + controller.torque_active = torque_active + controller.torque_active_frames = ext_controller.MIN_TORQUE_FRAMES + controller.driver_override_recovery = False + controller.galaxy_torque_recovery = False + controller.hands_off_frames = ext_controller.DRIVER_HANDS_OFF_EXIT_FRAMES + controller.lat_active_last = lat_active_last + controller.eac_dead_frames = 0 + controller.eac_rearm_release_frames = 0 + controller.eac_rearm_attempted = False + controller.force_torque = False + controller.torque_prearm = False + controller.prearm_frames = 0 + controller.prearm_torque_peak = 0 + controller.prearm_stall_frames = 0 + controller.prearm_abort_lockout = 0 + controller.apply_torque_last = 0 + controller.rate_budget = SimpleNamespace(bounds=lambda *_: (-1000.0, 1000.0)) + return controller + + @staticmethod + def _handoff_actuators(torque=0.0): + return SimpleNamespace(torque=torque) + + @staticmethod + def _handoff_car_state(*, speed=10.0, angle=0.0, rate=0.0, torque=0.0, pressed=False, + eac_status=1, eac_error_code=0, temporary_fault=False, permanent_fault=False): + return SimpleNamespace( + out=SimpleNamespace( + vEgoRaw=speed, + steeringAngleDeg=angle, + steeringRateDeg=rate, + steeringTorque=torque, + steeringPressed=pressed, + steerFaultTemporary=temporary_fault, + steerFaultPermanent=permanent_fault, + ), + eac_status=eac_status, + eac_error_code=eac_error_code, + ) + + def test_rx_dev_status_available_hands_off_hands_back_without_extra_delay(self): + controller = self._handoff_controller(torque_active=True) + car_state = self._handoff_car_state() + + controller._update_torque_active(car_state, True, 0.0, self._handoff_actuators()) + + assert not controller.torque_active + + def test_non_driver_recovery_does_not_inherit_25_degree_limit(self): + controller = self._handoff_controller(torque_active=True) + car_state = self._handoff_car_state(speed=2.0, angle=50.0) + + controller._update_torque_active(car_state, True, 50.0, self._handoff_actuators()) + + assert not controller.torque_active + + def test_galaxy_torque_to_angle_waits_until_wheel_is_under_25_degrees(self): + controller = self._handoff_controller(torque_active=True) + controller.galaxy_torque_recovery = True + controller.hands_off_frames = 0 + car_state = self._handoff_car_state(speed=2.0, angle=ext_controller.HANDOFF_MAX_ANGLE_DEG) + + controller._update_torque_active( + car_state, + True, + ext_controller.HANDOFF_MAX_ANGLE_DEG, + self._handoff_actuators(), + ) + + assert controller.torque_active + + car_state.out.steeringAngleDeg = ext_controller.HANDOFF_MAX_ANGLE_DEG - 1.0 + controller._update_torque_active( + car_state, + True, + ext_controller.HANDOFF_MAX_ANGLE_DEG - 1.0, + self._handoff_actuators(), + ) + + assert not controller.torque_active + assert not controller.galaxy_torque_recovery + + def test_route_opposing_reaction_torque_does_not_force_fallback_without_hands_on(self): + controller = self._handoff_controller() + # Route-derived signature from the first incident. rx-dev-src requires its + # filtered hands-on signal in addition to steeringPressed. + car_state = self._handoff_car_state(speed=6.85, angle=130.5, torque=-1.64, pressed=True) + + controller._update_torque_active(car_state, True, 135.7, self._handoff_actuators()) + + assert not controller.torque_active + + def test_confirmed_driver_override_enters_torque_fallback(self): + controller = self._handoff_controller() + controller.hands_on = True + car_state = self._handoff_car_state(torque=2.35, pressed=True) + + controller._update_torque_active(car_state, True, 15.0, self._handoff_actuators()) + + assert controller.torque_active + assert controller.driver_override_recovery + + def test_driver_override_requires_one_second_hands_off_before_angle_handback(self): + controller = self._handoff_controller(torque_active=True) + controller.driver_override_recovery = True + controller.hands_off_frames = ext_controller.DRIVER_HANDS_OFF_EXIT_FRAMES - 1 + car_state = self._handoff_car_state(angle=0.0) + + controller._update_torque_active(car_state, True, 0.0, self._handoff_actuators()) + + assert controller.torque_active + + controller.hands_off_frames += 1 + controller._update_torque_active(car_state, True, 0.0, self._handoff_actuators()) + + assert not controller.torque_active + assert not controller.driver_override_recovery + + def test_driver_override_does_not_hand_back_to_angle_mid_turn(self): + controller = self._handoff_controller(torque_active=True) + controller.driver_override_recovery = True + car_state = self._handoff_car_state(angle=ext_controller.HANDOFF_MAX_ANGLE_DEG) + + controller._update_torque_active( + car_state, + True, + ext_controller.HANDOFF_MAX_ANGLE_DEG, + self._handoff_actuators(), + ) + + assert controller.torque_active + + def test_inertial_torsion_peak_is_rejected_while_column_is_swinging(self): + controller = ExternalController.__new__(ExternalController) + controller.torsion_cnt = 0 + controller.torsion_sign = 0 + controller.rate_hist = ext_controller.deque( + [0.0] * ext_controller.TORSION_RATE_WINDOW, + maxlen=ext_controller.TORSION_RATE_WINDOW, + ) + + for _ in range(20): + assert not controller._update_torsion(8.0, 100.0, 4.0, 9) + + assert controller.torsion_cnt == 0 + + def test_driver_torsion_is_detected_when_column_is_settled(self): + controller = ExternalController.__new__(ExternalController) + controller.torsion_cnt = 0 + controller.torsion_sign = 0 + controller.rate_hist = ext_controller.deque( + [0.0] * ext_controller.TORSION_RATE_WINDOW, + maxlen=ext_controller.TORSION_RATE_WINDOW, + ) + + detected = False + for _ in range(10): + detected = controller._update_torsion(4.1, 0.0, 4.0, 9) + + assert detected + + def test_strong_driver_override_bypasses_inertial_torsion_rejection(self): + controller = ExternalController(self._car_params((0x1310,), gen2=True)) + car_state = SimpleNamespace( + out=SimpleNamespace( + steeringTorque=ext_controller.DRIVER_OVERRIDE_TORQUE + 0.1, + steeringRateDeg=200.0, + steeringPressed=True, + ), + sccm_wheel_touch=None, + hands_on_level=0, + ) + + for _ in range(ext_controller.DRIVER_OVERRIDE_FRAMES - 1): + controller._update_hands_on(car_state) + assert not controller.hands_on + + controller._update_hands_on(car_state) + + assert controller.hands_on + + def test_light_torsion_presence_delays_driver_hands_off_timer(self): + controller = ExternalController.__new__(ExternalController) + controller.torsion_lpf = ext_controller.FirstOrderFilter(0.0, ext_controller.PRESENCE_LPF_RC, 0.01) + controller.presence_cnt = 0 + controller.presence_sign = 0 + controller.presence_hold = 0 + + presence = False + for _ in range(ext_controller.PRESENCE_MIN_FRAMES + 20): + presence = controller._update_torsion_presence(2.0) + + assert presence + assert controller.presence_hold == ext_controller.PRESENCE_HOLD_FRAMES + + def test_gen2_hands_on_detection_does_not_require_sccm_message(self): + controller = ExternalController(self._car_params((0x1310,), gen2=True)) + car_state = SimpleNamespace( + out=SimpleNamespace(steeringTorque=0.0, steeringRateDeg=0.0), + sccm_wheel_touch=None, + hands_on_level=0, + ) + + controller._update_hands_on(car_state) + + assert not controller.hands_on + + def test_fresh_low_speed_engagement_starts_in_angle_when_epas_available(self): + controller = self._handoff_controller(lat_active_last=False) + car_state = self._handoff_car_state(speed=0.0) + + controller._update_torque_active(car_state, True, -25.4, self._handoff_actuators()) + + assert not controller.torque_active + + def test_fresh_engagement_starts_in_torque_when_epas_not_available(self): + controller = self._handoff_controller(lat_active_last=False) + car_state = self._handoff_car_state(eac_status=0) + + controller._update_torque_active(car_state, True, -25.4, self._handoff_actuators()) + + assert controller.torque_active + + def test_live_force_torque_prearms_before_releasing_angle(self): + controller = self._handoff_controller() + controller.force_torque = True + car_state = self._handoff_car_state(eac_status=2) + actuators = self._handoff_actuators(torque=0.5) + + controller._update_torque_active(car_state, True, 0.0, actuators) + + assert controller.torque_prearm + assert not controller.torque_active + + steer_max = round(float(ext_controller.np.interp( + car_state.out.vEgoRaw, + ext_controller.CCP.STEER_MAX_LOOKUP[0], + ext_controller.CCP.STEER_MAX_LOOKUP[1], + ))) + controller.apply_torque_last = round(0.5 * steer_max) + controller._update_torque_active(car_state, True, 0.0, actuators) + + assert controller.torque_active + assert not controller.torque_prearm + assert controller.galaxy_torque_recovery + assert controller.prearm_last_outcome == "reached" + + def test_live_force_torque_prearm_ramps_with_eac_active(self): + controller = self._handoff_controller() + controller.force_torque = True + controller.torque_cmd = 0 + controller.toi_act_cmd = False + controller.toi_controller = ToiController() + car_state = self._handoff_car_state(speed=33.8, eac_status=2) + car_state.toi_fault = False + car_state.toi_active = False + car_state.toi_unavailable = False + actuators = self._handoff_actuators(torque=0.103) + + for _ in range(10): + controller._update_torque_active(car_state, True, 0.0, actuators) + controller._update_torque(car_state, actuators) + assert controller.toi_act_cmd + assert controller.torque_cmd != 0 + assert not controller.toi_controller.recovery_failed + if controller.torque_active: + break + + assert controller.torque_active + assert not controller.torque_prearm + assert controller.prearm_last_outcome == "reached" + assert controller.toi_controller.state == ToiState.ACTIVATING + + car_state.toi_active = True + for _ in range(TOI_ACK_FRAMES): + controller._update_torque(car_state, actuators) + + assert controller.toi_controller.state == ToiState.TORQUE + assert controller.torque_cmd != 0 + + def test_cancel_live_force_during_prearm_keeps_angle_active(self): + controller = self._handoff_controller() + controller.force_torque = True + car_state = self._handoff_car_state(eac_status=2) + actuators = self._handoff_actuators(torque=0.5) + controller._update_torque_active(car_state, True, 0.0, actuators) + assert controller.torque_prearm + + controller.force_torque = False + controller._update_torque_active(car_state, True, 0.0, actuators) + + assert not controller.torque_prearm + assert not controller.torque_active + assert not controller.galaxy_torque_recovery + + def test_inhibited_no_error_rearms_once_after_continuous_release(self): + controller = self._handoff_controller(torque_active=True) + car_state = self._handoff_car_state(eac_status=0) + + for _ in range(ext_controller.EAC_REARM_RELEASE_FRAMES - 1): + controller._update_torque_active(car_state, True, 0.0, self._handoff_actuators()) + assert controller.torque_active + + controller._update_torque_active(car_state, True, 0.0, self._handoff_actuators()) + + assert not controller.torque_active + assert controller.eac_rearm_attempted + assert controller.eac_dead_frames == 1 + + def test_inhibited_rearm_release_window_resets_on_driver_input(self): + controller = self._handoff_controller(torque_active=True) + car_state = self._handoff_car_state(eac_status=0) + + for _ in range(ext_controller.EAC_REARM_RELEASE_FRAMES - 1): + controller._update_torque_active(car_state, True, 0.0, self._handoff_actuators()) + + car_state.out.steeringPressed = True + controller._update_torque_active(car_state, True, 0.0, self._handoff_actuators()) + assert controller.eac_rearm_release_frames == 0 + assert controller.torque_active + + car_state.out.steeringPressed = False + controller._update_torque_active(car_state, True, 0.0, self._handoff_actuators()) + assert controller.eac_rearm_release_frames == 1 + assert controller.torque_active + + def test_failed_inhibited_rearm_falls_back_without_repeated_probes(self): + controller = self._handoff_controller(torque_active=True) + car_state = self._handoff_car_state(eac_status=0) + + for _ in range(ext_controller.EAC_REARM_RELEASE_FRAMES): + controller._update_torque_active(car_state, True, 0.0, self._handoff_actuators()) + assert not controller.torque_active + + for _ in range(ext_controller.EAC_RECOVER_FRAMES - 1): + controller._update_torque_active(car_state, True, 0.0, self._handoff_actuators()) + assert not controller.torque_active + controller._update_torque_active(car_state, True, 0.0, self._handoff_actuators()) + assert controller.torque_active + + for _ in range(ext_controller.MIN_TORQUE_FRAMES + ext_controller.EAC_REARM_RELEASE_FRAMES): + controller._update_torque_active(car_state, True, 0.0, self._handoff_actuators()) + assert controller.torque_active + assert controller.eac_rearm_attempted + + def test_successful_inhibited_rearm_stays_in_angle(self): + controller = self._handoff_controller(torque_active=True) + car_state = self._handoff_car_state(eac_status=0) + + for _ in range(ext_controller.EAC_REARM_RELEASE_FRAMES): + controller._update_torque_active(car_state, True, 0.0, self._handoff_actuators()) + assert not controller.torque_active + + car_state.eac_status = 2 + controller._update_torque_active(car_state, True, 0.0, self._handoff_actuators()) + + assert not controller.torque_active + assert not controller.eac_rearm_attempted + assert controller.eac_dead_frames == 0 + + def test_inhibited_rearm_rejects_errors_and_faults(self): + for state in ( + self._handoff_car_state(eac_status=0, eac_error_code=1), + self._handoff_car_state(eac_status=3), + self._handoff_car_state(eac_status=0, temporary_fault=True), + self._handoff_car_state(eac_status=0, permanent_fault=True), + ): + controller = self._handoff_controller(torque_active=True) + for _ in range(ext_controller.MIN_TORQUE_FRAMES + ext_controller.EAC_REARM_RELEASE_FRAMES): + controller._update_torque_active(state, True, 0.0, self._handoff_actuators()) + assert controller.torque_active + assert not controller.eac_rearm_attempted + + def test_inhibited_rearm_requires_settled_wheel_near_command(self): + unsafe_states = ( + (self._handoff_car_state(eac_status=0, angle=100.0), 0.0), + (self._handoff_car_state(eac_status=0, rate=ext_controller.UNWIND_HANDOFF_RATE), 0.0), + (self._handoff_car_state(eac_status=0, angle=20.0), 0.0), + ) + for state, desired_angle in unsafe_states: + controller = self._handoff_controller(torque_active=True) + for _ in range(ext_controller.MIN_TORQUE_FRAMES + ext_controller.EAC_REARM_RELEASE_FRAMES): + controller._update_torque_active(state, True, desired_angle, self._handoff_actuators()) + assert controller.torque_active + assert not controller.eac_rearm_attempted + + def test_inhibited_rearm_budget_resets_after_disengagement(self): + controller = self._handoff_controller(torque_active=True) + car_state = self._handoff_car_state(eac_status=0) + + for _ in range(ext_controller.EAC_REARM_RELEASE_FRAMES): + controller._update_torque_active(car_state, True, 0.0, self._handoff_actuators()) + assert controller.eac_rearm_attempted + + controller._update_torque_active(car_state, False, 0.0, self._handoff_actuators()) + + assert not controller.torque_active + assert not controller.eac_rearm_attempted diff --git a/opendbc_repo/opendbc/car/rivian/toi_controller.py b/opendbc_repo/opendbc/car/rivian/toi_controller.py new file mode 100644 index 000000000..67bd3cbc7 --- /dev/null +++ b/opendbc_repo/opendbc/car/rivian/toi_controller.py @@ -0,0 +1,179 @@ +from enum import IntEnum + + +TOI_MAX_ANGLE_DEG = 90 +TOI_REARM_ANGLE_DEG = 80 +TOI_MAX_ANGLE_FRAMES = 89 +TOI_ACK_FRAMES = 2 +TOI_RECOVERY_TIMEOUT_FRAMES = 50 + + +class ToiState(IntEnum): + INACTIVE = 0 + TORQUE = 1 + RELEASING = 2 + REARMING = 3 + PREARMING = 4 + ACTIVATING = 5 + HIGH_ANGLE_LOCKOUT = 6 + + +class ToiController: + """Handshake Rivian's torque-overlay request with the EPAS status feedback.""" + + def __init__(self): + self.state = ToiState.INACTIVE + self.high_angle_frames = 0 + self.ack_frames = 0 + self.recovery_frames = 0 + self.recovery_failed = False + + @property + def recovering(self) -> bool: + return self.state in (ToiState.RELEASING, ToiState.REARMING, ToiState.HIGH_ANGLE_LOCKOUT) + + def _reset(self) -> None: + self.state = ToiState.INACTIVE + self.high_angle_frames = 0 + self.ack_frames = 0 + self.recovery_frames = 0 + self.recovery_failed = False + + def _start_release(self) -> None: + self.state = ToiState.RELEASING + self.high_angle_frames = 0 + self.ack_frames = 0 + self.recovery_frames = 0 + + def _start_rearm(self) -> None: + self.state = ToiState.REARMING + self.ack_frames = 0 + self.recovery_frames = 0 + + def _start_prearm(self) -> None: + self.state = ToiState.PREARMING + self.ack_frames = 0 + self.recovery_frames = 0 + + def _start_activation(self) -> None: + self.state = ToiState.ACTIVATING + self.ack_frames = 0 + self.recovery_frames = 0 + + def _start_high_angle_lockout(self) -> None: + self.state = ToiState.HIGH_ANGLE_LOCKOUT + self.ack_frames = 0 + self.recovery_frames = 0 + + def _finish_activation(self) -> None: + self.state = ToiState.TORQUE + self.high_angle_frames = 0 + self.ack_frames = 0 + self.recovery_frames = 0 + self.recovery_failed = False + + def update(self, requested: bool, high_angle: bool, toi_fault: bool, + toi_active: bool, toi_unavailable: bool, prearming: bool = False, + high_angle_rearm: bool | None = None, hold_high_angle_release: bool = False) -> tuple[bool, bool]: + """Return the request bit and whether non-zero torque may be sent.""" + high_angle_rearm = not high_angle if high_angle_rearm is None else high_angle_rearm + if not requested: + self._reset() + return False, False + + # A live angle->torque selection ramps the rate-limited torque command while + # EAC is still active. ToiActive cannot acknowledge until angle releases, so + # prearm must be distinguished from an ordinary zero-torque rearm. + if self.state == ToiState.INACTIVE: + if prearming: + self._start_prearm() + else: + self._start_rearm() + elif prearming and self.state == ToiState.REARMING: + self._start_prearm() + elif not prearming and self.state == ToiState.PREARMING: + self._start_activation() + + if self.state == ToiState.TORQUE: + if toi_fault or toi_unavailable: + self._start_release() + else: + self.high_angle_frames = self.high_angle_frames + 1 if high_angle else 0 + if self.high_angle_frames > TOI_MAX_ANGLE_FRAMES: + self._start_release() + + if self.state == ToiState.RELEASING: + self.recovery_frames += 1 + released = not toi_active and not toi_fault and not toi_unavailable + self.ack_frames = self.ack_frames + 1 if released else 0 + self.recovery_failed |= self.recovery_frames >= TOI_RECOVERY_TIMEOUT_FRAMES + if self.ack_frames >= TOI_ACK_FRAMES: + if hold_high_angle_release and not high_angle_rearm: + self._start_high_angle_lockout() + elif prearming: + self._start_prearm() + else: + self._start_rearm() + return False, False + + if self.state == ToiState.HIGH_ANGLE_LOCKOUT: + if toi_fault or toi_unavailable: + self.recovery_frames += 1 + self.recovery_failed |= self.recovery_frames >= TOI_RECOVERY_TIMEOUT_FRAMES + elif not high_angle_rearm: + self.recovery_frames = 0 + elif prearming: + self._start_prearm() + else: + self._start_rearm() + return False, False + + if self.state == ToiState.PREARMING: + if toi_fault or toi_unavailable: + self._start_release() + return False, False + + self.high_angle_frames = self.high_angle_frames + 1 if high_angle else 0 + if self.high_angle_frames > TOI_MAX_ANGLE_FRAMES: + self._start_release() + return False, False + return True, True + + if self.state == ToiState.ACTIVATING: + if toi_fault or toi_unavailable: + self._start_release() + return False, False + + self.high_angle_frames = self.high_angle_frames + 1 if high_angle else 0 + if self.high_angle_frames > TOI_MAX_ANGLE_FRAMES: + self._start_release() + return False, False + + self.recovery_frames += 1 + self.ack_frames = self.ack_frames + 1 if toi_active else 0 + if self.ack_frames >= TOI_ACK_FRAMES: + self._finish_activation() + elif self.recovery_frames >= TOI_RECOVERY_TIMEOUT_FRAMES: + self.recovery_failed = True + self._start_release() + return False, False + return True, True + + if self.state == ToiState.REARMING: + if toi_fault or toi_unavailable: + self._start_release() + return False, False + + self.recovery_frames += 1 + self.ack_frames = self.ack_frames + 1 if toi_active else 0 + if self.ack_frames >= TOI_ACK_FRAMES: + self._finish_activation() + elif self.recovery_frames >= TOI_RECOVERY_TIMEOUT_FRAMES: + # Drop the request before retrying instead of leaving an unacknowledged + # torque command asserted indefinitely. + self.recovery_failed = True + self._start_release() + return False, False + return True, False + + return True, True diff --git a/opendbc_repo/opendbc/car/rivian/values.py b/opendbc_repo/opendbc/car/rivian/values.py index 1686a3f6e..521eddecb 100644 --- a/opendbc_repo/opendbc/car/rivian/values.py +++ b/opendbc_repo/opendbc/car/rivian/values.py @@ -1,7 +1,8 @@ from dataclasses import dataclass, field from enum import StrEnum, IntFlag -from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, structs, uds +from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, structs, uds +from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 from opendbc.car.vin import Vin @@ -22,6 +23,7 @@ class ModelYear(StrEnum): P_2023 = "P" R_2024 = "R" S_2025 = "S" + T_2026 = "T" @dataclass @@ -33,23 +35,28 @@ class RivianCarDocs(CarDocs): @dataclass class RivianPlatformConfig(PlatformConfig): - dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'rivian_primary_actuator', Bus.radar: 'rivian_mando_front_radar_generated'}) + dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'rivian_primary_actuator', Bus.radar: 'rivian_mando_front_radar_generated', + Bus.alt: 'rivian_park_assist_can'}) wmis: set[WMI] = field(default_factory=set) lines: set[ModelLine] = field(default_factory=set) years: set[ModelYear] = field(default_factory=set) class CAR(Platforms): + # Retain the historical platform identifier for stored fingerprints and + # routes; RivianFlags.GEN2 distinguishes the vehicle generation at runtime. RIVIAN_R1_GEN1 = RivianPlatformConfig( # TODO: verify this [ RivianCarDocs("Rivian R1S 2022-24"), + RivianCarDocs("Rivian R1S 2025", setup_video=None, car_parts=CarParts.common([CarHarness.rivian_b])), RivianCarDocs("Rivian R1T 2022-24"), + RivianCarDocs("Rivian R1T 2025", setup_video=None, car_parts=CarParts.common([CarHarness.rivian_b])), ], CarSpecs(mass=3206., wheelbase=3.08, steerRatio=15.2), wmis={WMI.RIVIAN_TRUCK, WMI.RIVIAN_MPV}, lines={ModelLine.R1T, ModelLine.R1S}, - years={ModelYear.N_2022, ModelYear.P_2023, ModelYear.R_2024}, + years={ModelYear.N_2022, ModelYear.P_2023, ModelYear.R_2024, ModelYear.S_2025}, ) @@ -105,18 +112,22 @@ GEAR_MAP = { 4: structs.CarState.GearShifter.drive, } +AVERAGE_ROAD_ROLL = 0.06 # ~3.4 degrees, conservative banked-road allowance for angle safety + class CarControllerParams: # The R1T 2023 and R1S 2023 we tested on achieves slightly more lateral acceleration going left vs. right # and lateral acceleration falls linearly as speed decreases from 38 mph to 20 mph. These values are set # conservatively to reach a maximum of 3.0 m/s^2 turning left at 80 mph - # These refer to turning left: # 250 is ~2.8 m/s^2 above 17 m/s, then linearly ramps to ~1.6 m/s^2 from 17 m/s to 9 m/s # TODO: it is theorized older models have different steering racks and achieve down to half the # lateral acceleration referenced here at all speeds. detect this and ship a torque increase for those models - STEER_MAX = 250 # 350 is intended to maintain lateral accel, not increase it - STEER_MAX_LOOKUP = [9, 17], [350, 250] + # AdventurePilot's road-tested four-point tune preserves the original low- + # speed authority, lifts the 29-55 mph saturation band, and keeps the + # highway cap conservative. + STEER_MAX = 385 + STEER_MAX_LOOKUP = [9, 13, 25, 27], [385, 350, 295, 275] STEER_STEP = 1 STEER_DELTA_UP = 3 # torque increase per refresh STEER_DELTA_DOWN = 5 # torque decrease per refresh @@ -124,15 +135,37 @@ class CarControllerParams: STEER_DRIVER_MULTIPLIER = 2 # weight driver torque STEER_DRIVER_FACTOR = 100 + ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits( + 500, + ([], []), + ([], []), + MAX_LATERAL_ACCEL=ISO_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), + MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), + MAX_ANGLE_RATE=2.5, + ) + ACCEL_MIN = -3.5 # m/s^2 ACCEL_MAX = 2.0 # m/s^2 + # Compensate for the Rivian VDM's measured regen/creep drag without adding a + # noisy proportional term around wheel-speed-derived aEgo. The offset fades + # to zero at standstill so braking hold behavior is unchanged. + ACCEL_FF_DRAG_BP = [0.8, 3.0, 8.0, 13.0, 20.0, 30.0] + ACCEL_FF_DRAG_V = [0.0, 0.17, 0.17, 0.12, 0.10, 0.08] + def __init__(self, CP): pass class RivianSafetyFlags(IntFlag): LONG_CONTROL = 1 + ANGLE_CONTROL = 2 + + +class RivianFlags(IntFlag): + ANGLE_HARNESS = 1 + LONGITUDINAL_HARNESS = 2 + GEN2 = 4 DBC = CAR.create_dbc_map() diff --git a/opendbc_repo/opendbc/dbc/generator/rivian/rivian_mando_front_radar.dbc b/opendbc_repo/opendbc/dbc/generator/rivian/rivian_mando_front_radar.dbc index 6c42cd817..d26b11572 100644 --- a/opendbc_repo/opendbc/dbc/generator/rivian/rivian_mando_front_radar.dbc +++ b/opendbc_repo/opendbc/dbc/generator/rivian/rivian_mando_front_radar.dbc @@ -40,319 +40,414 @@ BO_ 1280 RADAR_TRACK_500: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1281 RADAR_TRACK_501: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1282 RADAR_TRACK_502: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1283 RADAR_TRACK_503: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1284 RADAR_TRACK_504: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1285 RADAR_TRACK_505: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1286 RADAR_TRACK_506: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1287 RADAR_TRACK_507: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1288 RADAR_TRACK_508: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1289 RADAR_TRACK_509: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1290 RADAR_TRACK_50a: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1291 RADAR_TRACK_50b: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1292 RADAR_TRACK_50c: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1293 RADAR_TRACK_50d: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1294 RADAR_TRACK_50e: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1295 RADAR_TRACK_50f: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1296 RADAR_TRACK_510: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1297 RADAR_TRACK_511: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1298 RADAR_TRACK_512: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1299 RADAR_TRACK_513: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1300 RADAR_TRACK_514: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1301 RADAR_TRACK_515: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1302 RADAR_TRACK_516: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1303 RADAR_TRACK_517: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1304 RADAR_TRACK_518: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1305 RADAR_TRACK_519: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1306 RADAR_TRACK_51a: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1307 RADAR_TRACK_51b: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1308 RADAR_TRACK_51c: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1309 RADAR_TRACK_51d: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1310 RADAR_TRACK_51e: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1311 RADAR_TRACK_51f: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - \ No newline at end of file + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + +VAL_ 1280 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1280 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1281 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1281 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1282 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1282 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1283 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1283 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1284 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1284 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1285 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1285 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1286 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1286 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1287 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1287 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1288 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1288 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1289 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1289 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1290 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1290 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1291 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1291 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1292 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1292 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1293 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1293 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1294 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1294 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1295 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1295 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1296 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1296 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1297 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1297 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1298 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1298 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1299 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1299 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1300 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1300 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1301 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1301 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1302 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1302 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1303 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1303 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1304 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1304 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1305 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1305 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1306 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1306 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1307 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1307 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1308 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1308 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1309 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1309 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1310 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1310 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1311 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1311 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; diff --git a/opendbc_repo/opendbc/dbc/generator/rivian/rivian_mando_front_radar.py b/opendbc_repo/opendbc/dbc/generator/rivian/rivian_mando_front_radar.py index cdddfd8f2..345297298 100755 --- a/opendbc_repo/opendbc/dbc/generator/rivian/rivian_mando_front_radar.py +++ b/opendbc_repo/opendbc/dbc/generator/rivian/rivian_mando_front_radar.py @@ -51,9 +51,15 @@ BO_ {a} RADAR_TRACK_{a:x}: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - """) + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX +""") + + for a in range(0x500, 0x500 + 32): + f.write(f""" +VAL_ {a} STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ {a} MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; +""") diff --git a/opendbc_repo/opendbc/dbc/rivian_mando_front_radar_generated.dbc b/opendbc_repo/opendbc/dbc/rivian_mando_front_radar_generated.dbc index 47388ed89..5415d479b 100644 --- a/opendbc_repo/opendbc/dbc/rivian_mando_front_radar_generated.dbc +++ b/opendbc_repo/opendbc/dbc/rivian_mando_front_radar_generated.dbc @@ -43,319 +43,414 @@ BO_ 1280 RADAR_TRACK_500: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1281 RADAR_TRACK_501: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1282 RADAR_TRACK_502: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1283 RADAR_TRACK_503: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1284 RADAR_TRACK_504: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1285 RADAR_TRACK_505: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1286 RADAR_TRACK_506: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1287 RADAR_TRACK_507: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1288 RADAR_TRACK_508: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1289 RADAR_TRACK_509: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1290 RADAR_TRACK_50a: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1291 RADAR_TRACK_50b: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1292 RADAR_TRACK_50c: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1293 RADAR_TRACK_50d: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1294 RADAR_TRACK_50e: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1295 RADAR_TRACK_50f: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1296 RADAR_TRACK_510: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1297 RADAR_TRACK_511: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1298 RADAR_TRACK_512: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1299 RADAR_TRACK_513: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1300 RADAR_TRACK_514: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1301 RADAR_TRACK_515: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1302 RADAR_TRACK_516: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1303 RADAR_TRACK_517: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1304 RADAR_TRACK_518: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1305 RADAR_TRACK_519: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1306 RADAR_TRACK_51a: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1307 RADAR_TRACK_51b: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1308 RADAR_TRACK_51c: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1309 RADAR_TRACK_51d: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1310 RADAR_TRACK_51e: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + BO_ 1311 RADAR_TRACK_51f: 8 RADAR SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX SG_ COUNTER : 11|4@0+ (1,0) [0|15] "" XXX SG_ UNKNOWN_1 : 23|8@0- (1,0) [-128|127] "" XXX - SG_ AZIMUTH : 28|10@0- (0.1,0) [-61.2|62.1] "" XXX + SG_ AZIMUTH : 28|10@0- (0.1,0) [-51.2|51.1] "" XXX SG_ STATE : 31|3@0+ (1,0) [0|7] "" XXX SG_ LONG_DIST : 34|11@0+ (0.1,0) [0|204.7] "" XXX - SG_ STATE_2 : 55|1@0+ (1,0) [0|1] "" XXX - SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX - \ No newline at end of file + SG_ MODE : 55|2@0+ (1,0) [0|3] "" XXX + SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "m/s" XXX + +VAL_ 1280 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1280 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1281 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1281 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1282 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1282 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1283 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1283 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1284 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1284 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1285 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1285 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1286 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1286 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1287 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1287 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1288 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1288 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1289 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1289 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1290 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1290 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1291 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1291 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1292 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1292 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1293 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1293 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1294 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1294 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1295 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1295 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1296 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1296 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1297 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1297 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1298 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1298 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1299 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1299 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1300 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1300 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1301 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1301 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1302 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1302 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1303 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1303 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1304 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1304 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1305 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1305 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1306 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1306 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1307 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1307 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1308 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1308 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1309 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1309 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1310 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1310 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; + +VAL_ 1311 STATE 0 "Empty" 1 "New" 2 "New_updated" 3 "Updated" 4 "Coasting" 7 "New_coasting" ; +VAL_ 1311 MODE 0 "None" 1 "SRR" 2 "LRR" 3 "SRR_and_LRR" ; diff --git a/opendbc_repo/opendbc/dbc/rivian_park_assist_can.dbc b/opendbc_repo/opendbc/dbc/rivian_park_assist_can.dbc index b18f0a53d..bc2156be7 100644 --- a/opendbc_repo/opendbc/dbc/rivian_park_assist_can.dbc +++ b/opendbc_repo/opendbc/dbc/rivian_park_assist_can.dbc @@ -51,5 +51,23 @@ BO_ 848 BSM_BlindSpotIndicator: 4 XXX SG_ BSM_BlindSpotIndicator_Left : 29|2@0+ (1,0) [0|3] "" XXX SG_ BSM_BlindSpotIndicator_Right : 30|2@1+ (1,0) [0|3] "" XXX +BO_ 4890 WheelButtons_Fwd: 7 XXX + SG_ LeftButton_ScrollClick : 19|2@0+ (1,0) [0|3] "" XXX + SG_ LeftButton_RightClick : 21|2@0+ (1,0) [0|3] "" XXX + SG_ LeftButton_LeftClick : 22|2@1+ (1,0) [0|3] "" XXX + SG_ LeftButton_Scroll : 31|8@0+ (1,0) [0|255] "" XXX + SG_ RightButton_ScrollClick : 35|2@0+ (1,0) [0|3] "" XXX + SG_ RightButton_RightClick : 37|2@0+ (1,0) [0|3] "" XXX + SG_ RightButton_LeftClick : 38|2@1+ (1,0) [0|3] "" XXX + SG_ RightButton_Scroll : 47|8@0+ (1,0) [0|255] "" XXX + +BO_ 4944 BSM_BlindSpotIndicator_Fwd: 4 XXX + SG_ BSM_BlindSpotIndicator_Checksum : 0|8@1+ (1,0) [0|255] "" XXX + SG_ BSM_BlindSpotIndicator_Counter : 11|4@0+ (1,0) [0|15] "" XXX + SG_ BSM_BlindSpotIndicator_Left : 29|2@0+ (1,0) [0|3] "" XXX + SG_ BSM_BlindSpotIndicator_Right : 30|2@1+ (1,0) [0|3] "" XXX + VAL_ 848 BSM_BlindSpotIndicator_Left 0 "OFF" 1 "OBJECT_DETECTED" 2 "ACTIVE_WARNING" ; -VAL_ 848 BSM_BlindSpotIndicator_Right 0 "OFF" 1 "OBJECT_DETECTED" 2 "ACTIVE_WARNING" ; \ No newline at end of file +VAL_ 848 BSM_BlindSpotIndicator_Right 0 "OFF" 1 "OBJECT_DETECTED" 2 "ACTIVE_WARNING" ; +VAL_ 4944 BSM_BlindSpotIndicator_Left 0 "OFF" 1 "OBJECT_DETECTED" 2 "ACTIVE_WARNING" ; +VAL_ 4944 BSM_BlindSpotIndicator_Right 0 "OFF" 1 "OBJECT_DETECTED" 2 "ACTIVE_WARNING" ; diff --git a/opendbc_repo/opendbc/dbc/rivian_primary_actuator.dbc b/opendbc_repo/opendbc/dbc/rivian_primary_actuator.dbc index 1e0112432..9845bf24e 100644 --- a/opendbc_repo/opendbc/dbc/rivian_primary_actuator.dbc +++ b/opendbc_repo/opendbc/dbc/rivian_primary_actuator.dbc @@ -343,7 +343,8 @@ BO_ 801 SCCM_WheelTouch: 7 SCCM SG_ SCCM_WheelTouch_Counter : 11|4@0+ (1,0) [0|15] "" XXX SG_ SCCM_WheelTouch_HandsOn : 21|1@0+ (1,0) [0|1] "" XXX SG_ SETME_X52 : 31|8@0+ (1,0) [0|255] "" XXX - SG_ SCCM_WheelTouch_CapacitiveValue : 32|12@1+ (1,0) [0|4095] "" XXX + SG_ SCCM_WheelTouch_CapacitiveValue : 32|8@1+ (1,0) [0|4095] "" XXX + SG_ SCCM_WheelTouch_CapacitiveValue2 : 47|8@0+ (1,0) [0|255] "" XXX BO_ 811 ESP_EpbStatus: 8 ESP SG_ ESP_EpbStatus_Checksum : 7|8@0+ (1,0) [0|255] "" ACM,VDM @@ -881,6 +882,7 @@ VAL_ 354 VDM_AdasFaultStatus 0 "VDM_AdasFlaultStatus_No_Fault" 1 "VDM_AdasFaultS VAL_ 354 VDM_AdasDriverModeStatus 0 "VDM_AdasDriverModeStatus_Human" 1 "VDM_AdasDriverModeStatus_Adas" 2 "VDM_AdasDriverModeStatus_Reserved" 3 "VDM_AdasDriverModeStatus_Sna"; VAL_ 354 VDM_AdasInterfaceStatus 0 "VDM_AdasInterfaceStatus_Unavailable" 1 "VDM_AdasInterfaceStatus_Available" 2 "VDM_AdasInterfaceStatus_Enabled" 3 "VDM_AdasInterfaceStatus_Faulted"; VAL_ 354 VDM_AdasVehicleHoldStatus 0 "VDM_AdasVehicleHoldStatus_NotHold" 1 "VDM_AdasVehicleHoldStatus_Hold"; +VAL_ 354 VDM_UserAdasRequest 0 "IDLE" 1 "UP_1" 2 "UP_2" 3 "DOWN_1" 4 "DOWN_2"; VAL_ 357 VDM_AdasStalkGapAdjust 0 "VDM_AdasStalkGapAdjust_No_Required" 1 "VDM_AdasStalkGapAdjust_GapDecrement" 2 "VDM_AdasStalkGapAdjust_GapIncrement"; VAL_ 357 VDM_AdasStalkAccCancelRes 0 "VDM_AdasStalkAccCancelRes_NoRequired" 1 "VDM_AdasStalkAccCancelRes_Cancel" 2 "VDM_AdasStalkAccCancelRes_Resume"; VAL_ 357 VDM_AdasStalkAutonomyButton 0 "VDM_AdasStalkAutonomyButton_No_Required" 1 "VDM_AdasStalkAutonomyButton_Pressed"; diff --git a/opendbc_repo/opendbc/safety/modes/rivian.h b/opendbc_repo/opendbc/safety/modes/rivian.h index b9f488ae6..9934b82ae 100644 --- a/opendbc_repo/opendbc/safety/modes/rivian.h +++ b/opendbc_repo/opendbc/safety/modes/rivian.h @@ -2,6 +2,8 @@ #include "opendbc/safety/declarations.h" +static bool rivian_angle_control = false; + static uint8_t rivian_get_counter(const CANPacket_t *msg) { uint8_t cnt = 0; if ((msg->addr == 0x208U) || (msg->addr == 0x150U)) { @@ -86,6 +88,13 @@ static void rivian_rx_hook(const CANPacket_t *msg) { update_sample(&torque_driver, torque_driver_new); } + // Measured steering angle from EPAS, stored in 0.1 degrees to match + // ACM_SteeringAngleRequest and angle_deg_to_can. + if (msg->addr == 0x390U) { + int angle_meas_new = ((msg->data[5] << 6) | (msg->data[6] >> 2)) - 8192U; + update_sample(&angle_meas, angle_meas_new); + } + // Brake pressed if (msg->addr == 0x38fU) { brake_pressed = (msg->data[2] >> 7) & 1U; @@ -97,20 +106,33 @@ static void rivian_rx_hook(const CANPacket_t *msg) { if (msg->addr == 0x100U) { const int feature_status = msg->data[2] >> 5U; pcm_cruise_check(feature_status == 1); - acc_main_on = (feature_status == 0) || (feature_status == 1); } } } static bool rivian_tx_hook(const CANPacket_t *msg) { - // Rivian utilizes more torque at low speed to maintain the same lateral accel + const AngleSteeringLimits RIVIAN_ANGLE_STEERING_LIMITS = { + .max_angle = 5000, + .angle_deg_to_can = 10, + .frequency = 100U, + }; + + const AngleSteeringParams RIVIAN_ANGLE_STEERING_PARAMS = { + .slip_factor = -0.0005445721739802007, + .steer_ratio = 15.2, + .wheelbase = 3.08, + }; + const TorqueSteeringLimits RIVIAN_STEERING_LIMITS = { - .max_torque = 350, + .max_torque = 385, .dynamic_max_torque = true, + // Conservative three-point envelope around the software's four-point + // [9,13,25,27] -> [385,350,295,275] tune. This curve is never below the + // software lookup, so Panda permits every command the controller can emit. .max_torque_lookup = { - {9., 17., 17.}, - {350, 250, 250}, + {9., 25., 27.}, + {385, 295, 275}, }, .max_rate_up = 3, .max_rate_down = 5, @@ -118,6 +140,10 @@ static bool rivian_tx_hook(const CANPacket_t *msg) { .driver_torque_multiplier = 2, .driver_torque_allowance = 100, .type = TorqueDriverLimited, + .min_valid_request_frames = 89, + .max_invalid_request_frames = 2, + .min_valid_request_rt_interval = 810000, + .has_steer_req_tolerance = true, }; const LongitudinalLimits RIVIAN_LONG_LIMITS = { @@ -129,6 +155,18 @@ static bool rivian_tx_hook(const CANPacket_t *msg) { bool tx = true; if (msg->bus == 0U) { + // Angle steering supplied only by the Extreme harness bridge. + if (msg->addr == 0x110U) { + int desired_angle = ((msg->data[2] << 7) | (msg->data[3] >> 1)) - 16384U; + bool angle_active = GET_BIT(msg, 12U); + const bool angle_violation = !rivian_angle_control || steer_angle_cmd_checks_vm(desired_angle, angle_active, + RIVIAN_ANGLE_STEERING_LIMITS, + RIVIAN_ANGLE_STEERING_PARAMS); + if (angle_violation) { + tx = false; + } + } + // Steering control if (msg->addr == 0x120U) { int desired_torque = ((msg->data[2] << 3U) | (msg->data[3] >> 5U)) - 1024U; @@ -156,20 +194,24 @@ static safety_config rivian_init(uint16_t param) { // VDM_AdasSts: for canceling stock ACC // 0x120 = ACM_lkaHbaCmd, 0x321 = SCCM_WheelTouch, 0x162 = VDM_AdasSts static const CanMsg RIVIAN_TX_MSGS[] = {{0x120, 0, 8, .check_relay = true}, {0x321, 2, 7, .check_relay = true}, {0x162, 2, 8, .check_relay = true}}; + static const CanMsg RIVIAN_ANGLE_TX_MSGS[] = {{0x100, 0, 8, .check_relay = true}, {0x110, 0, 8, .check_relay = true}, {0x120, 0, 8, .check_relay = true}, {0x321, 2, 7, .check_relay = true}, {0x162, 2, 8, .check_relay = true}}; // 0x160 = ACM_longitudinalRequest static const CanMsg RIVIAN_LONG_TX_MSGS[] = {{0x120, 0, 8, .check_relay = true}, {0x321, 2, 7, .check_relay = true}, {0x160, 0, 5, .check_relay = true}}; + static const CanMsg RIVIAN_ANGLE_LONG_TX_MSGS[] = {{0x100, 0, 8, .check_relay = true}, {0x110, 0, 8, .check_relay = true}, {0x120, 0, 8, .check_relay = true}, {0x321, 2, 7, .check_relay = true}, {0x160, 0, 5, .check_relay = true}}; static RxCheck rivian_rx_checks[] = { {.msg = {{0x208, 0, 8, 50U, .max_counter = 14U}, { 0 }, { 0 }}}, // ESP_Status (speed) {.msg = {{0x150, 0, 7, 50U, .max_counter = 14U}, { 0 }, { 0 }}}, // VDM_PropStatus (gas pedal & 2nd speed) {.msg = {{0x380, 0, 5, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // EPAS_SystemStatus (driver torque) + {.msg = {{0x390, 0, 7, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // EPAS_AdasStatus (measured angle) {.msg = {{0x38f, 0, 6, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // iBESP2 (brakes) {.msg = {{0x100, 2, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // ACM_Status (cruise state) }; bool rivian_longitudinal = false; + const int FLAG_RIVIAN_ANGLE_CONTROL = 2; + rivian_angle_control = GET_FLAG(param, FLAG_RIVIAN_ANGLE_CONTROL); - SAFETY_UNUSED(param); #ifdef ALLOW_DEBUG const int FLAG_RIVIAN_LONG_CONTROL = 1; rivian_longitudinal = GET_FLAG(param, FLAG_RIVIAN_LONG_CONTROL); @@ -178,8 +220,17 @@ static safety_config rivian_init(uint16_t param) { // FIXME: cppcheck thinks that rivian_longitudinal is always false. This is not true // if ALLOW_DEBUG is defined but cppcheck is run without ALLOW_DEBUG // cppcheck-suppress knownConditionTrueFalse - return rivian_longitudinal ? BUILD_SAFETY_CFG(rivian_rx_checks, RIVIAN_LONG_TX_MSGS) : \ - BUILD_SAFETY_CFG(rivian_rx_checks, RIVIAN_TX_MSGS); + safety_config config; + if (rivian_longitudinal && rivian_angle_control) { + config = BUILD_SAFETY_CFG(rivian_rx_checks, RIVIAN_ANGLE_LONG_TX_MSGS); + } else if (rivian_longitudinal) { + config = BUILD_SAFETY_CFG(rivian_rx_checks, RIVIAN_LONG_TX_MSGS); + } else if (rivian_angle_control) { + config = BUILD_SAFETY_CFG(rivian_rx_checks, RIVIAN_ANGLE_TX_MSGS); + } else { + config = BUILD_SAFETY_CFG(rivian_rx_checks, RIVIAN_TX_MSGS); + } + return config; } const safety_hooks rivian_hooks = { diff --git a/opendbc_repo/opendbc/safety/tests/common.py b/opendbc_repo/opendbc/safety/tests/common.py index 55943c234..811dce322 100644 --- a/opendbc_repo/opendbc/safety/tests/common.py +++ b/opendbc_repo/opendbc/safety/tests/common.py @@ -987,6 +987,8 @@ class SafetyTest(SafetyTestBase): continue if attr.startswith('TestHyundaiCanfd') and current_test.startswith('TestHyundaiCanfd'): continue + if attr.startswith('TestRivian') and current_test.startswith('TestRivian'): + continue if attr.startswith('TestHyundaiCanCanfdBlended') and current_test.startswith('TestHyundaiCanCanfdBlended'): continue if {attr, current_test}.issubset({'TestHyundaiLongitudinalSafety', 'TestHyundaiLongitudinalSafetyCameraSCC', @@ -1003,6 +1005,13 @@ class SafetyTest(SafetyTestBase): if attr == 'TestHyundaiCanfdLKASteeringLongEV' and current_test.startswith('TestToyota'): tx = list(filter(lambda m: m[0] not in [0x160, ], tx)) + # Rivian and Hyundai angle-control safety modes intentionally share these CAN address/bus pairs. + rivian_angle_tests = {'TestRivianAngleSafety', 'TestRivianAngleLongitudinalSafety'} + hyundai_alt_angle_test = 'TestHyundaiCanfdLKASteeringAltAngleLongEV' + if ((current_test in rivian_angle_tests and attr == hyundai_alt_angle_test) or + (attr in rivian_angle_tests and current_test == hyundai_alt_angle_test)): + tx = list(filter(lambda m: [m[0], m[1]] not in ([0x100, 0], [0x110, 0]), tx)) + # Volkswagen MQB longitudinal actuating message overlaps with the Subaru lateral actuating message if attr == 'TestVolkswagenMqbLongSafety' and current_test.startswith('TestSubaru'): tx = list(filter(lambda m: m[0] not in [0x122, ], tx)) diff --git a/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py b/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py index 6887988ae..25ff3c09b 100644 --- a/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py +++ b/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py @@ -1,5 +1,6 @@ from opendbc.car.ford.values import FordSafetyFlags from opendbc.car.hyundai.values import HyundaiSafetyFlags +from opendbc.car.rivian.values import RivianSafetyFlags from opendbc.car.subaru.values import SubaruSafetyFlags from opendbc.car.toyota.values import ToyotaSafetyFlags from opendbc.car.structs import CarParams @@ -41,7 +42,7 @@ def is_steering_msg(mode, param, addr): elif mode == CarParams.SafetyModel.nissan: ret = addr == 0x169 elif mode == CarParams.SafetyModel.rivian: - ret = addr == 0x120 + ret = addr == (0x110 if param & RivianSafetyFlags.ANGLE_CONTROL else 0x120) elif mode == CarParams.SafetyModel.tesla: ret = addr == 0x488 return ret @@ -94,7 +95,10 @@ def get_steer_value(mode, param, msg): angle = (msg.data[0] << 10) | (msg.data[1] << 2) | (msg.data[2] >> 6) angle = -angle + (1310 * 100) elif mode == CarParams.SafetyModel.rivian: - torque = ((msg.data[2] << 3) | (msg.data[3] >> 5)) - 1024 + if param & RivianSafetyFlags.ANGLE_CONTROL: + angle = ((msg.data[2] << 7) | (msg.data[3] >> 1)) - 16384 + else: + torque = ((msg.data[2] << 3) | (msg.data[3] >> 5)) - 1024 elif mode == CarParams.SafetyModel.tesla: angle = (((msg.data[0] & 0x7F) << 8) | (msg.data[1])) - 16384 # ceil(1638.35/0.1) return torque, angle diff --git a/opendbc_repo/opendbc/safety/tests/test_rivian.py b/opendbc_repo/opendbc/safety/tests/test_rivian.py index f1393e605..32fc22372 100755 --- a/opendbc_repo/opendbc/safety/tests/test_rivian.py +++ b/opendbc_repo/opendbc/safety/tests/test_rivian.py @@ -1,7 +1,12 @@ #!/usr/bin/env python3 import unittest +import numpy as np +from opendbc.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness +from opendbc.car.lateral import get_max_angle_delta_vm, get_max_angle_vm from opendbc.car.structs import CarParams +from opendbc.car.vehicle_model import VehicleModel +from opendbc.car.rivian.values import CarControllerParams from opendbc.safety.tests.libsafety import libsafety_py import opendbc.safety.tests.common as common from opendbc.safety.tests.common import CANPackerSafety @@ -9,6 +14,18 @@ from opendbc.car.rivian.values import RivianSafetyFlags from opendbc.car.rivian.riviancan import checksum as _checksum +def get_safety_vm(): + CP = CarParams() + CP.mass = 3206. + STD_CARGO_KG + CP.wheelbase = 3.08 + CP.centerToFront = CP.wheelbase * 0.5 + CP.steerRatio = 15.2 + CP.steerRatioRear = 0. + CP.rotationalInertia = scale_rot_inertia(CP.mass, CP.wheelbase) + CP.tireStiffnessFront, CP.tireStiffnessRear = scale_tire_stiffness(CP.mass, CP.wheelbase, CP.centerToFront, 1.0) + return VehicleModel(CP) + + def checksum(msg): addr, dat, bus = msg ret = bytearray(dat) @@ -18,18 +35,20 @@ def checksum(msg): ret[0] = _checksum(ret[1:], 0x1D, 0xB1) elif addr == 0x150: ret[0] = _checksum(ret[1:], 0x1D, 0x9A) + elif addr == 0x162: + ret[0] = _checksum(ret[1:], 0x1D, 0xD1) return addr, ret, bus -class TestRivianSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTest, common.LongitudinalAccelSafetyTest, - common.VehicleSpeedSafetyTest): +class TestRivianSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeringSafetyTest, common.SteerRequestCutSafetyTest, + common.LongitudinalAccelSafetyTest, common.VehicleSpeedSafetyTest): TX_MSGS = [[0x120, 0], [0x321, 2], [0x162, 2]] RELAY_MALFUNCTION_ADDRS = {0: (0x120,), 2: (0x321, 0x162)} FWD_BLACKLISTED_ADDRS = {0: [0x321, 0x162], 2: [0x120]} - MAX_TORQUE_LOOKUP = [9, 17], [350, 250] + MAX_TORQUE_LOOKUP = [9, 25, 27], [385, 295, 275] DYNAMIC_MAX_TORQUE = True MAX_RATE_UP = 3 MAX_RATE_DOWN = 5 @@ -38,6 +57,10 @@ class TestRivianSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeringSafe DRIVER_TORQUE_ALLOWANCE = 100 DRIVER_TORQUE_FACTOR = 2 + MIN_VALID_STEERING_FRAMES = 89 + MAX_INVALID_STEERING_FRAMES = 2 + + ANGLE = False cnt_speed = 0 cnt_speed_2 = 0 @@ -109,6 +132,92 @@ class TestRivianSafetyBase(common.CarSafetyTest, common.DriverTorqueSteeringSafe self.assertFalse(self._rx(msg)) self.assertFalse(self.safety.get_controls_allowed()) + def test_angle_messages_require_harness_flag(self): + if self.ANGLE: + raise unittest.SkipTest("Angle harness safety configuration") + self.assertFalse(self._tx(self.packer.make_can_msg_safety("ACM_SteeringControl", 0, {}))) + self.assertFalse(self._tx(self.packer.make_can_msg_safety("ACM_Status", 0, {}))) + + +class TestRivianAngleSafetyBase(TestRivianSafetyBase, common.AngleSteeringSafetyTest): + ANGLE = True + LONGITUDINAL = False + TX_MSGS = [[0x100, 0], [0x110, 0], [0x120, 0], [0x321, 2], [0x162, 2]] + RELAY_MALFUNCTION_ADDRS = {0: (0x100, 0x110, 0x120), 2: (0x321, 0x162)} + FWD_BLACKLISTED_ADDRS = {0: [0x321, 0x162], 2: [0x100, 0x110, 0x120]} + + STEER_ANGLE_MAX = 500 + DEG_TO_CAN = 10 + ANGLE_RATE_BP = None + ANGLE_RATE_UP = None + ANGLE_RATE_DOWN = None + LATERAL_FREQUENCY = 100 + cnt_angle_cmd = 0 + + def _get_steer_cmd_angle_max(self, speed): + return get_max_angle_vm(max(speed, 1), self.VM, CarControllerParams) + + def _angle_cmd_msg(self, angle: float, enabled: bool, increment_timer: bool = True): + values = {"ACM_SteeringAngleRequest": angle, "ACM_EacEnabled": enabled} + if increment_timer: + self.safety.set_timer(self.cnt_angle_cmd * int(1e6 / self.LATERAL_FREQUENCY)) + self.__class__.cnt_angle_cmd += 1 + return self.packer.make_can_msg_safety("ACM_SteeringControl", 0, values) + + def _angle_meas_msg(self, angle: float): + return self.packer.make_can_msg_safety("EPAS_AdasStatus", 0, {"EPAS_InternalSas": angle}) + + def test_angle_cmd_when_enabled(self): + # The VM-based lateral acceleration and jerk limits are exercised below. + pass + + def _can_to_deg(self, can_value): + return can_value / self.DEG_TO_CAN + + @staticmethod + def _round_speed(speed): + speed_kph_can = round((speed + 1) * 3.6 / 0.01) * 0.01 + stored = round(speed_kph_can / 3.6 * 1000) + return max(stored / 1000.0 - 1.0, 1.0) + + def test_lateral_accel_limit(self): + for speed in np.linspace(0, 40, 100): + speed = max(self._round_speed(speed), 1) + for sign in (-1, 1): + self.safety.set_controls_allowed(True) + self._reset_speed_measurement(speed + 1) + max_angle_can = int(get_max_angle_vm(speed, self.VM, CarControllerParams) * self.DEG_TO_CAN) + 1 + max_angle_can = min(max_angle_can, self.STEER_ANGLE_MAX * self.DEG_TO_CAN) + + self.safety.set_desired_angle_last(max_angle_can * sign) + self.assertTrue(self._tx(self._angle_cmd_msg(self._can_to_deg(max_angle_can) * sign, True))) + + above_can = max_angle_can + 1 + above_deg = self._can_to_deg(above_can) * sign + self._tx(self._angle_cmd_msg(above_deg, True)) + should_tx = above_can > self.STEER_ANGLE_MAX * self.DEG_TO_CAN + self.assertEqual(should_tx, self._tx(self._angle_cmd_msg(above_deg, True))) + + def test_lateral_jerk_limit(self): + for speed in np.linspace(0, 40, 100): + speed = max(self._round_speed(speed), 1) + for sign in (-1, 1): + self.safety.set_controls_allowed(True) + self._reset_speed_measurement(speed + 1) + self._tx(self._angle_cmd_msg(0, True)) + max_delta_can = int(get_max_angle_delta_vm(speed, self.VM, CarControllerParams) * self.DEG_TO_CAN) + 1 + + self.assertTrue(self._tx(self._angle_cmd_msg(self._can_to_deg(max_delta_can) * sign, True))) + self.assertTrue(self._tx(self._angle_cmd_msg(self._can_to_deg(max_delta_can) * sign, True))) + self.assertTrue(self._tx(self._angle_cmd_msg(0, True))) + + above_can = max_delta_can + 1 + self.assertFalse(self._tx(self._angle_cmd_msg(self._can_to_deg(above_can) * sign, True))) + self.safety.set_desired_angle_last(above_can * sign) + self.assertTrue(self._tx(self._angle_cmd_msg(self._can_to_deg(above_can) * sign, True))) + self.assertFalse(self._tx(self._angle_cmd_msg(0, True))) + self.assertTrue(self._tx(self._angle_cmd_msg(0, True))) + class TestRivianStockSafety(TestRivianSafetyBase): @@ -142,5 +251,29 @@ class TestRivianLongitudinalSafety(TestRivianSafetyBase): self.safety.init_tests() +class TestRivianAngleSafety(TestRivianAngleSafetyBase): + def setUp(self): + self.VM = get_safety_vm() + self.packer = CANPackerSafety("rivian_primary_actuator") + self.safety = libsafety_py.libsafety + self.safety.set_safety_hooks(CarParams.SafetyModel.rivian, RivianSafetyFlags.ANGLE_CONTROL) + self.safety.init_tests() + + +class TestRivianAngleLongitudinalSafety(TestRivianAngleSafetyBase): + LONGITUDINAL = True + TX_MSGS = [[0x100, 0], [0x110, 0], [0x120, 0], [0x321, 2], [0x160, 0]] + RELAY_MALFUNCTION_ADDRS = {0: (0x100, 0x110, 0x120, 0x160), 2: (0x321,)} + FWD_BLACKLISTED_ADDRS = {0: [0x321], 2: [0x100, 0x110, 0x120, 0x160]} + + def setUp(self): + self.VM = get_safety_vm() + self.packer = CANPackerSafety("rivian_primary_actuator") + self.safety = libsafety_py.libsafety + flags = RivianSafetyFlags.ANGLE_CONTROL | RivianSafetyFlags.LONG_CONTROL + self.safety.set_safety_hooks(CarParams.SafetyModel.rivian, flags) + self.safety.init_tests() + + if __name__ == "__main__": unittest.main() diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py index f3df75ce4..e48a90fa0 100644 --- a/selfdrive/car/car_specific.py +++ b/selfdrive/car/car_specific.py @@ -4,6 +4,7 @@ from opendbc.car import DT_CTRL, structs from opendbc.car.chrysler.values import RAM_DT from opendbc.car.gm.values import CAR as GM_CAR, GMFlags, SDGM_CAR from opendbc.car.interfaces import MAX_CTRL_SPEED +from opendbc.car.rivian.values import RivianFlags from openpilot.selfdrive.selfdrived.events import Events @@ -65,6 +66,21 @@ class CarSpecificEvents: self.gm_low_speed_alert_shown = False self.no_steer_warning = False self.silent_steer_warning = True + self.rivian = self.CP.brand == "rivian" + self.rivian_angle_harness = self.rivian and bool(self.CP.flags & RivianFlags.ANGLE_HARNESS) + self.rivian_status_frame = 0 + self.rivian_angle_saturated = False + self.rivian_toi_recovery_failed = False + self.rivian_status_params = None + self.rivian_angle_params = None + if self.rivian: + try: + from openpilot.common.params import Params + self.rivian_status_params = Params(memory=True) + if self.rivian_angle_harness: + self.rivian_angle_params = self.rivian_status_params + except Exception: + pass def update(self, CS: car.CarState, CS_prev: car.CarState, CC: car.CarControl): extra_gears = BRAND_EXTRA_GEARS.get(self.CP.brand, None) @@ -186,6 +202,18 @@ class CarSpecificEvents: else: events = self.create_common_events(CS, CS_prev, extra_gears=extra_gears) + if self.rivian: + self.rivian_status_frame += 1 + if self.rivian_status_params is not None and self.rivian_status_frame % 5 == 0: + self.rivian_toi_recovery_failed = self.rivian_status_params.get_bool("RivianToiRecoveryFailed") + if self.rivian_angle_harness: + self.rivian_angle_saturated = self.rivian_status_params.get_bool("RivianAngleSaturated") + if self.rivian_toi_recovery_failed: + events.add(EventName.steerTempUnavailable) + if self.rivian_angle_harness: + if self.rivian_angle_saturated: + events.add(EventName.steerSaturated) + return events def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears: list | None = None, pcm_enable=True, diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index bec0ed642..3ebdad816 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -226,7 +226,10 @@ class Car: self.starpilot_card = StarPilotCard(self.CP, self.FPCP) - self.sm = self.sm.extend(['starpilotOnroadEvents', 'starpilotPlan', 'starpilotSelfdriveState', 'liveCalibration', 'selfdriveState']) + starpilot_services = ['starpilotOnroadEvents', 'starpilotPlan', 'starpilotSelfdriveState', 'liveCalibration', 'selfdriveState'] + if self.CP.brand == "rivian": + starpilot_services.append('liveParameters') + self.sm = self.sm.extend(starpilot_services) self.pm = self.pm.extend(['starpilotCarState']) def _inject_favorite_virtual_cruise_events(self, CS: car.CarState) -> None: @@ -416,6 +419,10 @@ class Car: now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) self._update_redneck_cruise(CS, CC) self._update_openpilot_lead_state(CC) + if self.CP.brand == "rivian" and self.sm.all_checks(['liveParameters']) and hasattr(self.CI.CC, 'update_live_params'): + live_params = self.sm['liveParameters'] + self.CI.CC.update_live_params(live_params.roll, live_params.angleOffsetDeg, + live_params.stiffnessFactor, live_params.steerRatio) self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos, self.starpilot_toggles) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) diff --git a/selfdrive/car/tests/test_rivian_angle_saturation.py b/selfdrive/car/tests/test_rivian_angle_saturation.py new file mode 100644 index 000000000..a659b3e46 --- /dev/null +++ b/selfdrive/car/tests/test_rivian_angle_saturation.py @@ -0,0 +1,68 @@ +import importlib.util +from pathlib import Path +import sys +from types import ModuleType, SimpleNamespace + + +MODULE_PATH = Path(__file__).resolve().parents[1] / "car_specific.py" + + +class FakeEvents: + def __init__(self): + self.names = [] + + def add(self, event): + self.names.append(event) + + +def load_car_specific(monkeypatch): + messaging = ModuleType("cereal.messaging") + messaging.SubMaster = object + monkeypatch.setitem(sys.modules, "cereal.messaging", messaging) + + events = ModuleType("openpilot.selfdrive.selfdrived.events") + events.Events = FakeEvents + monkeypatch.setitem(sys.modules, "openpilot.selfdrive.selfdrived.events", events) + + spec = importlib.util.spec_from_file_location("rivian_car_specific_under_test", MODULE_PATH) + assert spec is not None and spec.loader is not None + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +def test_angle_saturation_raises_stock_take_control_event(monkeypatch): + module = load_car_specific(monkeypatch) + handler = module.CarSpecificEvents(SimpleNamespace(brand="rivian", flags=1)) + handler.rivian_status_params = SimpleNamespace(get_bool=lambda key: key == "RivianAngleSaturated") + handler.rivian_angle_params = handler.rivian_status_params + handler.create_common_events = lambda *args, **kwargs: FakeEvents() + + events = None + for _ in range(5): + events = handler.update(SimpleNamespace(), SimpleNamespace(), SimpleNamespace()) + + assert module.EventName.steerSaturated in events.names + + +def test_saturation_bridge_is_inert_without_angle_harness(monkeypatch): + module = load_car_specific(monkeypatch) + handler = module.CarSpecificEvents(SimpleNamespace(brand="rivian", flags=0)) + handler.create_common_events = lambda *args, **kwargs: FakeEvents() + + events = handler.update(SimpleNamespace(), SimpleNamespace(), SimpleNamespace()) + + assert module.EventName.steerSaturated not in events.names + + +def test_toi_recovery_timeout_raises_temporary_steering_event(monkeypatch): + module = load_car_specific(monkeypatch) + handler = module.CarSpecificEvents(SimpleNamespace(brand="rivian", flags=0)) + handler.rivian_status_params = SimpleNamespace(get_bool=lambda key: key == "RivianToiRecoveryFailed") + handler.create_common_events = lambda *args, **kwargs: FakeEvents() + + events = None + for _ in range(5): + events = handler.update(SimpleNamespace(), SimpleNamespace(), SimpleNamespace()) + + assert module.EventName.steerTempUnavailable in events.names diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index dcc1b4b2a..3b4d4be82 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -26,7 +26,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( ) from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.car.cruise_state import should_cancel_stock_cruise -from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS +from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS, get_car_lateral_smooth_seconds from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles @@ -35,6 +35,7 @@ from openpilot.starpilot.controls.lib.neural_network_feedforward import LatContr State = log.SelfdriveState.OpenpilotState LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection +LateralControlMode = car.CarControl.Actuators.LateralControlMode ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys()) @@ -216,6 +217,19 @@ def get_plan_reach(model_v2) -> float: return xs[-1] if len(xs) else 0.0 +def get_control_lateral_smooth_seconds(brand: str, v_ego: float, vehicle_smooth_seconds: float) -> float: + if brand != "rivian": + return LAT_SMOOTH_SECONDS + return get_car_lateral_smooth_seconds(brand, v_ego, vehicle_smooth_seconds) + + +def turn_lead_allowed(brand: str, lateral_control_mode: car.CarControl.Actuators.LateralControlMode) -> bool: + # Torque steering mechanically damps the turn-lead fade. A direct angle + # controller follows the resulting lead/catch-up cycle literally, which can + # reverse the wheel command several times during one turn initiation. + return brand != "rivian" or lateral_control_mode != LateralControlMode.angle + + # Turn-initiation lead. The model's action and the fixed 4/7 m probes are anchored in # METERS, so the seconds of warning they give shrinks with speed — at 12 mph a corner # enters the 7 m window only ~1.3 s out, too late to wind the wheel, which is why every @@ -577,7 +591,9 @@ class Controls: # bend is not a turn. The model-oppose veto is defense-in-depth for the fade-in # edge: a model actively steering against the blinker is correcting something the # lead must not fight (see the constants comment for the 2026-07-19 failures). - if (CC.latActive and blinker_dir != 0.0 and + lateral_control_mode = self.sm['carOutput'].actuatorsOutput.lateralControlMode + if (turn_lead_allowed(self.CP.brand, lateral_control_mode) and + CC.latActive and blinker_dir != 0.0 and model_v2.meta.laneChangeState == LaneChangeState.off and TURN_LEAD_MIN_SPEED <= CS.vEgo < TURN_LEAD_MAX_SPEED and new_desired_curvature * blinker_dir > -TURN_LEAD_MODEL_OPPOSE): @@ -650,7 +666,8 @@ class Controls: self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll, jerk_factor) - lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS + lat_smooth_seconds = get_control_lateral_smooth_seconds(self.CP.brand, CS.vEgo, self.CP.lateralSmoothSeconds) + lat_delay = self.sm["liveDelay"].lateralDelay + lat_smooth_seconds actuators.curvature = self.desired_curvature steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, diff --git a/selfdrive/controls/tests/test_turn_lead.py b/selfdrive/controls/tests/test_turn_lead.py new file mode 100644 index 000000000..7f3d975fd --- /dev/null +++ b/selfdrive/controls/tests/test_turn_lead.py @@ -0,0 +1,30 @@ +from cereal import car + +import pytest + +from openpilot.selfdrive.controls.controlsd import get_control_lateral_smooth_seconds, turn_lead_allowed + + +LateralControlMode = car.CarControl.Actuators.LateralControlMode + + +def test_turn_lead_is_suppressed_only_during_applied_angle_control(): + assert not turn_lead_allowed("rivian", LateralControlMode.angle) + assert turn_lead_allowed("rivian", LateralControlMode.torque) + assert turn_lead_allowed("rivian", LateralControlMode.torqueRecovering) + assert turn_lead_allowed("rivian", LateralControlMode.inactive) + assert turn_lead_allowed("ford", LateralControlMode.angle) + + +@pytest.mark.parametrize("v_ego", [0.0, 5.0, 30.0]) +def test_non_rivian_control_smoothing_matches_starpilot(v_ego): + assert get_control_lateral_smooth_seconds("toyota", v_ego, 0.0) == 0.1 + + +@pytest.mark.parametrize(("v_ego", "expected"), [ + (0.0, 0.4), + (5.0, 0.2), + (30.0, 0.0), +]) +def test_rivian_control_smoothing_remains_speed_scheduled(v_ego, expected): + assert get_control_lateral_smooth_seconds("rivian", v_ego, 0.4) == pytest.approx(expected) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 7c6985d9c..68ba3d6bc 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -67,6 +67,17 @@ MIN_LAT_CONTROL_SPEED = 0.3 BIG_MODEL_TIMEOUT = 60 BIG_MODEL_LOAD_WAIT_TIMEOUT_MS = 30000 BIG_MODEL_RUN_WAIT_TIMEOUT_MS = 3000 +LAT_SMOOTH_BP = [2.0, 8.0] + + +def get_lateral_smooth_seconds(v_ego: float, maximum: float = 0.0) -> float: + return float(np.interp(v_ego, LAT_SMOOTH_BP, [maximum, 0.0])) + + +def get_car_lateral_smooth_seconds(brand: str, v_ego: float, maximum: float) -> float: + if brand == "rivian": + return get_lateral_smooth_seconds(v_ego, maximum) + return maximum def _get_param_str(params: Params, key: str, default: str = "") -> str: @@ -686,13 +697,15 @@ def main(demo=False): meta_extra = meta_main sm.update(0) - lat_smooth_seconds = _model_smooth_seconds(params, "LatSmoothSeconds", LAT_SMOOTH_SECONDS) long_smooth_seconds = _model_smooth_seconds(params, "LongSmoothSeconds", LONG_SMOOTH_SECONDS) long_delay = CP.longitudinalActuatorDelay + long_smooth_seconds desire = DH.desire is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) + lat_smooth_default = CP.lateralSmoothSeconds if CP.brand == "rivian" else LAT_SMOOTH_SECONDS + lat_smooth_maximum = _model_smooth_seconds(params, "LatSmoothSeconds", lat_smooth_default) + lat_smooth_seconds = get_car_lateral_smooth_seconds(CP.brand, v_ego, lat_smooth_maximum) lat_delay = sm["liveDelay"].lateralDelay + lat_smooth_seconds lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32) if sm.frame % 60 == 0: diff --git a/selfdrive/modeld/tests/test_lateral_smoothing.py b/selfdrive/modeld/tests/test_lateral_smoothing.py new file mode 100644 index 000000000..c70c069f9 --- /dev/null +++ b/selfdrive/modeld/tests/test_lateral_smoothing.py @@ -0,0 +1,34 @@ +import pytest + +from openpilot.selfdrive.modeld.modeld import get_car_lateral_smooth_seconds, get_lateral_smooth_seconds + + +@pytest.mark.parametrize(("v_ego", "expected"), [ + (0.0, 0.4), + (2.0, 0.4), + (5.0, 0.2), + (8.0, 0.0), + (30.0, 0.0), +]) +def test_lateral_smoothing_tapers_with_speed(v_ego, expected): + assert get_lateral_smooth_seconds(v_ego, 0.4) == pytest.approx(expected) + + +@pytest.mark.parametrize("v_ego", [0.0, 5.0, 30.0]) +def test_default_lateral_smoothing_is_disabled(v_ego): + assert get_lateral_smooth_seconds(v_ego) == 0.0 + + +@pytest.mark.parametrize("v_ego", [0.0, 5.0, 30.0]) +def test_non_rivian_cars_keep_configured_starpilot_smoothing(v_ego): + assert get_car_lateral_smooth_seconds("toyota", v_ego, 0.4) == 0.4 + + +@pytest.mark.parametrize(("v_ego", "maximum", "expected"), [ + (0.0, 0.4, 0.4), + (5.0, 0.4, 0.2), + (30.0, 0.4, 0.0), + (0.0, 0.0, 0.0), +]) +def test_rivian_uses_configured_smoothing(v_ego, maximum, expected): + assert get_car_lateral_smooth_seconds("rivian", v_ego, maximum) == pytest.approx(expected) diff --git a/selfdrive/pandad/pandad.py b/selfdrive/pandad/pandad.py index f08079fe0..2b8d52a0f 100755 --- a/selfdrive/pandad/pandad.py +++ b/selfdrive/pandad/pandad.py @@ -11,6 +11,7 @@ from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params, UnknownKeyName from openpilot.system.hardware import HARDWARE from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.pandad.rivian_long_flasher import prepare_rivian_bridge def get_selected_firmware_name(app_fn: str, remote_start: bool, hkg_remote_start: bool, ignore_ignition_line: bool) -> str: @@ -159,6 +160,13 @@ def main() -> None: cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}") + # Update and reserve the Rivian harness bridge before managing internal Pandas. + bridge_serials = prepare_rivian_bridge(panda_serials) + panda_serials = [serial for serial in panda_serials if serial not in bridge_serials] + if len(panda_serials) == 0: + no_internal_panda_count += 1 + continue + # Flash pandas pandas: list[Panda] = [] remote_start = get_remote_start_boots_comma(params) diff --git a/selfdrive/pandad/rivian_long_flasher.py b/selfdrive/pandad/rivian_long_flasher.py new file mode 100644 index 000000000..9722ebb54 --- /dev/null +++ b/selfdrive/pandad/rivian_long_flasher.py @@ -0,0 +1,127 @@ +#!/usr/bin/env python3 +"""Firmware management for the Rivian Gen 1 harness bridge.""" + +import os +from itertools import accumulate + +from cereal import car +from panda import Panda +from openpilot.common.params import Params +from openpilot.common.swaglog import cloudlog + +FW_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), "rivian_long_fw.bin.signed") +SECTOR_SIZES = [0x4000] * 4 + [0x10000] + [0x20000] * 11 + + +def _is_rivian() -> bool: + params = Params() + + for key in ("CarParamsPersistent", "CarParamsCache", "CarParamsPrevRoute"): + cp_bytes = params.get(key) + if cp_bytes is None: + continue + try: + with car.CarParams.from_bytes(cp_bytes) as CP: + if CP.brand == "rivian": + return True + except Exception: + cloudlog.exception(f"Unable to read {key} while identifying Rivian bridge") + + return False + + +def is_rivian_vehicle() -> bool: + return _is_rivian() + + +def _flash_static(handle, code: bytes) -> None: + assert Panda.flasher_present(handle) + last_sector = next((i + 1 for i, value in enumerate(accumulate(SECTOR_SIZES[1:])) if value > len(code)), -1) + assert 1 <= last_sector < 7, "Invalid Rivian bridge firmware size" + + handle.controlWrite(Panda.REQUEST_IN, 0xB1, 0, 0, b'') + for sector in range(1, last_sector + 1): + handle.controlWrite(Panda.REQUEST_IN, 0xB2, sector, 0, b'') + for offset in range(0, len(code), 0x10): + handle.bulkWrite(2, code[offset:offset + 0x10]) + try: + handle.controlWrite(Panda.REQUEST_IN, 0xD8, 0, 0, b'', expect_disconnect=True) + except Exception: + pass + + +def _flash_panda(panda: Panda) -> None: + expected_signature = Panda.get_signature_from_firmware(FW_PATH) + if not panda.bootstub and panda.get_signature() == expected_signature: + cloudlog.info(f"Rivian bridge {panda.get_usb_serial()} already up to date") + return + + cloudlog.info(f"Flashing Rivian Extreme harness bridge {panda.get_usb_serial()}") + with open(FW_PATH, "rb") as firmware: + code = firmware.read() + + if not panda.bootstub: + # Old F4 firmware cannot use Panda.reset(); enter its bootstub directly. + try: + panda._handle.controlWrite(Panda.REQUEST_IN, 0xD1, 1, 0, b'', timeout=15000, expect_disconnect=True) + except Exception: + pass + panda.close() + panda.reconnect() + + _flash_static(panda._handle, code) + panda.reconnect() + cloudlog.info(f"Successfully flashed Rivian Extreme harness bridge {panda.get_usb_serial()}") + + +def is_rivian_bridge_panda(panda: Panda, rivian: bool | None = None) -> bool: + if panda.is_internal() or panda.get_type() != Panda.HW_TYPE_BLACK: + return False + if rivian is None: + rivian = _is_rivian() + if rivian or panda.bootstub: + return rivian + try: + expected_signature = Panda.get_signature_from_firmware(FW_PATH) + return panda.get_signature() == expected_signature + except Exception: + return False + + +def prepare_rivian_bridge(panda_serials: list[str]) -> set[str]: + """Identify bridge serials which must not be passed to normal Panda management.""" + firmware_available = os.path.isfile(FW_PATH) + if not firmware_available: + cloudlog.error(f"Rivian bridge firmware not found at {FW_PATH}") + + rivian = _is_rivian() + usb_serials = set(Panda.usb_list()) + bridge_serials: set[str] = set() + + for serial in panda_serials: + if serial not in usb_serials: + continue + panda = None + try: + panda = Panda(serial) + if panda.is_internal() or panda.get_type() != Panda.HW_TYPE_BLACK: + continue + + bridge_confirmed = is_rivian_bridge_panda(panda, rivian) + if not bridge_confirmed: + continue + + bridge_serials.add(serial) + if rivian and firmware_available: + _flash_panda(panda) + except Exception: + cloudlog.exception(f"Failed to prepare Rivian Extreme harness bridge {serial}") + finally: + if panda is not None: + panda.close() + + return bridge_serials + + +if __name__ == '__main__': + prepare_rivian_bridge(Panda.list()) diff --git a/selfdrive/pandad/rivian_long_fw.bin.signed b/selfdrive/pandad/rivian_long_fw.bin.signed new file mode 100644 index 000000000..bdbd237ba Binary files /dev/null and b/selfdrive/pandad/rivian_long_fw.bin.signed differ diff --git a/selfdrive/pandad/tests/test_rivian_long_flasher.py b/selfdrive/pandad/tests/test_rivian_long_flasher.py new file mode 100644 index 000000000..1f9f4884e --- /dev/null +++ b/selfdrive/pandad/tests/test_rivian_long_flasher.py @@ -0,0 +1,80 @@ +import importlib.util +from pathlib import Path +from unittest.mock import MagicMock, patch # noqa: TID251 - mocks are required to guarantee no hardware access + + +MODULE_PATH = Path(__file__).parents[1] / "rivian_long_flasher.py" +MODULE_SPEC = importlib.util.spec_from_file_location("rivian_long_flasher", MODULE_PATH) +assert MODULE_SPEC is not None and MODULE_SPEC.loader is not None +flasher = importlib.util.module_from_spec(MODULE_SPEC) +MODULE_SPEC.loader.exec_module(flasher) + + +def _external_black_panda(signature: bytes = b"current", bootstub: bool = False): + panda = MagicMock() + panda.is_internal.return_value = False + panda.get_type.return_value = b"\x03" + panda.get_signature.return_value = signature + panda.bootstub = bootstub + return panda + + +def test_current_rivian_bridge_uses_bridge_flash_path(): + panda = _external_black_panda(signature=b"expected") + with patch.object(flasher, "_is_rivian", return_value=True), \ + patch.object(flasher.os.path, "isfile", return_value=True), \ + patch.object(flasher, "Panda", wraps=flasher.Panda) as panda_class, \ + patch.object(flasher, "_flash_panda") as flash_panda: + panda_class.return_value = panda + panda_class.HW_TYPE_BLACK = b"\x03" + panda_class.usb_list.return_value = ["bridge"] + panda_class.get_signature_from_firmware.return_value = b"expected" + + assert flasher.prepare_rivian_bridge(["internal", "bridge"]) == {"bridge"} + flash_panda.assert_called_once_with(panda) + panda.close.assert_called_once() + + +def test_outdated_rivian_bridge_uses_bridge_flash_path(): + panda = _external_black_panda(signature=b"unexpected") + with patch.object(flasher, "_is_rivian", return_value=True), \ + patch.object(flasher.os.path, "isfile", return_value=True), \ + patch.object(flasher, "Panda", wraps=flasher.Panda) as panda_class, \ + patch.object(flasher, "_flash_panda") as flash_panda: + panda_class.return_value = panda + panda_class.HW_TYPE_BLACK = b"\x03" + panda_class.usb_list.return_value = ["bridge"] + panda_class.get_signature_from_firmware.return_value = b"expected" + + assert flasher.prepare_rivian_bridge(["internal", "bridge"]) == {"bridge"} + flash_panda.assert_called_once_with(panda) + + +def test_non_rivian_matching_bridge_is_reserved_without_flashing(): + panda = _external_black_panda(signature=b"expected") + with patch.object(flasher, "_is_rivian", return_value=False), \ + patch.object(flasher.os.path, "isfile", return_value=True), \ + patch.object(flasher, "Panda", wraps=flasher.Panda) as panda_class, \ + patch.object(flasher, "_flash_panda") as flash_panda: + panda_class.return_value = panda + panda_class.HW_TYPE_BLACK = b"\x03" + panda_class.usb_list.return_value = ["bridge"] + panda_class.get_signature_from_firmware.return_value = b"expected" + + assert flasher.prepare_rivian_bridge(["internal", "bridge"]) == {"bridge"} + flash_panda.assert_not_called() + + +def test_non_rivian_external_black_panda_is_not_misidentified(): + panda = _external_black_panda(signature=b"unexpected") + with patch.object(flasher, "_is_rivian", return_value=False), \ + patch.object(flasher.os.path, "isfile", return_value=True), \ + patch.object(flasher, "Panda", wraps=flasher.Panda) as panda_class, \ + patch.object(flasher, "_flash_panda") as flash_panda: + panda_class.return_value = panda + panda_class.HW_TYPE_BLACK = b"\x03" + panda_class.usb_list.return_value = ["external"] + panda_class.get_signature_from_firmware.return_value = b"expected" + + assert flasher.prepare_rivian_bridge(["internal", "external"]) == set() + flash_panda.assert_not_called() diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index 3c364d926..1794f2a5d 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/selfdrive/ui/mici/onroad/hud_renderer.py @@ -4,6 +4,7 @@ import pyray as rl from dataclasses import dataclass from openpilot.common.constants import CV from openpilot.selfdrive.ui.onroad.starpilot.torque_bar import TorqueBar +from openpilot.selfdrive.ui.onroad.starpilot.rivian_lateral_mode import rivian_lateral_mode from openpilot.selfdrive.ui.mici.onroad.speed_limit_utils import resolve_display_speed_limit_ms from openpilot.selfdrive.ui.onroad.starpilot.navigation_card import NavigationCardRenderer from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus @@ -162,6 +163,7 @@ class HudRenderer(Widget): self._wheel_alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps) self._wheel_y_filter = FirstOrderFilter(0, 0.1, 1 / gui_app.target_fps) + self._wheel_tint: rl.Color | None = None self._set_speed_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) self._egpu_alpha_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps) @@ -185,10 +187,13 @@ class HudRenderer(Widget): self.is_cruise_set = False self.set_speed = SET_SPEED_NA self.speed = 0.0 + self._wheel_tint = None return controls_state = sm['controlsState'] car_state = sm['carState'] + rivian_lateral_mode.update() + self._wheel_tint = rivian_lateral_mode.wheel_tint v_cruise_cluster = car_state.vCruiseCluster set_speed = ( @@ -380,7 +385,8 @@ class HudRenderer(Widget): origin = (wheel_txt.width / 2, wheel_txt.height / 2) # color and draw - color = rl.Color(255, 255, 255, int(self._wheel_alpha_filter.x)) + base_color = self._wheel_tint if self._wheel_tint is not None and not self._show_wheel_critical else rl.Color(255, 255, 255, 255) + color = rl.Color(base_color.r, base_color.g, base_color.b, int(self._wheel_alpha_filter.x)) rl.draw_texture_pro(wheel_txt, src_rect, dest_rect, origin, rotation, color) if self._show_wheel_critical: diff --git a/selfdrive/ui/onroad/exp_button.py b/selfdrive/ui/onroad/exp_button.py index 3783b5341..6e2278cda 100644 --- a/selfdrive/ui/onroad/exp_button.py +++ b/selfdrive/ui/onroad/exp_button.py @@ -25,6 +25,7 @@ class ExpButton(Widget): self._white_color: rl.Color = rl.Color(255, 255, 255, 255) self._black_bg: rl.Color = rl.Color(0, 0, 0, 166) + self.wheel_tint: rl.Color | None = None self._txt_wheel: rl.Texture = gui_app.texture('icons/chffr_wheel.png', icon_size, icon_size) self._txt_exp: rl.Texture = gui_app.texture('icons/experimental.png', icon_size, icon_size) self._rect = rl.Rectangle(0, 0, button_size, button_size) @@ -97,17 +98,31 @@ class ExpButton(Widget): self._white_color.a = 180 if self.is_pressed or not self._engageable else 255 - texture = self._txt_exp if self._held_or_actual_mode() else self._txt_wheel + exp_mode = self._held_or_actual_mode() + texture = self._txt_exp if exp_mode else self._txt_wheel + color = self._white_color + tint = None + if self.wheel_tint is not None: + tint = rl.Color(self.wheel_tint.r, self.wheel_tint.g, self.wheel_tint.b, self._white_color.a) + rl.draw_circle(center_x, center_y, self._rect.width / 2, self._bg_color) + if tint is not None: + if exp_mode: + # The experimental icon is already colored, so show the lateral mode + # around it instead of obscuring the icon with a texture tint. + radius = self._rect.width / 2 + rl.draw_ring(rl.Vector2(center_x, center_y), radius - 8, radius, 0, 360, 0, tint) + else: + color = tint rotating_wheel = ui_state.starpilot_toggles.get("rotating_wheel", False) or self._params.get_bool("RotatingWheel") if texture == self._txt_wheel and rotating_wheel: source_rect = rl.Rectangle(0, 0, texture.width, texture.height) dest_rect = rl.Rectangle(center_x, center_y, texture.width, texture.height) origin = rl.Vector2(texture.width / 2, texture.height / 2) - rl.draw_texture_pro(texture, source_rect, dest_rect, origin, -self._steer_angle_filter.x, self._white_color) + rl.draw_texture_pro(texture, source_rect, dest_rect, origin, -self._steer_angle_filter.x, color) else: - rl.draw_texture_ex(texture, rl.Vector2(center_x - texture.width / 2, center_y - texture.height / 2), 0.0, 1.0, self._white_color) + rl.draw_texture_ex(texture, rl.Vector2(center_x - texture.width / 2, center_y - texture.height / 2), 0.0, 1.0, color) def _held_or_actual_mode(self): now = time.monotonic() diff --git a/selfdrive/ui/onroad/starpilot/rivian_lateral_mode.py b/selfdrive/ui/onroad/starpilot/rivian_lateral_mode.py new file mode 100644 index 000000000..2e309b09e --- /dev/null +++ b/selfdrive/ui/onroad/starpilot/rivian_lateral_mode.py @@ -0,0 +1,55 @@ +import pyray as rl + +from cereal import car +from openpilot.selfdrive.ui.ui_state import ui_state + +ANGLE_COLOR = rl.Color(0x3A, 0xDB, 0x6D, 255) +TORQUE_COLOR = rl.Color(0x4D, 0x9D, 0xFF, 255) +DRIVER_OVERRIDE_COLOR = rl.Color(255, 255, 255, 255) +LateralControlMode = car.CarControl.Actuators.LateralControlMode + + +class RivianLateralMode: + """Display Rivian's active lateral channel and driver steering input.""" + + def __init__(self): + self.mode: str | None = None + self.driver_override = False + self._frame = -1 + + def update(self) -> None: + sm = ui_state.sm + if sm.frame == self._frame: + return + self._frame = sm.frame + + CP = ui_state.CP + rivian = CP is not None and CP.brand == "rivian" + car_state_received = sm.recv_frame["carState"] >= ui_state.started_frame + car_control_received = sm.recv_frame["carControl"] >= ui_state.started_frame + if not rivian or not car_state_received or not car_control_received or not sm["carControl"].latActive: + self.mode = None + self.driver_override = False + return + + self.driver_override = sm["carState"].steeringPressed + lateral_mode = sm["carOutput"].actuatorsOutput.lateralControlMode + if lateral_mode == LateralControlMode.angle: + self.mode = "angle" + elif lateral_mode in (LateralControlMode.torque, LateralControlMode.torqueRecovering): + self.mode = "torque" + else: + self.mode = None + + @property + def wheel_tint(self) -> "rl.Color | None": + if self.driver_override: + return DRIVER_OVERRIDE_COLOR + if self.mode == "angle": + return ANGLE_COLOR + if self.mode == "torque": + return TORQUE_COLOR + return None + + +rivian_lateral_mode = RivianLateralMode() diff --git a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py index eb81f245f..ef3325e24 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py @@ -5,6 +5,7 @@ from openpilot.selfdrive.ui.onroad.starpilot.starpilot_border import render_behi from openpilot.selfdrive.ui.onroad.starpilot.path import render_adjacent_lanes, render_path_edges from openpilot.selfdrive.ui.ui_state import ui_state from openpilot.selfdrive.ui.onroad.starpilot.torque_bar import TorqueBar +from openpilot.selfdrive.ui.onroad.starpilot.rivian_lateral_mode import rivian_lateral_mode from openpilot.selfdrive.ui.onroad.starpilot.widget_layout_manager import WidgetLayoutManager from openpilot.selfdrive.ui.onroad.starpilot.widgets import ( SetSpeedWidget, SpeedLimitWidget, PedalIconsWidget, @@ -76,6 +77,10 @@ class StarPilotOnroadView(AugmentedRoadView): self._child(self._driver_monitor_widget) self._child(self._stopped_timer_widget) + def _update_state(self) -> None: + rivian_lateral_mode.update() + self._hud_renderer._exp_button.wheel_tint = rivian_lateral_mode.wheel_tint + def _render(self, rect: rl.Rectangle): border_width = self._get_border_width() border_color = get_screen_edge_color(ui_state) diff --git a/selfdrive/ui/onroad/starpilot/torque_bar.py b/selfdrive/ui/onroad/starpilot/torque_bar.py index c940d9643..f146efc9d 100644 --- a/selfdrive/ui/onroad/starpilot/torque_bar.py +++ b/selfdrive/ui/onroad/starpilot/torque_bar.py @@ -7,6 +7,7 @@ import numpy as np import pyray as rl from opendbc.car import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.ui.lib.starpilot_visuals import blend_colors +from openpilot.selfdrive.ui.onroad.starpilot.rivian_lateral_mode import rivian_lateral_mode from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient @@ -162,8 +163,13 @@ class TorqueBar(Widget): if self._demo: return - # torque line - if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': + rivian_lateral_mode.update() + # Angle-controlled cars, including Rivian's hybrid controller while its + # angle channel is active, use a lateral-acceleration estimate for the bar. + # The shared Rivian mode detector keys off the actual CAN torque so the bar + # follows live angle/torque handoffs rather than the controller request. + if (ui_state.sm['controlsState'].lateralControlState.which() == 'angleState' or + rivian_lateral_mode.mode == "angle"): controls_state = ui_state.sm['controlsState'] car_state = ui_state.sm['carState'] live_parameters = ui_state.sm['liveParameters'] diff --git a/selfdrive/ui/tests/test_rivian_lateral_mode.py b/selfdrive/ui/tests/test_rivian_lateral_mode.py new file mode 100644 index 000000000..403fc8369 --- /dev/null +++ b/selfdrive/ui/tests/test_rivian_lateral_mode.py @@ -0,0 +1,295 @@ +import importlib.util +from enum import IntFlag +from pathlib import Path +import sys +from types import ModuleType, SimpleNamespace + +import pytest + +from cereal import car + + +MODULE_PATH = Path(__file__).resolve().parents[1] / "onroad" / "starpilot" / "rivian_lateral_mode.py" +EXP_BUTTON_PATH = Path(__file__).resolve().parents[1] / "onroad" / "exp_button.py" + + +LateralControlMode = car.CarControl.Actuators.LateralControlMode + + +class FakeColor: + def __init__(self, r, g, b, a): + self.r = r + self.g = g + self.b = b + self.a = a + + +class FakeRectangle: + def __init__(self, x, y, width, height): + self.x = x + self.y = y + self.width = width + self.height = height + + +class FakeTexture: + def __init__(self, name, width, height): + self.name = name + self.width = width + self.height = height + + +class FakeSubMaster(dict): + def __init__(self, *, lateral_mode, steering_pressed=False, lat_active=True): + super().__init__({ + "carControl": SimpleNamespace(latActive=lat_active), + "carState": SimpleNamespace(steeringPressed=steering_pressed), + "carOutput": SimpleNamespace( + actuatorsOutput=SimpleNamespace(lateralControlMode=lateral_mode), + ), + }) + self.frame = 1 + self.recv_frame = {"carControl": 1, "carState": 1} + + +def load_lateral_mode(monkeypatch, *, brand="rivian", angle_harness=True, longitudinal_harness=False, + steering_pressed=False, lat_active=True, lateral_mode=LateralControlMode.inactive): + class RivianFlags(IntFlag): + ANGLE_HARNESS = 1 + LONGITUDINAL_HARNESS = 2 + + fake_pyray = ModuleType("pyray") + fake_pyray.Color = lambda *args: args + monkeypatch.setitem(sys.modules, "pyray", fake_pyray) + + values_module = ModuleType("opendbc.car.rivian.values") + values_module.RivianFlags = RivianFlags + monkeypatch.setitem(sys.modules, "opendbc.car.rivian.values", values_module) + + flags = RivianFlags(0) + if angle_harness: + flags |= RivianFlags.ANGLE_HARNESS + if longitudinal_harness: + flags |= RivianFlags.LONGITUDINAL_HARNESS + + ui_state = SimpleNamespace( + CP=SimpleNamespace(brand=brand, flags=flags), + sm=FakeSubMaster(lateral_mode=lateral_mode, steering_pressed=steering_pressed, lat_active=lat_active), + started_frame=0, + ) + ui_state_module = ModuleType("openpilot.selfdrive.ui.ui_state") + ui_state_module.ui_state = ui_state + monkeypatch.setitem(sys.modules, "openpilot.selfdrive.ui.ui_state", ui_state_module) + + spec = importlib.util.spec_from_file_location("rivian_lateral_mode_under_test", MODULE_PATH) + assert spec is not None and spec.loader is not None + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +def load_exp_button(monkeypatch): + draws = {"textures": [], "rings": []} + fake_pyray = ModuleType("pyray") + fake_pyray.Color = FakeColor + fake_pyray.Rectangle = FakeRectangle + fake_pyray.Texture = FakeTexture + fake_pyray.Vector2 = lambda x, y: SimpleNamespace(x=x, y=y) + fake_pyray.draw_circle = lambda *args: None + fake_pyray.draw_ring = lambda *args: draws["rings"].append(args) + fake_pyray.draw_texture_ex = lambda *args: draws["textures"].append(args) + fake_pyray.draw_texture_pro = lambda *args: draws["textures"].append(args) + monkeypatch.setitem(sys.modules, "pyray", fake_pyray) + + params_module = ModuleType("openpilot.common.params") + params_module.Params = type("Params", (), {"get_bool": lambda self, *args, **kwargs: False}) + monkeypatch.setitem(sys.modules, "openpilot.common.params", params_module) + + fake_ui_state = SimpleNamespace( + ui_params=SimpleNamespace(get_bool=lambda *args, **kwargs: False), + sm={ + "selfdriveState": SimpleNamespace(experimentalMode=False, engageable=True, enabled=False), + "carState": SimpleNamespace(steeringAngleDeg=0.0), + }, + starpilot_toggles={}, + always_on_lateral_active=False, + conditional_status=0, + switchback_mode_enabled=False, + traffic_mode_enabled=False, + params_memory=SimpleNamespace(), + has_longitudinal_control=False, + ) + ui_state_module = ModuleType("openpilot.selfdrive.ui.ui_state") + ui_state_module.ui_state = fake_ui_state + monkeypatch.setitem(sys.modules, "openpilot.selfdrive.ui.ui_state", ui_state_module) + + class FakeGuiApp: + target_fps = 60 + + @staticmethod + def texture(name, width, height): + return FakeTexture(name, width, height) + + application_module = ModuleType("openpilot.system.ui.lib.application") + application_module.gui_app = FakeGuiApp() + monkeypatch.setitem(sys.modules, "openpilot.system.ui.lib.application", application_module) + + class FakeWidget: + def __init__(self): + self.is_pressed = False + + def set_visible(self, visible): + self._visible = visible + + def _handle_mouse_release(self, _): + pass + + widgets_module = ModuleType("openpilot.system.ui.widgets") + widgets_module.Widget = FakeWidget + monkeypatch.setitem(sys.modules, "openpilot.system.ui.widgets", widgets_module) + + class FakeFilter: + def __init__(self, x, *args): + self.x = x + + def update(self, x): + self.x = x + return x + + filter_module = ModuleType("openpilot.common.filter_simple") + filter_module.FirstOrderFilter = FakeFilter + monkeypatch.setitem(sys.modules, "openpilot.common.filter_simple", filter_module) + + experimental_module = ModuleType("openpilot.starpilot.common.experimental_state") + experimental_module.CEStatus = {"OFF": 0} + experimental_module.next_manual_ce_status = lambda *args: 0 + experimental_module.sync_manual_ce_state = lambda *args: None + monkeypatch.setitem(sys.modules, "openpilot.starpilot.common.experimental_state", experimental_module) + + spec = importlib.util.spec_from_file_location("exp_button_under_test", EXP_BUTTON_PATH) + assert spec is not None and spec.loader is not None + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module, draws + + +def test_angle_mode_uses_controller_report(monkeypatch): + module = load_lateral_mode(monkeypatch, lateral_mode=LateralControlMode.angle) + mode = module.RivianLateralMode() + + mode.update() + + assert mode.mode == "angle" + assert mode.wheel_tint == module.ANGLE_COLOR + + +def test_zero_torque_at_standstill_stays_in_reported_torque_mode(monkeypatch): + module = load_lateral_mode(monkeypatch, lateral_mode=LateralControlMode.torque) + mode = module.RivianLateralMode() + + mode.update() + + assert mode.mode == "torque" + assert mode.wheel_tint == module.TORQUE_COLOR + + +def test_torque_recovery_stays_blue(monkeypatch): + module = load_lateral_mode(monkeypatch, lateral_mode=LateralControlMode.torqueRecovering) + mode = module.RivianLateralMode() + + mode.update() + + assert mode.mode == "torque" + assert mode.wheel_tint == module.TORQUE_COLOR + + +def test_basic_harness_rivian_uses_torque_color(monkeypatch): + module = load_lateral_mode(monkeypatch, angle_harness=False, lateral_mode=LateralControlMode.torque) + mode = module.RivianLateralMode() + + mode.update() + + assert mode.mode == "torque" + assert mode.wheel_tint == module.TORQUE_COLOR + + +def test_longitudinal_harness_rivian_uses_torque_color(monkeypatch): + module = load_lateral_mode(monkeypatch, angle_harness=False, longitudinal_harness=True, + lateral_mode=LateralControlMode.torque) + mode = module.RivianLateralMode() + + mode.update() + + assert mode.mode == "torque" + assert mode.wheel_tint == module.TORQUE_COLOR + + +@pytest.mark.parametrize(("angle_harness", "longitudinal_harness", "lateral_mode"), [ + (False, False, LateralControlMode.torque), + (False, True, LateralControlMode.torque), + (True, True, LateralControlMode.angle), + (True, True, LateralControlMode.torque), + (True, True, LateralControlMode.torqueRecovering), +]) +def test_driver_steering_is_white_in_every_configuration(monkeypatch, angle_harness, longitudinal_harness, lateral_mode): + module = load_lateral_mode(monkeypatch, angle_harness=angle_harness, longitudinal_harness=longitudinal_harness, + steering_pressed=True, lateral_mode=lateral_mode) + mode = module.RivianLateralMode() + + mode.update() + + expected_mode = "angle" if lateral_mode == LateralControlMode.angle else "torque" + assert mode.mode == expected_mode + assert mode.driver_override + assert mode.wheel_tint == module.DRIVER_OVERRIDE_COLOR + + +def test_releasing_wheel_restores_active_mode_color(monkeypatch): + module = load_lateral_mode(monkeypatch, steering_pressed=True, lateral_mode=LateralControlMode.angle) + mode = module.RivianLateralMode() + mode.update() + assert mode.wheel_tint == module.DRIVER_OVERRIDE_COLOR + + module.ui_state.sm["carState"].steeringPressed = False + module.ui_state.sm.frame += 1 + mode.update() + + assert not mode.driver_override + assert mode.wheel_tint == module.ANGLE_COLOR + + +def test_non_rivian_is_not_classified(monkeypatch): + module = load_lateral_mode(monkeypatch, brand="toyota", steering_pressed=True, + lateral_mode=LateralControlMode.torque) + mode = module.RivianLateralMode() + + mode.update() + + assert mode.mode is None + assert not mode.driver_override + assert mode.wheel_tint is None + + +def test_inactive_lateral_is_not_classified(monkeypatch): + module = load_lateral_mode(monkeypatch, steering_pressed=True, lat_active=False, + lateral_mode=LateralControlMode.torque) + mode = module.RivianLateralMode() + + mode.update() + + assert mode.mode is None + assert not mode.driver_override + assert mode.wheel_tint is None + + +def test_non_mici_wheel_icon_uses_rivian_tint(monkeypatch): + module, draws = load_exp_button(monkeypatch) + button = module.ExpButton(192, 144) + button.wheel_tint = FakeColor(0x4D, 0x9D, 0xFF, 255) + button._update_state() + + button._render(FakeRectangle(0, 0, 192, 192)) + + assert len(draws["textures"]) == 1 + texture_color = draws["textures"][0][-1] + assert (texture_color.r, texture_color.g, texture_color.b, texture_color.a) == (0x4D, 0x9D, 0xFF, 255) diff --git a/starpilot/common/starpilot_utilities.py b/starpilot/common/starpilot_utilities.py index a80f25ac4..f8b1e2bf6 100644 --- a/starpilot/common/starpilot_utilities.py +++ b/starpilot/common/starpilot_utilities.py @@ -189,6 +189,8 @@ def get_selected_panda_firmware_name(app_fn, remote_start, hkg_remote_start, ign def flash_panda(params_memory): + from openpilot.selfdrive.pandad.rivian_long_flasher import is_rivian_bridge_panda, is_rivian_vehicle + params = Params() try: remote_start = params.get_bool("RemoteStartBootsComma") @@ -203,9 +205,14 @@ def flash_panda(params_memory): except Exception: ignore_ignition_line = False + rivian = is_rivian_vehicle() + usb_serials = set(Panda.usb_list()) for serial in Panda.list(): try: with Panda(serial=serial) as panda: + if serial in usb_serials and is_rivian_bridge_panda(panda, rivian): + print(f"Skipping Rivian harness bridge {serial}") + continue print(f"Flashing Panda {serial}") flash_fn = None app_fn = panda.get_mcu_type().config.app_fn diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 9d51d6947..bf251e142 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -330,6 +330,9 @@ def get_starpilot_toggles(sm=messaging.SubMaster(["starpilotPlan"]), *, read_per # Controller selection happens before the first live StarPilot broadcast. Do # not let a cached CarParams/controller type hide the persisted user request. toggles.force_torque_controller = get_starpilot_toggles._params.get_bool("ForceTorqueController") + # Controller selection happens before the first live StarPilot broadcast. + # Realtime callers use the serialized value to avoid blocking reads. + toggles.rivian_angle_control = get_starpilot_toggles._params.get_bool("RivianAngleControl") return toggles @cache @@ -563,6 +566,7 @@ class StarPilotVariables: toggle = self.starpilot_toggles # CarParams uses this value to select the matching Panda safety configuration. toggle.tesla_cooperative_steering = self.params.get_bool("TeslaCoopSteering") + toggle.rivian_angle_control = self.params.get_bool("RivianAngleControl") fallback_platform = GM_CAR.CHEVROLET_BOLT_ACC_2022_2023 if HARDWARE.get_device_type() == "pc" else MOCK.MOCK @@ -1422,6 +1426,7 @@ class StarPilotVariables: "TeslaCoopSteering", condition=toggle.car_make == "tesla" and toggle.car_model == TESLA_CAR.TESLA_MODEL_3, ) + toggle.rivian_angle_control = self.get_value("RivianAngleControl", condition=toggle.car_make == "rivian") toggle.tethering_config = self.get_value("TetheringEnabled", cast=float) diff --git a/starpilot/common/tests/test_starpilot_variables.py b/starpilot/common/tests/test_starpilot_variables.py index 069ff5ac2..b81421c80 100644 --- a/starpilot/common/tests/test_starpilot_variables.py +++ b/starpilot/common/tests/test_starpilot_variables.py @@ -64,6 +64,19 @@ def test_get_starpilot_toggles_realtime_path_does_not_read_persisted_force_param assert toggles.force_torque_controller is False +def test_get_starpilot_toggles_uses_live_rivian_angle_request(monkeypatch): + params = SimpleNamespace(get_bool=lambda key: key == "RivianAngleControl") + monkeypatch.setattr(spv.get_starpilot_toggles, "_params", params, raising=False) + + payload = '{"rivian_angle_control": false}' + toggles = spv.get_starpilot_toggles( + {"starpilotPlan": SimpleNamespace(starpilotToggles=payload)}, + read_persisted_force_params=True, + ) + + assert toggles.rivian_angle_control is True + + class _FakeParams: def __init__(self, floats=None, ints=None, bools=None): self.floats = dict(floats or {}) diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings.js b/starpilot/system/the_galaxy/assets/components/tools/device_settings.js index c7447d9e4..5fa241d50 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings.js +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings.js @@ -15,6 +15,7 @@ const HIDDEN_SETTING_KEYS = new Set(["HumanAcceleration", "ReverseCruise"]) const GM_MAKES = ["Buick", "Cadillac", "Chevrolet", "GMC", "Holden"] const HKG_MAKES = ["Genesis", "Hyundai", "Kia"] const VEHICLE_SETTING_MAKES = { + RivianAngleControl: ["Rivian"], TeslaCoopSteering: ["Tesla"], NAPRadarEnabled: ["Tesla"], NAPRadarBehindNosecone: ["Tesla"], @@ -105,6 +106,7 @@ function isVehicleSettingVisible(section, param) { function isSettingVisible(section, param) { // This policy controls Galaxy rendering only; hidden params retain their stored values. if (HIDDEN_SETTING_KEYS.has(param.key) || !isVehicleSettingVisible(section, param)) return false + if (param.requires_capability && !state.values[param.requires_capability]) return false if (RADAR_REQUIRED_KEYS.has(param.key) && !state.values.HasRadar) return false if (param.key === "AlphaLongitudinalEnabled" && !state.values.AlphaLongitudinalAvailable) return false if (state.values[GALAXY_DEVELOPER_MODE_KEY]) return true diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json index 60385ae77..f503db503 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json @@ -2703,6 +2703,16 @@ "options_endpoint": "/api/fingerprints/models?make={CarMake}", "settings_tier": "simple" }, + { + "key": "RivianAngleControl", + "label": "Rivian Angle Steering", + "description": "Use the Extreme harness angle channel with cooperative torque fallback.", + "data_type": "bool", + "ui_type": "toggle", + "favorite_eligible": true, + "requires_capability": "HasRivianAngleHarness", + "settings_tier": "simple" + }, { "key": "ForceFingerprint", "label": "Disable Automatic Fingerprint Detection", diff --git a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py index 9ec8c06f0..841844076 100644 --- a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py +++ b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py @@ -172,6 +172,17 @@ def test_human_acceleration_param_is_removed(): assert '{"HumanAcceleration",' not in params_source +def test_rivian_angle_control_is_live_favorite_and_harness_gated(): + sections = _params_by_section(_layout()) + setting = sections["Vehicle"]["RivianAngleControl"] + + assert setting["ui_type"] == "toggle" + assert setting["favorite_eligible"] is True + assert setting["requires_capability"] == "HasRivianAngleHarness" + assert "reboot" not in setting["description"].lower() + assert _declared_default("RivianAngleControl") == "0" + + def test_vasm_is_default_off_and_configured_only_in_galaxy(): sections = _params_by_section(_layout()) lateral = sections["Lateral (Steering)"] diff --git a/starpilot/system/the_galaxy/tests/test_navigation_params.py b/starpilot/system/the_galaxy/tests/test_navigation_params.py index 1919a603f..e32ea6963 100644 --- a/starpilot/system/the_galaxy/tests/test_navigation_params.py +++ b/starpilot/system/the_galaxy/tests/test_navigation_params.py @@ -249,6 +249,23 @@ def test_favorite_slot_options_include_virtual_cruise_actions(monkeypatch): assert "__starpilot_favorite_action__:distance_increase" in option_keys +def test_rivian_angle_favorite_requires_detected_extreme_harness(monkeypatch): + options = [ + {"key": "RivianAngleControl", "requiresCapability": "HasRivianAngleHarness"}, + {"key": "NonGatedFavorite", "requiresCapability": ""}, + ] + monkeypatch.setattr(the_galaxy, "_get_favorite_slot_options", lambda: options) + + monkeypatch.setattr(the_galaxy, "_get_has_rivian_angle_harness", lambda: False) + assert [option["key"] for option in the_galaxy._get_available_favorite_slot_options()] == ["NonGatedFavorite"] + + monkeypatch.setattr(the_galaxy, "_get_has_rivian_angle_harness", lambda: True) + assert [option["key"] for option in the_galaxy._get_available_favorite_slot_options()] == [ + "RivianAngleControl", + "NonGatedFavorite", + ] + + def test_favorite_action_endpoint_increments_virtual_button_counter(monkeypatch): client, _ = _params_client(monkeypatch, {}, "tici") fake_memory = WritableFakeParams() diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index 29f0ceb4a..de0400bf4 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -99,6 +99,8 @@ from openpilot.starpilot.system.the_galaxy import flm_workspace, utilities from openpilot.starpilot.system.the_galaxy.update_recovery import inspect_interrupted_update, public_recovery_status, recover_interrupted_update DISCORD_WEBHOOK_URL = os.getenv("DISCORD_WEBHOOK_URL") +# Keep Galaxy independent of opendbc's generated car bindings while matching RivianFlags.ANGLE_HARNESS. +RIVIAN_ANGLE_HARNESS_FLAG = 1 << 0 GITLAB_API = "https://gitlab.com/api/v4" GITLAB_SUBMISSIONS_PROJECT_ID = "71992109" @@ -2721,6 +2723,7 @@ def _get_favorite_slot_options(): "label": str(param_data.get("label") or key), "description": str(param_data.get("description") or ""), "section": section_name, + "requiresCapability": str(param_data.get("requires_capability") or ""), }) except Exception: options = [] @@ -2729,6 +2732,15 @@ def _get_favorite_slot_options(): _favorite_slot_options = options return _favorite_slot_options +def _get_available_favorite_slot_options(): + capabilities = { + "HasRivianAngleHarness": _get_has_rivian_angle_harness(), + } + return [ + option for option in _get_favorite_slot_options() + if not option.get("requiresCapability") or capabilities.get(option["requiresCapability"], False) + ] + def _favorite_slot_values(options): return { option["key"]: _safe_params_get_bool(option["key"]) @@ -3517,6 +3529,17 @@ def _get_alpha_longitudinal_available(): except Exception: return False +def _get_has_rivian_angle_harness(): + cp_bytes = _safe_params_get_live_raw("CarParamsPersistent") + if not cp_bytes: + return False + + try: + with car.CarParams.from_bytes(cp_bytes) as cp: + return cp.brand == "rivian" and bool(int(getattr(cp, "flags", 0)) & RIVIAN_ANGLE_HARNESS_FLAG) + except Exception: + return False + def _get_hardware_snapshot_items(): starpilot_toggles = _get_starpilot_toggles_snapshot() @@ -4655,7 +4678,7 @@ def setup(app): @app.route("/api/favorites/slots", methods=["GET", "PUT"]) def favorite_slots(): - options = _get_favorite_slot_options() + options = _get_available_favorite_slot_options() option_by_key = {option["key"]: option for option in options} eligible_keys = set(option_by_key) @@ -4705,7 +4728,7 @@ def setup(app): @app.route("/api/favorites/values", methods=["GET"]) def favorite_values(): - options = _get_favorite_slot_options() + options = _get_available_favorite_slot_options() eligible_keys = {option["key"] for option in options} slots = normalize_favorite_slots(params.get(FAVORITE_SLOTS_PARAM), params=params, eligible_keys=eligible_keys) return jsonify({"values": _configured_favorite_slot_values(slots)}), 200 @@ -4736,7 +4759,7 @@ def setup(app): if not isinstance(raw_slots, list): return jsonify({"error": "Favorite slots must be configured with the Favorites editor."}), 400 - options = _get_favorite_slot_options() + options = _get_available_favorite_slot_options() option_by_key = {option["key"]: option for option in options} eligible_keys = set(option_by_key) slots = normalize_favorite_slots(raw_slots, params=params, eligible_keys=eligible_keys) @@ -5099,6 +5122,8 @@ def setup(app): update_starpilot_toggles() response = {"message": f"Parameter '{key}' updated successfully."} + if key == "RivianAngleControl": + response["message"] = "Rivian steering mode updated. The safe channel handoff is in progress." updated = {} if key in PANDA_FIRMWARE_TOGGLE_KEYS: threading.Thread(target=_flash_panda_then_reboot, daemon=True).start() @@ -5178,6 +5203,7 @@ def setup(app): result["HasRadar"] = _get_has_radar() result["VehicleParked"] = _get_vehicle_parked() result["AlphaLongitudinalAvailable"] = _get_alpha_longitudinal_available() + result["HasRivianAngleHarness"] = _get_has_rivian_angle_harness() return jsonify(_sanitize_json_value(result)), 200