From c0e9507313ed1e1b35a5ff83f0c22ee135e535f9 Mon Sep 17 00:00:00 2001 From: TonyJOM Date: Fri, 14 Aug 2026 22:15:52 -0400 Subject: [PATCH] Rivian Angle Support --- common/params_keys.h | 3 + opendbc_repo/opendbc/car/car.capnp | 9 + opendbc_repo/opendbc/car/docs_definitions.py | 1 + .../opendbc/car/rivian/carcontroller.py | 135 ++- opendbc_repo/opendbc/car/rivian/carstate.py | 78 +- .../opendbc/car/rivian/carstate_ext.py | 104 ++ .../opendbc/car/rivian/ext_controller.py | 432 +++++++ opendbc_repo/opendbc/car/rivian/faults.py | 12 + opendbc_repo/opendbc/car/rivian/interface.py | 47 +- .../opendbc/car/rivian/radar_interface.py | 22 +- opendbc_repo/opendbc/car/rivian/riviancan.py | 29 + .../opendbc/car/rivian/tests/test_rivian.py | 1050 ++++++++++++++++- .../opendbc/car/rivian/toi_controller.py | 179 +++ opendbc_repo/opendbc/car/rivian/values.py | 45 +- .../rivian/rivian_mando_front_radar.dbc | 351 ++++-- .../rivian/rivian_mando_front_radar.py | 14 +- .../rivian_mando_front_radar_generated.dbc | 351 ++++-- .../opendbc/dbc/rivian_park_assist_can.dbc | 20 +- .../opendbc/dbc/rivian_primary_actuator.dbc | 4 +- opendbc_repo/opendbc/safety/modes/rivian.h | 67 +- opendbc_repo/opendbc/safety/tests/common.py | 9 + .../safety/tests/safety_replay/helpers.py | 8 +- .../opendbc/safety/tests/test_rivian.py | 139 ++- selfdrive/car/car_specific.py | 28 + selfdrive/car/card.py | 9 +- .../car/tests/test_rivian_angle_saturation.py | 68 ++ selfdrive/controls/controlsd.py | 23 +- selfdrive/controls/tests/test_turn_lead.py | 30 + selfdrive/modeld/modeld.py | 15 +- .../modeld/tests/test_lateral_smoothing.py | 34 + selfdrive/pandad/pandad.py | 8 + selfdrive/pandad/rivian_long_flasher.py | 127 ++ selfdrive/pandad/rivian_long_fw.bin.signed | Bin 0 -> 59500 bytes .../pandad/tests/test_rivian_long_flasher.py | 80 ++ selfdrive/ui/mici/onroad/hud_renderer.py | 8 +- selfdrive/ui/onroad/exp_button.py | 21 +- .../onroad/starpilot/rivian_lateral_mode.py | 55 + .../onroad/starpilot/starpilot_onroad_view.py | 5 + selfdrive/ui/onroad/starpilot/torque_bar.py | 10 +- .../ui/tests/test_rivian_lateral_mode.py | 295 +++++ starpilot/common/starpilot_utilities.py | 7 + starpilot/common/starpilot_variables.py | 5 + .../common/tests/test_starpilot_variables.py | 13 + .../components/tools/device_settings.js | 2 + .../tools/device_settings_layout.json | 10 + .../tests/test_device_settings_layout.py | 11 + .../tests/test_navigation_params.py | 17 + starpilot/system/the_galaxy/the_galaxy.py | 32 +- 48 files changed, 3676 insertions(+), 346 deletions(-) create mode 100644 opendbc_repo/opendbc/car/rivian/carstate_ext.py create mode 100644 opendbc_repo/opendbc/car/rivian/ext_controller.py create mode 100644 opendbc_repo/opendbc/car/rivian/faults.py create mode 100644 opendbc_repo/opendbc/car/rivian/toi_controller.py create mode 100644 selfdrive/car/tests/test_rivian_angle_saturation.py create mode 100644 selfdrive/controls/tests/test_turn_lead.py create mode 100644 selfdrive/modeld/tests/test_lateral_smoothing.py create mode 100644 selfdrive/pandad/rivian_long_flasher.py create mode 100644 selfdrive/pandad/rivian_long_fw.bin.signed create mode 100644 selfdrive/pandad/tests/test_rivian_long_flasher.py create mode 100644 selfdrive/ui/onroad/starpilot/rivian_lateral_mode.py create mode 100644 selfdrive/ui/tests/test_rivian_lateral_mode.py diff --git a/common/params_keys.h b/common/params_keys.h index 68cf7ea8c..84b54c1dd 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -538,6 +538,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 2fa7fc388..ab4c9d7a9 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -220,7 +220,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: @@ -388,6 +391,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 11ef530e6..682ee1e7d 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -63,6 +63,17 @@ def _model_smooth_seconds(params, key, default): value = params.get_float(key, return_default=True, default=default) return round(min(max(value, SMOOTH_SECONDS_STEP), 2.0) / SMOOTH_SECONDS_STEP) * SMOOTH_SECONDS_STEP MIN_LAT_CONTROL_SPEED = 0.3 +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: @@ -601,13 +612,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 0000000000000000000000000000000000000000..bdbd237ba99589813bdda2199ad71f42bfc2937a GIT binary patch literal 59500 zcmafc33yXg`uDl_W^ZWI1t@KSECot~5-4h+sA*Cz>4IgO5rNUNsA2h!m1Up-r-8x% zq9agH77-LwTxM*GRjRZp6j#tmC@?4>Rn%x|JLyKQ-SYj;O-qZ;JRdyox##Ad^*!%) z-m~-i`=7TL8}jiUIoohXAhv-U5^Y-Un0yY5<=B{sp)QxB~bVF#0i~7z^+s z-T-I;{0ayHr1(7y5D6Fz7z!`|?gFF%(g9Ney{Mgr_$j_b2gO19+svJ)1{= zA+9bjaghd6Cn4?9{HgRm(S|^gHEYSn63#E-m}2I@JbAiHk{Py@NeOdG46_!nB}g@y zG1EFE*|AHiTg=4iX9xk{i;ED}%!I(t68ai7#iu z9~rfE@`6ZbJjKmSI?%9`VGuh3-S0XrwbBeT%JJNONf>bVfdz! zXMc(y*BZ#u*2e0kO^urN|Y;Oad|z(Vkjs5&aIVX)wFQYwew>$%mu_Bfp3>TC5rrVZf&&q z<>GvSP`QA&*y}iEb7Df}QPbhylGj}~RR^b}(l!PDTdcS>3+h#u_{XbjbqgJmQBB^@ zpCZX;Jd9qo>XbKZmCI{xeqLW}Iq5O1t!c4vMV9wHM+%-^GEd1FmF`!XIQ7*gLd^AQ zA~CWaqUY*}cJOeA#gYEpQ=>|UR*(u+DJ^U6Fedp$7eQ?Mg6F-opD%l~>t1mf6XYy>!22$5qDg zjF-}ZI-BU{uG+tF|C%BpQ5&}=ZuS0EU$4rRXS?KC@+^r#N-57E%Pz{38fx;0NPQx2 zf>#zZM4P}7trnE%FQ2=L==ZNV)l$vwUv&yN{u-fK!l0Fr{@4h{uaGl-jVx{r<7cG_ zs|UFES48JXbcaeWmPVl-(6&LO<8*G7zjA3sIKFKz4bM^;n!E#Yq&lCM+h`Y8|uKjv`x#=7IhfWPF4_Yr;=#TNdS0mk`e8Tc)qA2S`FwsT%>Q$ ze71nI&y-qxwR)yL$`GAl8Cu;G)RMMBa;1!!*kRff`?7_(8p~@cqd?z=j4NerJ+#dA zxB6`HF)T;aL>h6b~%$kY5>=Gf+p8{ccs36nIeP$aKGYm07<4fDf_Hf`({QKPQUC z;Y9lr_$&wg-->t10HQq;L9~55Y;LXqJ%ZNOnU(odkGJi7g7XkZi1W?O58_)mDE*nj zFt@q+An0C>=il%vhi4?-8BcYA7gYU+s8A_HEM1niAnmBjmj zxfd5j`q>B$9EqITZ@ol(%6RjHYdyHroF$5+NZ>D;Xs09IA6Ek{=sECH-BJX}xYMY1 zRXb=Of18H!m>r?-&j0#d?I|iaRlqvt#FF}5n$s;stR|zn%;^t`rG|S>7ZeplXRr=X zAK)~AUB`(jnW&#dtp2t}8CvC3a!E0+kd!w!vlu0b2le~&NM2I2cc)Q;F(G~vF(uWD zWjrm$+;k0i{!E z_EFp%Z+ldvfnPuyI$%xaLV9o;bY*!Thaqpue#>x3I;km|e{cD4;#qmbyNkoE&g zrK2ORPHkseC4NT2j8lv@WVCvi@rq=hr^v75plOJ|k|Z{HiB4t;<53e|hI(Zl`RI7J zWUXxU0FQr!s;&&}m8@m(>rHPN#!o()@kERmV}@>$N}-#WafS@V=qqKUi6P&3=2^~r zV}C>|`MAs|L7GPOA(a4;0;DmdK+q806uhs)v?2EIQ`oCT7&AYW1ci!`&cwk!36tAi z)S2tEN(Z=4Hj#*TU5p{YFa^C?W`_K@FV{C)Qtzqr(r+hRLv>5cXUked`GK(k??n2U zi2U*a*)m@3CVF*Fk#MgzVX+qUeHS0`k-=^hdE;qFV%Q3LX^t!>$3@dn@QM6Vhp~z| zM-(!6PUqEI9k^rdy8=yZu?RWxI1nC@`L20J% ziyM{?C)+F(p=hB{X`S=UbbOB(kR9-ZUVcbsDShPIg{}j^oB8fEAnzv z%9YqA@^vif4i4|@nYo#POlDoIXruSqA7?5H-g0Uaf?Ovl11hDMo^eOg#M`f$-jX~ z;f%7T7@6C{nC6T-BjS=tQhLnIaVh&O3U_JS?EF|4ZAbppP)PgLr%%{6=ggWG6=(IG zHgI`0Epo%m0anZ@*(a^}`BOFJlAH^IZCw1ay;BtxyM-J^QyJoUF6+Fo)nskW=T75^ zk)dS7;g{a*MWF@rZ5U`2UGUKri13iuPQtvQ>x>7aVkfN94!@~fCeP?9v;&M178HK z34>fwu|&J0Kv9|6Pm43W20g6A_ZT-@AEmD?i-hD;@EyG;T z&Yw2~dT>0r(aNQOAN-D+M&h5c(JF&f=Roylv=7gElT`4 z&Q#%37BKxIv8fBkYO^G?i6x}rgZyq%+#(i*+d(QSvDQ3wWVy53Zpf< zkYbn8t=h$C`_n9)g%3oJ9~#FUXF=ViZO+1Y^m%I`O%)3Z4zjO8=hu)Dd{K+^&f+&l zqylFl<%6yNg%3h)Wl{3naa`%b_d@N-FW10|86eJ0F3l%~1pXxpcU2kBh_iKATVq+j zUn(^!T**8uT0~J%F&Rt>Y(!CBD$0K^T_n0-pf;$$TsS0$eus$|HpRY*nu~l4!N`fTr$w-dTIM&4bl?Vl``1P75zbSJnl7LwYH2W@ z{W_`%0^YtB_1dB#-@z?g)KQIjZGT&o>J+mmIjJ?;-+FDrJFHX4Rf_k%l0$QvkR)XY z)kbvwBlB>MC;Upn`8h5&9hT>9dBoPZoCQz#+@f!aehvNaZx?MWfu?|$tH{FCQqLB1 zfJwROWivRw7``X8L15y4^>WGna_MD`XC}sF6kAO2Nu~RUY>=OmwR+S%#}DK+b+ZM{ zIxAqdpk8kU%oY@QD)5xxDOs<%H{y<8F6;df5?uy2BqsiqAzZTba-yGOBQv5iUWs#i zpN=1B-ybizZ(yF^8;SWW>dyYeYbqxOI=3TQQ{_kV&&_`oaVDkBOX;)C%Rzq?33GLn z!g^_r&ZFk)R`k#um8bK^8GHotbpE&y9h&2)?Z4tZ&OSWny0E@_w~#jjv>+Q~e%Q?M zR)JZ@)mvpX^&fLZK8>}eMah|)gLe;S<_mIB&qTTZHbvvUvtlSDcZk0ImCzUUR(VbR zCmeGh&GBB@kMIaIxYhM?E=Q1)IremOq&eOEP~JX!q;+3juTMZx%4k)PS?|La^_evo zI1b${%^Jd+Z+7NOQi?2*c{}r9iLnAkgQiVIyTfy6i zf|ov78_~6TF+%W#Qvax;TxP>O3;gs!u<|E`8Q=BccthV^G)>?4H*bAcz=UA4w^ug{J$M@^;_XuflcF$D%rSmBcYyps(B?zyPR z__Ukt!RheZ>gmI&(4+rh8Su6bX>VfCdPPvrN7)9TpB!QlHV(1wZaroqwC}%mhuM@O z9?OCX#ANvGA3?b-Os_!f7;vXCb9jru+siKWLK4D1rJ;zN>4N$Zas0*6&n6t&{#z*P~#<1O7Y^7R-%9oXB`v86rERA5`t?u#sXkH5{`ZO4lF~{4Sk3?M2 zBhOpNqvc^Iv!i+B9Z5z81LZ*z4+&IBC6L#vDUZsE&WW}W!=eSOZq|YudC?1JE#y#c zVYil7>BG>Y)F6#fx=0Bs^tt~0m65#Eb|fztFp52AZ3?{-UlU!u{SMJqFyIaj@4?Za zU_uXmB7^ZgG@0KWix@3;S@&SXSi~~A2O>uCcz<`ajS*w>gW>p%V*0LbHGZS9nL%Yw zI%BuIC!I~CVJUuRkRrV*M2{jbs4*TG6H|8FGHQ9nEuv4h=PBuVDnidzCw(r5O*S)b z@!aK>R;Oid=s6QlGxJLK5j96I`uuzDDmkhD?(4_!k%|L#d226rRn`7R4&>P zdPjT2-_aiNiS~$3v`2hu@D%fppSu{moywXSn+84HTWLl-*YC7GqTjy|YXEt37f>77 zX`Qn)e`;!ZFi;;Hm_0D7Mp!$N)z=7{MmiC09Z3@J9ua9P5?1{&_hxW~x%vGWHNtwn zMsR|LlDfidQ*J)8f?`nLrL^5I;&N!0JUfX`SU_c4FtGdsD%<|_IOcg8VpanPK%TWx zXjREUNo-r$3bww8k)<4O8&YV?6TtN+L?30vBC7AzHd^m{sG+G-b*r}ePDM{`KXyv9 zh#`7Gtd`0wNQMuR%#2wLAOJgP`sp;kAl?doI9dI zoOjsc*2Y1e-<+ESYnagwd1{V*lhBn+Iqb{MPzp}}&{KQu!NA8o@3DVHZ-VsOQ9)3} z+vniDMtpx)5RxC-epF~zmDSIPU~5Yda_4tgcDtAn?1*?f5zB`R>M4ozl)SGKmYC2r z>12;>i!nfd_65TXbF860b#!t1NPaRfQ`E^ImWJ5ju8B6e$d&hW-GdmFiQ~H_*ra+Y z6YuDfBHcKoOX?bn->6J9blriN9I;_tMsOwN?Wej%gCnWdS=%|Xhi6xICgLki_vg;x zkRCLhv`dQ^l>ww{D8ACzBV9ufqj~3c4nzx7h&6RaBSz)YFKB6)E&?sc?g|F>1WVf5 zA9)7)3Npn;bDKMr$W3K>T4%2wyj!G_UOhPWWJnKEy(P*n$O$?xjt|WlF?Ljc>yKmn zlG##4#eJn@1f$y|bjIx#y5b^ByS;buzj~SE%l3Uw{SCh7NY5njkfaCuSiNGudMb8F zFwoI4up+zy`IgcdFZ~_;=%6D!iI2DcBId_8OVmZOU*REjTvWrq`MK_yWB-~U{dZx6g zDIeI=Ge*DfsmpeZT~AvXeW<0{TkoOoFNv*;xA%U3z9Sg;dp7|NZmOu(Q=q?(iDk=R z|JlYZPKw{2dy>uE+!6^L z*bW-Qh@WX6Axeen_C#9@-cPn0#OH~2z4&~$T_@`Jo9)^jsq{4oeo;R+Qv8MF~CQAlr0axKHVM($wbnFfz9m{H&VQ9U4XWH zkkr%eSUk0W{{2zoorn;k!34xJ0QplT*&HnF?qsYnfd+cty0V(0v7PGM~KP#cWun3hh+O+15z)NUP#JB_(vGN<(|FrewjbW5 z9fDRnsr2-=UGO=Sb+p}-b_~*=-ay~?TZXzeZj&zARfhFS*Bi5oHqg?zreMmIGFZA= zYGd|Uius}qzv4X^X4iQ-tav)D&Y}%98&nm+0Dq!)JQ$u@za_3v|1*Zg2+aLlg{D#k zpC;8B{|Rc>g1)JBTdw9!6&zC6!=efXehPNO;w|}>`i>*Bx0FbE@W-1Ryuq>dhwOuR z?qfyDhnDg12FI^W$v;XYlW!z2U(4YG9?>poq5k7o(Mueo27L$N6LGmv7NyyLyHd-; z!0GeH#ag6%n1^y$X^_q|{IFVKGX@#xgWx2Z!e~8%Ff?CrBmdXmQ5`Kos=c8(0s3$m zwtWM>(J`RfjTnKq84D|@t#Lt6EPH6_1woQ_Mo`3uWgLhU5a0I7ll>hFi-gEqOg{M=7D*DTGCKUWN21I(iZjB7B&~P zscCJ-=#2QdGl&oAH)Eo5j-btNU_66?*IX>ZBQ6f%VV4BqL6;QaewPg43obdr-7W>f zoh~K9zq?cjx4Xg+Zgo*Dx!FZWaic37VWBGm;d++_VSy_W;a^-)2v@qI5$3yM5dNQQ zAi|}tK?omr4Mw=w6^n3zi^`XWU2zEKxu}$x<4QpI2iH)9GhM?F=DLO>%yDTEX1a6; zd6yoc)n!0vb|oUb*EIs+B-coU_qawOoZw1AILLbh z7R+LTfvzqy;*&9t2?hdPsrF#tZp>t$>$(_&VI`zC>M?cGQYmHA^5QG@^hi6&-S9@N ze90nd(nzXA?>KQ$Uj(J;2=HY*U@U<0s3ALk30B#S_qz2tWaQDdvekRQO~s}9B}eT0 zE#c>fqR#ibOlZr^u6V>tyAGo*eppoC|E?qU8+pv##PHaiNAe_1v64E^ML})62+pmI z9qy7h?npk-__zblNdle&vemBZ4Fi^b(l}~q4R%8j!$(N@5jc;9XEkm?KGhkR17uiX z|DtVE0CNHU9PlFkS%=ZtHfJ1i`T9?spH+oNiyjPJIJCUR+#OifZEyb82745b>)a3wt4jD^y5}xw>M?u8!JQZZdq+K>Z zhTojDf7mR`Caf#ZN~f#KM=W$^JlH+drq*=_H1gu#ZVm{1Dm9SyE14n`?_z-CNmylkf{n1OMZEGiOdy3e0 zDp!9wDat6!NB@pC)3H1PFY1c-hGeo^cM5swixM_~Dy&;~L32W5xi>C}w#Ul`T(oYT zVn0w;KYv!9b?autsQwhSTZ+{=Wh=M!A4q@DQfrTKS^ZAV0I1DFN$Cj{JA1vuJ$4|i zZD`JBd!H`lw)WSh7QFS>r53cMPnTMp^quNb7A=A<9YJ*|=e$jqj<{WyLb5{tP&%P~ zzX$%E0Znp-YNQ`J6ct>Fsq&y8Hz)Br{8AEhjFEV_Y)MvJURR*DW9^uqJni1jApHJS z|M+GbQD!N#%Q~8xseQRv$3XYTGvlc(A?ie{kBu(MgLaO8pO9z%HkESpR(>$QL1)U6 z@WXhFWmj%%`-(;OujHdHPHi^LDD4oMn2G6r)ssnC>8<8*lBNWLvFsXKn(H^Ghu*&H zx)X1t=j}?@(5z~m7G>?|DeHLc)?@za9BizzHO^Z4!d+Q;~3Fg3kACrNWv_>=I8DD z+}5#AY7u)jN|DH6%`VXr4E!^g+nT>{0A@huC%Xgx>QKW++Z~AP3=^LN;8UXSN=V(1 zc8L*jGu%QHL;^Za#e56|B%K~O}~2Sb23&(=+kU!g6BO@{s5g}GxF_ZEb;Gl>bEY~DlI%+&qkdm zkfHVIOk~D31u-me8k{e0#qQ40tKDS8opogNpWVq7hILFzw=v1pZMahQ*gC_?J!+xY%Tnxam&&x!TVkgQOZ2JPcmJbRN?JG}G-_T>Ck1Bros}-+o1wF2FVvU*6awU*-=Gdtit(8YM z+*lV@%B0;`d8k3|AR|7;T3RD^sQ%*RFslja20|uSC;`*4VtH0vTbL-WHDK(@y92Lv z4ibIJ-GN^^qD7x_cfi{bWn(d0?+$#|LA5fA*zgV({wcMM(i^J}v&r#WZ3nBPbD?Lu z2`mFQq(gV$*$(PAB#2#biGCouc0&0<0pDh}=3VOPONdqR->#onN|H4v6*jeNvHlVG z?;5MPdMBq_KgX^)xe?q98{)&BKG3!&k;~tJBJ#`Qr;)^qC}qeHu6`4zM0x@Xp{e8c z-x&P1K>Q7*0HY2gbxR6M}(rL2iuO18+Xk zyw=_V*%h)ahP(y53ji8O6xwJ+|F!s)y^UKq6(x$<)HffKLF+#@g781kuS{`+i zG*FFUQ0Z=AM3|zK7$0^shRs68UAu*mX-8lwXWYdYH6BMr3}&#S(#i$xUGEA>MrtYD z;ig{BQEAdTVz}{R-GW%=HzBpF{d$qEk{{W98hQ1s2wJ-UJN;km_|;&43p;I( zTcqyzEbRJmkI3r`{2@35<3`tw&CQD}H7#LWv4zUWSl7tB{m^fbUEqRlgZ(&m*kC-m z|BW`zTe^2DqoaPg!`+Y^d12WP*})CJZk>8jzJGL1WhiQ-v%u ziC2N&^Ud9XeJ4~{Wg;#eXrZnkINbduQEV)MHI}=j;6x$KJJ-?psHd|TYN zwb1gWC$iunfzTF}`2cpKw|D#ZykW z7W11d$6ONBf6TSVA~}FvqR?^C*0>iv!j3S2@syA=kJfpno@EzWNYXJ^B6d$eDkU`d8dTk%cKX05qsfA$>D`Qi2Y2&nC2m4=eu+L zjB+yOXf?uNV~wyEkOSBO*uAVqcyXDH&#fDV7HkEt(3&~aykZLNfdy^0@Xzxe;Tfak z07*XPIvV@ieypDIE_SLg;joOut-f$>uQ!#i@M^Qj*s<>O!rtr&AD$OpOyl;o#hn)p zr){_B_JuJ2hGnaWcg}jb3~QDXPUGuov}X2{3$D}12Y8Dl9rjQvIOLX74U$WDP0U8i z%znC;@!$2zK;p>Q;pPY^ZfaUC-Q)3Z!R!p3V!15DYKtwe zfx;j2yntP_Y%;v+*lo{cyjXChAlD~Jl(g0@&hT-PqixQ`ac+{FgMB?q;=)Jf`rlL9 z^h6hPtSg`me(`hsV}EvhA>@UEWPk=v0>(3zBJ;=S-!Fqy7Wd*|LLAb&L7SdqnE@u-qU}Vi&bB2 z8RgkjP*X54Gm*{EOIq_6+xY}*SD>mp5;8kDPJQ-h8-auw`Mb6pG%8?4$FDb5(AI$F zwDinLG-hy&wjTAt_t$FDyq{ME!2Z{$fb@!!AUvx<*} zqhVOqH`2EczpL1C5?+A6NYqM`#FyBH$Lj9gN;Pfbisxhgmm;fq(oL{K;G5+{gQBW z`w37VO+zJ!xux8xsF%nSAdj9h9jli}i18u!C1FfjZ|$hrajjt}J+~!VUs=~K3AQvU zD|%D(v~N;x`+|)mE2VM7q>@G}+V@#!sC|q7OZ%*}efv8D?{}i@S(k(}#<>|?jo&+% z@jIsAOWd++LwrffRM$5CSiMsI0%XNt{#DCIO@#Z)l^5}rz*kyUUO9x=aI}9tq{Cm) zcAMDtcN>40ORdKfX!m7TN#mn)KWQ{LMl?$29%}f5Q(cHEQ+fyi1ljt(_RUG6LPA` z8_S>cQraVF;;AMNX|r3+gtk+ z_}2C>=>Mz1skgVU8GGP%mW=T5>$r8%IiEP*Q}Di`LH%13YDuAjw&T4v^2qBrXq)7E zte&V^j?ST~ zon-U`_yUfGT@cQs61IwGK7h;~0tG4>rV!GcbSStjoc1$P)7_0Zg6 zQ;FyYKIZ%>oM8SGj4j2vx+||(bP0_QzHI z!l*XkP1O`MQM$bPGiUgwfv8?=VB=nIbDHUT`95L1DaHCh6B7}#P|Q9~wii3sFAGE) zYD4Js9PimHDDwAVU-Did?C?IpL;pZC zuTSt1pU_m4(`bt6NHIGovtsuFEC!8e5157@??%gkapb-!+7~jj ziI0X9Vn$x^G9v}=9JFyd;25C4EIQs+Tv!ZAd5UluIeb0z79KuA+Te{^|dM5E+c(!T(aFzzLUJ&gkVr=!KI5?bX6D{`<^z<9uf9*z} zKLuz#p>gWp|MNy_%}o`@<>LZlW&&E%oe^WbqRjOzj-eQk`Q5T!99;hOwWjGrpG|2! zgo*npCM4hYvn-Fd%`Mb<-f~<|96S0sVQk`RE`1 zhUbeIH);K5@NS$qZW6OLG;WC(aZZSy+_~wI!!{(wW{(jksJRHb&Vro+i9W9l{W16S zDF)BmWx-EfsTBkMiww9u)y1x)Qq_SY7WPUQLisiI|2M};^KlBZPa(zLB2YS8ccSV3h}XXA<4JS#C5(;* z=i2oS+5;WveAciBe5b<~u2cK-)iaeiov60>POO&*yLsYn~GOT1Su(+mL5N!A1vqm5}2*rH*GA2iiR=%AXJcU50aFz4KJM zJ9Y#(Xr|KQt6|H~r(quXx*CTIlB!7F+anE58Hzpn0zCrIaicx@1A6o%|F+npUB8-r za_JN=Gs+8C58mAix>lSI@on|*X#6jo$^#SwWQ@9J)ELfU-Q0W@7T`3` zb>XCQk07r$Zo4d`)`5O==6!IWEF3(qZmk=WYfe9+o}tPcs5{WNu@I+WyZUpB%FtrQRx&3`JCj!%yMyO zU|TRk)FN~ht)=^QFOzr-GUVH+V#{TZ413`wW0q3uu7`xo=khS)t$m6lCYV1L?T+cd zu?-NNP2hJe5XGW}0pnmesjoukIPTwjNK3&IT7yowx^ z`2Dh2uNix&j@Cl&#S(4e1<+9((XPUKYy#2lMqi!72+kA8#I_X$<Y(LleXTL# ziwU5E-c$6%m5Tgy?yV4e5LKb6h+TDgb^g?^aWh4`rx<;f<1Pt$Ye+>C{2>o z+71TRG*)kIo8`15Q{ja)Kz;daW|b z4b}AAB$+`_GPIkvdfw)c5o;IA_I&#lpq3?H}g0Ru|;@IfZ;w$g|WL zxZcI;$LqfMd3e-A2=!6d1TLMdd==#M{8K2=h zpE%;fciUoMgDo63+cGrR{K+10|=?D~d9$!Bev}CmB z`ldcO)mJ-8?Q-H=1gm(z&03h|G4|ZdV)ln^2nJsH)wlI;IlzsQB-4|4M5#nP;#|ne zqoYf0yoAZricNB0<)fo-Byrb^;Si|rpfd$HGW)-DhRU{1G5bGtz6E{7QlFVfjjp=) zalTXEul93PGS|IT@K{c+@1AU# zUynXy(YoJn`@V36`}?ubcM{^?<$S;G^FrePyYq*!C7xa$xu-i<f`3&w_0lSG#0#75TA3mlJ{{vo(Vcm@I^dvjQSz|*f;N>l=Y!NDgXO2 zEw2z~OmwUS&)Ws{1qs&gdnAk>l#I2>#9sjAfXE&i+XT9n5T~XD&!&>EV=1juT9dCG z><^bvZEf}&H&89U_krFSqZN8Q&$B0|M36Zn&h^gW_O|a8hPQ7OP5@iST*um+sR@&} zpw)DA&1j`vcwV^EWG$s$FSB31VVUEKwNK}s>1V}1FN}f5l=we(nw!T#gPQ%PKx?77 zh1vh3)3{BXV~AAOUvm1Mh`fd8JG%ZX(uAjnCPwtbbdPr5Vs0*i4IzcCwiC5zAURJ$ za?w3I;Yj@+<`I-nE&2Gd%UtDDl#~)Tb>sC^(+n+^ce8ae{MP0s+80E-%Q>DUFJio?48i#ulc}Fy_;_0x zd>s-#p=Z?o)PY_ZPP3s=Q)NEtpj0#8(i!;vS9vzP2MnF#cL%1?IX>_eT9N7SozUT4C=y6LD8TM?mNvh|!SI#DC0*DV_gSr-|P?mHxH?x_c|Us8`p$ zQBN3}*V9Clne~hk>+d_h7GgC?%W1ynP*03f*+J&NC`7^5C;q>oj8IORYZITee2*|q z`^Ivld59UmPl&49EKI8oZ(_pBI=X)i?--&#U$z^3&frX9aC~Hj&iCz;($U9UQr#Pi zW9WYK>0e{rtY$q)Ug##qJdcUrA+~EXPVuMnso=OsK?)CYopB=C+!6Ro_r0ZzN$Q@6 z(_MS-H}UiAd#Bzg+$6Zu78RyoKZ4(=DqZ-Ls+8#Ut2POK>`re=qrW%b_IopaZ%K>C z@2y6*Lci*HLE_vbw5FxN_OQ*d`D&BMs0+eRX&+h=t{T=GN|^)E70iJ%0-2n|i+g3B z1D?oIZw$r#%$aDt8v6@~{>{a^y=b}VWiK=0sP}^Ky^*b#M^9v8*X=ExIma?3$}@iL zJp1Vu3%fsV7H&7WAP7d>`S%rSz`2Ibu+BhRkh$j+?qCR=r=WWg1o1|Pgq(y-=~}dD zS?CTl4%QGC{ZWfWiu;eMQBxYx{oOL~Dlxn(NXA4avu-Y#xQ(S65l;tzB@X_LP9xm| z!}(ayo=`a)w}D7KkJu}5f5{>H$tkRx8K+5scMo?RhHN!zqtQ0Rf@3?4`Q?*f<^Ezk zR=NpijxnAS7FN^>Kh3K}?RzZhtD(BC*yBraj|X;Q1}9K!kWH-j4qJC$>BH;ptQcnh zJL-Da{+i`KZf3kD1?!+mJ~yMk)Ked=i{45W%_|ZgE}Ms&-HLofn&KPDCAo||Gr85X zeKz*E;`W6G?5EL)H<&Sy1~Q&&vN2<|efLJl#uN#1z?}48;8JjMscaf%U;S<|af%B0-TuXUrzMqJw4VjVodQ*FpDbO%m@;iN;-$ zOHVZNfX5vlH~!6`GKhPw8h6b7lNG7v(tX+VZpprTC42XM<%sJ58-&>=)rCmV^-CdN zos}8C(q3%&_-eIRnu(o&tC++#+=X@>^~Asyo}>7W*ku-<+2-x$ znz$X%l(BsNRBnl;+`6E~&Mhdi*W_94MR}aP`e#`r?yHMtdD0Za%oHT|Z(4H1a%Ra1 zSJ8qa*ke(%;Au+>P8Oy1lW#q9Fh1IYdnU0?o$HwiN~leX|Bza#)O-59plPkn1%GSl zZmoTSZf&GLoUwTNd$ndjPOBiNY5H?Qb&HC5xy`x2jD1OO1^cHHZ+!3^E`WD{A$~bz z_GY2~_b0~}<4&@1@H=?Yic60;7C0z{%<_4nLpuX%*U0K5xJ_-e*hXS}+ml+kOE{02 zQM-H5y3n4bUvS@=%%epam+*TdP6sr@28b<;@WdmR$4HXD$K3loo^_su>vk!rBqKTkHQkb% zV@ziRVcrjGAn)cqiqJoAlh6cc2DAWL=l!ty(bcc6es#ea>{B^|eJ5Be0-OUJ1?&dA z2sjMbJCE+Wm$coJNAHM{wB3bgMNo3{80O98K}I_Q^0RZ^G0^7s-J|(Uf@1y|;VSag z0lo+Pg!+D&CwnUybQ!!mcB^fQ)Ke=+4xiws@r;g)WX2Cl=9&~-lV@CMT=r2xG9P|6ktf}&+cH@^e zuQXd*`t?(B71Sd_>O!8Ekjw-9i|Z|PwdGHt)s|_!UguccYFZ+MX&%g;W4p36^Mh?= zrJ|h(dLH+Xb+KV*M@tjV0Ciu4bvoU~;Pl1Tw2Q(sY4kkRGimP%a?R~|f`R1jBL(sD zunX@BO3f78Z@1{`J8lxDzEhnp#=)YiM12ggZ2$-p270aqrJa-FAz@ z6Y^hwgWO8o_Hv{k;C(asUmi)50=L;7F8Hp%;=^r1ZM8Gm2@#W?u|(rGKq?~-HAG<_ zSd^;0@n4R(VUJ^d^H4*CgFfMp){km*s+O(}21erqTdJAi>7LIcJ^M@P7)}RoQ2y8o zQ2pC5nXY4gpZfRv)OgqQ8>7j$t%#j29N=NrS=Xi8lq}O)w_u1BxA;7fHPrDdxPBA*iDzuljE`}ibu!{!&@gI5Z4j(8>RkB(mGh*GlkOjI;InT?U{ zy>I4@p9OIa_Hb^m7hniR^93OzPr{G(FrqFq`=5c|vODlZ7c^Hou5}P;X_oC!!z@)M zcDiR`=dwgRXYz1YQ5h?tw!brl(m&n*L8+Xc@nU!z=sTseubs!+G=*WfZy0;hSc>0b z%Y&Xeibv?ZlCJ*g4i;wJmd@(o^o&^Jh%Vgnp`zgjduEs zHN~gI&1(vPo2zM2b04*+rJuE^l_YIi#-}Z61xD6T-Exjh$ym4A7#rf;Vc^waBIkzq zHbLauP;aKAH}r(sxsLv=dbf{!zyw?U`2Z&lTl*DtPJa_|4*_51qqy1AL!1CgE$@CtuMnEF|fBcZ{06Xo|~;uvwI znHD|Tx%Repz5@BFT^GW0XTR~dV2l!HaTI3-og_?&Ss{(D>?EnckTyJf&T0+z&$W9! zM)j?fi#rwBn%2UI5FgH6(*SNhT<8PUvbVVTMo2f^Cb!~0?Bl`^#m0NMZNswYI*+PX zzO3%UZzzopn5(b2;fUx-5tUU78AK^bg7qAYtq@*3=CwPZ8s5p0HI%K}9-HQP?d`xZVyv7#otW^=m5d zy$aZ*x3p6l+l-dI1Kubqd|Uj*$!$g}U+ZTF_qV%3J7Fi@vb)@c$>M&BE6gn4Oso@A z84mdmEfSL3z!q-awqmVl#}gww1}b;hH&@Sxs6xEk|~DF zPtK92`&cHyW)rbA|JN_j3%XN=nerJ1LyKU}8?-M))EmoFT@>5Eopr;6S{3Z0rrF*J)ciO6%U!h zM)&sqb)0Xw-Z00J*EkAaA5?|3WaY06KN~V@;%#`O_pxh0q2J+bShl!tBY!1!k8-3@ zGwFlI|ERS1yA#2{>f7x4XW)rF;+Xs!3Xm8hZ=(QuUwkHdLT^Bvi!;=-(oN{eI1_F( zGVJym`CXu8dQ+kTX+~$!RQrZyTe8z99 zCER5)RMb8N513)2Fu_E_yG%CkClJZ_y&(Y}q<_Y`<41<#Q6WOSQ{(KUG~olx6V z_3tCF2ehZD?N7)0cKiQ!JsOQ(#DiO=qkplSqNgo=V|l!dDO?L(@m&GkMP*(YZRL9` z=&wavp57xL>!|^ir(-13p{Wj(^^S5#H{ILM4mu|={3M)Rv-KOTke=3fjzHU)P{z35 zl@X$jP5rF-oyLE760@#1j97Xed({s$j9l7Vr(qlAh5oJhwNbUy+xWMo*RlKf_I<6` zzX~cG;s{aDU(q%ebNO!Ei}zQIFV*3=Ae8Xicb_hW7sFFr|;M|tDg(X(W31fWCqSc_rnssX*23t;6P6DE{W8cL&bnZb3Rv4o*eA9*x{wa3?AkHpRGtF&4TNPgc6yJF)xOVSa4{&)2}nV#Iu`r2P1sd$GRLVZ+5>w# zaq?Vj$MgzTkH0t~yckg=90Z&kTO?GD#qG~pqHji9>0BOvA%f1OB2K~cJqffT5%u;3 zgHkymXTf~zWQaKeEu*T1s{c(X*U&(ycY{Sn}fjryY>UOCwf?i-{*5b z|K@z=%*>jZHEY(aJ+D2pHgxpPbrR2ANHM3|w&VSz!Ij#H;yreA_zDv9e+D{Z=_@It z1;^A%oY{y+Ki^pA4&AZY!ns}x4Z654qcrubJMPW0XM`8~WC>^8uy*e5JgP=m-)Dv3 z5hBgMnxRXwK`+&l>We$t^$WV!2>BR?04A3Pw$0&)!YXb8canl%Xo zKQ0vO4+x3#4+tqCukSb}^bX-74&eWL^HFE*iDN=vG2A!g2T?!454jxPIW_Dhy+aLq z>6{eRm+DG-Mxc)dD;C=IehRJ@yTFfELBn`D-5B;Y`_6SqdK0ttYr`;8t8(be_2tC- z#hc@9s~SqDyg43sgV#r>4I_Ds98 zre|`qDX7UCN#a)iDnDLC3>tcT@e|6?IH;@I7616>{jA#&CO^PqJD@VDE!g$yUq6 z7JY$xDo%Oh`$3Leg>au2a6Da@TuW>?Z34f)KXHs~B|o5kGsR_AKa; z>aiQTw__m}tTIjmEWN97$_*`Ybf+c=X8=QdPBMI%=RRPsO^e+@buDPqCrS4atd^^| zuVl4+FP2BxCtx(8Tpo~rr5vX~_Y{|E=Rl8Y7uYVT`-Lx~p2YhVSIF*14#$LEF$D27 z`|t1`$@%?Ir@?ta<$T;(>@CxC0pVPT!V~Ro(nO}JrJdl8lg54%+}Vm{ zxJhzcsKgFhGEOje#jcsg3q0o0OKH5~mx=h6gfA1Ek!ne}uWlJ`Z-_p(W%y!(=K}+#d_&?o?9_HFc?9 z;!=<_PIwPm-Sr2c2RQ zobWV(_JgReA}t4n%J9>u!$H*HG^jiXYKb3I9;L&*^)|6(p#O^6#O2N@hW=Z3dK>TzW)jfhTGeVP7g*q#ny%;zmqb%}OQQ4U`eW@vbmkRPA5aPnqI9}x z4zw@a;NBr{Z^#Eqe@$aC(NzQ*${pzB4aTChnW;0=Nz#^XCs}hz!WiZAjd`t$rp8q~ z$8u5_5OrMG4vpZSz{f$K)|1g{`6Ai6S+0=Uc%KVfn^tvAJKJ6Q;>E}Q?AcQHP@X1mIpDa~-WvSXLrXs0=PnN2^;WwnJ4*d8sDm=-6_gH_bo&Zg0 z6RKB@jO*o`G^m=SgslPX^1B|AZ}i=qhpVQcRTKB^0zc_#Scdm-0jZ8F+^;xuJtw+f zxr13Pj|zm>cgI>YzUhN^caB@2x2&1-yRwsf&TUThkKtL35TDhED7rVj67R#7x6Ab* zVYxfW?^P>}qRo~*tzAXwf5ED5zi-Ucg7Nsi-#58~>Tu_|?G^M*ij2{iL46aoDUHNl z*qf#sMJ=-$gud>FAU@058ik0Fu)$?%G+S zW59t+|GNFarLU$O5XNUi_j0zO?}&g3A#GfRkU#C7^m~vpYuo|BJWU%A*B6>N;GaA0 zz@>ix+j0GYOJ^~o7!Q3y6MGyGX230mE$dMsteh5voU5j#BF`4!nqeLl&^Mz($Qh3` z=74nM8lT?R10}s^ZVU87+86sdCP4=P@^1pJiGe5!Hs2d%BTpIpb7AKrT#oR1Gtwcv z1@tVWP2jC1~vy`wDoJIXXGB}QYn__>!x?{?bTgO8@-ijjqo3aKN0@@ za3>)=7=Ge2orQ4Aa8ExY3@T@L)skk7N9uMu?0GZyUcnK%MBH1rL3H{n8QM*pu6Q_I zmUZC`LEe>^!%`b*c-QkB?{c2;PUnFyb2!Gqu=GW^8Q3i;?VZnrL6Zh0EQ@ozHY6vp z)&p;I%}Gs`k!?Xqv6xddVV;cCp-7ePd~L{RzXUT**@lHSSz_8CuO7?~s|k)I>C+av zc^(%1u`nq5jIeV3-nh`C-jHx#*Penr9=h&H3~Vpq4wQg<;>g=%@wUOWxg{;>t2%rM zQ>4HZ6byyzN4URJtU#Y($I<&>ySQuPtr+o zKIfe&$!anBLgNn2Tn~)2VJ*&hQK1>e1v#8F^>RzV?PZ*2=|7iFI5{-2z@}<$a3h_$ zk9AI3f+y%&8yDtYb5F7r`koo)nk|;g;_N;$#o%hSyxPM0Mx;e#vViUb(4SAjd;dw9 z)tKE(t(K&l;^-V%oBv4RTog{n;DpVkeG-O9N~H(>NnX&Py=s#0To(Lg z^*GE=N&EMInvPAUlb$trOIeLK5tYK~Xi7^d_tzC+zPTBe!ySaT<4C^8oBMF--}FFVS+#x!1Yf+`qVMfct6!HIbTOnh`gZP9>#j z%$hkO^fAqH&1%i7qW_;7I%zavx__~^&d051*WBV)ivL~g7|`|X2dUcyj*%{-QxANI zb%Enq3V*oMigVX2ATUYzU{~t>OLWB(miX#Yi!}jP>!78w`AXf}c<&=4%Zbgkf&SgiW$P%I#U<^==D= z7&q=(_t_}ma|#{tuNOt#)%I}H_sas8^+-jPGyRRi0R%IkASD^ zXl?M^bw1#6F@E?C4!hU$A@TjL&rh8{%JD4u_1EX6J`9b6f@MwfLkRm^FR3fS+~CFN zF1fVknSd1!xw*M(W#9T-U-+KGK0qbD6%{Tbab_a5Z$JG@dD z8a6Dzy)G%$$87NPNf=p|kkZ9$h!pDLQ@lhT9lY)t&~jmr$VKrf4jR#TbsO>d)O0Qo z`>Fhe3)=+;4}!E={@rrnf`@;XVn_efVkiG6iX8&ZAePJ33>b0k3*>Sz3`o%eZ>yyd zH*ZEXR&VQrk;B;wqe|wp;@oS}^+OkYM(q?FG2}K`F0+U! zUBr~x`22(S#lI~|p;6v*UnqD)L67kD;q5%OS_+y5iJXA`6=5Lx_;KK&#tYo#rLznb z!j^%46ca|e=SZ_dCS43az1!wtGH0AKiOY+^E=cF3ma6X%2E_8o>+|dM$xc1^!cNNCxw(5Sw zu{Q_qyWozRhFAmVS}hM)yCip!^>UDm_(Be7&KCif#)uTS<*-qZiRa;xo>rP?Y)9SKWqC`tuPxwKcsG=lS8ZhDSishui;zcVY$y3YmD8E zqm6Bt(P@1akPvFZo z$2XRjX6G(RbufB2cy#ND5_(Q|TH&+??^NP6L4`F=Yn)au%-H{fuKPcF;j3Pnt@!$} z1HN;)^Fl6J~X77d%yY;D}IyeSGW-&8$4D@a!O&eYd2%7BsHy7=R2U9RJ z^*tsSWF2va;A7Bu^|_0~)w}Z&PfQo@9^Mw*W(g7cL9dc__-U7fs;Q}m1&`qr@^*2# z8aCB%Nxw(f645sv|MR&%<|M)KVJ_@7dpVQv1?nh+AQ6gH`X}^y;4K9Ngd^5{z%x;+ZX?D#k^3r#{9hF_^P$$zt-_M zLq2*zSa4GHGQk^t;2SYp1s}L?#TZhy3cg{^TXqZE0J{_GVz=w?_g;WAtsK6hGbDNG zPQh|ktP2|s9iMqb|6yn#|MS_1QHKS-e`pH6bm4bc2<(3td>NQ>STOh=7LtPxLzCWN zAs2pdM=&6-V?5%(gVgfZM~mPPLy#b>QB@W zX*PG zA$h0_8LFRvy+4#b0A&u)=+7bkjhI71LyYADNu*Qo9n~(-<>eUM1@9`gNlqfq(vn~5 zA6WI0Ua)-D8Y^N?FU%@=%-jTn;_u>~htkV8DspLXPoI@0^6*v&?HBmiCMbs5T6UOgbTCy|Sgs;NQnu z(aikrOUg=ec-CT>*ygZybA1=*?1XoB9ul&`Uq#-+J)h%il?WOAuvc{$en_(Tq|j4x z61!8MzmnNALS{51_9U_dK3fX#T=^dppf9wQ#*(KOK>@bmsEkOp7Cp8{|UPp_6^v*uq4MG7|)Nue-_rqkMVzjJp*`YsLa z8dAt>-x66{I%}|$FUA)P8oSb(h2AeunBiKzQrH+u8pke|XJcYjIpL-p*~YV&7iC9SOHxg05E^ zZsHslj=Nz5h&+fHUMs#wgL3h_7#g;roF}n9VIz)XeyN{-r^`xVQ?#}UJ7qI{;y>Am zlVFzpEr`1l--hNKtAVFia&OY(p+U?p#H^eoKpXmQ>!K?uSf$+u`c_6$$@ll_RmGva zmC@KcZSvSb8ZlQ#qgFAb-x4RhWBli@#3;^!=TRQcSBzDpp`PP!uZ!3r*BO+us3&lX2K5iNRCB|jIn}_4?+Y$K_T0|n&J^COVtug> zdM&G=y9(Om9nR`fCybBJattT^+;QG(;;tQY9i@S`(Ku+scS&6n*K`cMpin%L=9rEb zr2d%XnXJ%&+SW1Lg@%qii`gnz{?yiF=?Mwj)zoBhg_L~@t;$s9LAdv)(Hn=V>rIx& ztmDDsqb)134!bu+IxpB{IoXl}=wb2ypcvO=(X@^R>;tqI-V#9zD7oOP?4$QaU3N5C zTABsR0^t1y(n;|Xpwm>$@kEy$R-9*Swd`%i9admk)e?x6+2<|Jp;Kz25nhh(K{Z*H zVV9sg+U=`0u5TdD?ysKNYFW^HcM4XXxmJsFi#OVPQ)Fc7L19az4*Hv!7YjIe>9YU9 zOZ8)$EX-=Pn5@!@KI?iIzFcRuq+Rb1jr~?jGQJPhFQWd= zD?_h1v6Z+o==!0nEQ`5vg9S@=>u%^9tH-Gh;#@yDU9y;tm(vM2HP1y~t~pWX61uLY zTk`6%9i|J=og!oY)fwnf4<5q$9P4TwHW= zbaHmlx_R8@&4(GD;V|;T&;U$cPe%&f2p?6&6a1Wp{uy(ErKorZ3Q6h7t&rngC;)Ro z4ikPdbvEG`Ilq!V!3LknCVr|-`0pxu34b7nBD$O@023>RkYX1puh?O!pi%-VKXxe7*U6;s^@ z>Fe9t$S#-hD{eLak-y?`I{=aQ)xeY&`KTB1;YeQrI}dgp>}psm>}J>JGeXm=LA4O7S^W9 zGgj;`wW9$riX(F|LZT&C#TNw@rh?T1AGLF}3ZDX7u7a^UhxIEf$jH8U_2|D|T(J55 ze^gt(-Lt9UrGO!QG^5iVnElwA57z(ipHGe#m5fdHb=F6X8dp3s#^2FrL)EH9pZ)#2 zD_&ju51RJSBt!Fs{a-%4V)vG>4xDRx<1fpX-5u=KJ#O+`)5@jW-Z|Uy#o_tJqzT&F zBL?1=koV`%upYV{Z$0t+q1v{8_Z{Ag8fKf}%oGKKNB;DlIE0oE9Dt$=JrttG^?j^NJu)paVnCvJY$s z8j0Ge8?4e^@ZMx{zop=jI0?AkfZ0PVo+`sXK2003X0;E_ERwkaM{j|+Pi43iOaD<+ z2;-5L45dE;KM(iA5x}LwLvV^`2Iz475uSv*y#I=**aKIG7(BWnRXa}d-W2_NUA*l}pbomHQyK9o*AMOSmzp7DZQ)~5?i@;%_{|iV;5cRvJRI=+5+8^MB;w_; zRION8YLkwPL2ejjEXN^L(J1K`sltSdm%~bWy-ipr4-iWHQXBjmp!*!4)ce z6ejr~L-@#aL@(izvoTwGJ4#P@ZYBNC;kP$GNY5F`aRPLm$*f^VhDtEff7T8|nqT|8 zQhgQkvEqT-(ZhZOa0Rcz`|8I#?iZA2SB3H?z4K71ch@}uQCx43`r9M1)TdIvE%n_u z;NOjYFXp}8i$rUS@JnR^VjCfcN$zPpv3;JYuk-|*WLQ|M5nXuOO|m=Ym|$*C$lG`{ zxYVDwN3%9r4sSqi_EGIRp1vIOaAh#Wo;2XG&X)ldwe^kY>htBd5 zzH^vkQ=J>(CM4b)nm^=jV|f0cu+W6WxUnOpv591l=D!zF!GGfcJ&1}d0kIza5CQW& zRUC%uNj%OXhQLsmj1s2u$c&cb6@F44GD`ZRQc$Fm;`Z6}U60G5;WVM?O#?{S%a1^?ec= zO?a_Q`p<0Q2|pPHKZSo!d7X{lwm!#f>QgS)m%0h^y2||p$qN~sP5AFN{4bXADgDzc z$U}IP{LYA{u);4Ih}H2Pa#%*oZbz8>%J@@GiD4A_D4vWWk3UBbWv#@Y(moWn#rL%w zuh46oeu-Rvv3#W6qpCmA@rWEIvarlxKbM~3ZPQb}aLfU^WlJnt1pl3|L$jE60xXq5 zb*;!`+NB$rcF)5SY!bqIkY_MlO5+2!DFgGE$E7gA)FwNgX-C5@MH)3+@h`<3Wa$Q+ zwOk;@J0V>>Y#%xO`zx6?r$Ea2iSU8D#L_24BY}8k4oVe01p72 zv*7NA?FV~)Ez>TVEyZsg#k6n3>H(+xqaOqP&m-LuDZD=g{6u^@a8Ug2#XleKhBVJ2 z&+g6G`(G&KIh_oeVfVmhA}$v0?lq|6A}OBms!h{KraiQYX-E7Sd_;YAjAq(@!7c;* zHPCT22{f$6obgF1|JMkwgC!iN5Pu~RXG{{nV_6PfMVo&Bdpls1W~(Yb8hCBvr^#}% z2ri|!HHsYN{X~3hc6Q>J96l*FB`q;Qhwajc$U%eezGui#_Vl0`* zPc_b(QN|bL<;^N7G35{A!{)NFg|kXc`FufPnW>~?R&g0`HWud3FqQCm#u+nA`Fb52 zW13b_idag`=NHU2l}s}gmg)I)W3h;VM*`84(vE{tPAxzYd|6QuUo^GMRLBPcIIy%o z5T+E)HqI!>=i_5D__7jXVQHR;u=0Vd|1jQEXq++wyK|tm0OjjgPDz1?hc7ji@v{m` zXBDF|WdKq3nML`gVLXF3lK#T!54)#GI=QsiMEMG4;=gEC86OxY-uKxf&3GG1qm}1O(0PN(m(aIS)IwogJ64^xAXirm)75OrNPc{}#15n38 zW58E5p@^rbsBA`&G2c{jGrSbdq`>3a(}LTP#!m%u65~=63d=7Yz-LSCQ&1{4QF}v3tkyA|P8akOhEI=8 z$>6hds5U>5BuIw`@rGzmDKeHocIOqFSbS08)PiZlAh~>8J5+k^_>smk(;VYGkEp<(m`vT`K)ZtTdSi%3YO5O=SeXm1}&$d42DG`j4pZG!P zZR@9$Z_6K|hl~0o|K$Jj`)pEE;Tv~(JA)3 zcEUSbRUe)TuoK=pRCt33t)1`!k7yJ=*bDFfVY^8c_?`bpReaDBE<5qJuS(7W39X&* z{vYyJ=FIP7@E`n@_}5wePG|A=u1=EQe6WMwU67%^M+a-KXJZsi%3x?rvM1iJWqRq| zi#_q~BVu9bos@!CfvL+KM$dpf$Ijwd z#^a=wL9wnb%G=;`cqNw4TJFO;xp8=xW@g$_*jbaA7PrH+J>qb#3ifu`X1u#L+Q>^g+2#qk;(bTl&hy zHaKvCR3U{5`ORkKkMO$K1+Q|pi|R~s^xsnFjw+=-|0?3O6Ca+D3;R87)DgXZ$b-a{ z|228|quu^D+l|h<)3jIVckIjfQ}zV z(2-9EqR+)?fyRKNn@$?ZaUch=oD%C`U`{v3>YQb?*x`0?b@X(|V?}Hxenu#DF|q>Y z=1>f`5OZuJb9M-0;mp$^on_oC+fb(Q_HbfD-qj9z-F5xQCtB9eUUK`p zCdP$1^aA`$?&;d4iHFl(JZf6^QvTyO!|&jX`iYBs$$uMO9F0rK7@Hl(+D$pu4jdLb z3CzT1gOohDDIlc`ZV8(Tf(SB_jRiCTxiZ+pfDmjjrpH4;ULFfYs=MKz3f~~OLlH-? zL5PchF9Lo_e-GTj$YDf9O-S_!@C6bj9MPZ{TM8xkQ{-??NLNW7*ju5Q*_mn-KL!Fp zWY8)OSN&k@^W3QKtx4x~?1R@yUN6zQHXqhr>zbkWQY&q_-Chg zCF(m~y~Psj^BnIKaY^*r^SPiqUt|<{u$P=UtK`g{^9#O{@aj7W?>?3MD*Npc$A90y z`fi_IL}ed2lOJR8eTE`u1S9s)I}QaV7I_PQi#q5Z2u667XYjW?+ugulLQQN|_)9Rt ztA-(XOJ*V=fKk&%Fk5i z5b;5w?Z>HZ9b!t z?{S$)_U3PmjF+Axd(vxA(M#=SuW{4IhQId0JFJuNMs*Tib^l<`b(TSFEBnuPIkPcqZVFy|wwrogD9tBD1xa#i{&pveh=rl`u|4rB>z3a$XA%js`Fols zH88X;v}b=tjYdN!$(cRJp>W_Mqu{ld{8mV4?fzWM>mHK+O1b7@u4%@tuvmtT$P+j^U2ORhx1$3 z*pj5){BQdn&0cu_*UnvZ)_b(uX~6=c>USi-@BCNylk)uGcVd*X+rD4!SR34}7o}cz z+jIZH(dK!xm;8FE@XGv^d$?P#yE+HyRw2|R57PS~`<#0toh*cx#Lb@L@i-ek*lYeU z_Q&G|DF6LN(bZ$`d8&9J3gp=v?>c!MK>< ziDkTUww7Gwd~LqZIXBKBpN%etP8ghuyKX#m2BADU>-)*XpFZn*Z{nD2 zYLYpnujH?Ov?g}dxD|hkUh&AQ%UgyVu5&v5Ws0ls*^`?xL*Bl&d_(obrM;qC&J6qZ zlOb0RU&&ZCWW>RBg?$HhxpaTTlMg**FkBpb#CU4&qzg~gUdTSCtKL0f#k)aY8; 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 ea094e1de..b9c54d48a 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 @@ -1421,6 +1425,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 9d47c5bcb..a54193dcf 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 @@ -2737,6 +2737,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 3ea99252e..23872ca62 100644 --- a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py +++ b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py @@ -188,6 +188,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 ef5148741..bd20ecc5f 100644 --- a/starpilot/system/the_galaxy/tests/test_navigation_params.py +++ b/starpilot/system/the_galaxy/tests/test_navigation_params.py @@ -251,6 +251,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 285d91063..19f416ec0 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -92,6 +92,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" @@ -2509,6 +2511,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 = [] @@ -2517,6 +2520,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"]) @@ -3322,6 +3334,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() @@ -4456,7 +4479,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) @@ -4506,7 +4529,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 @@ -4537,7 +4560,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) @@ -4918,6 +4941,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() @@ -5001,6 +5026,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