Release 260111

This commit is contained in:
Comma Device
2026-01-11 18:23:29 +08:00
commit 3721ecbf8a
2601 changed files with 855070 additions and 0 deletions

View File

@@ -0,0 +1,57 @@
import numpy as np
from opendbc.can import CANPacker
from opendbc.car import Bus, 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
class CarController(CarControllerBase):
def __init__(self, dbc_names, CP):
super().__init__(dbc_names, CP)
self.apply_torque_last = 0
self.packer = CANPacker(dbc_names[Bus.pt])
self.cancel_frames = 0
def update(self, CC, CS, now_nanos):
actuators = CC.actuators
can_sends = []
apply_torque = 0
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)
# 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))
if self.frame % 5 == 0:
can_sends.append(create_wheel_touch(self.packer, CS.sccm_wheel_touch, CC.enabled))
# Longitudinal control
if self.CP.openpilotLongitudinalControl:
accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
can_sends.append(create_longitudinal(self.packer, self.frame, accel, CC.enabled))
else:
interface_status = None
if CC.cruiseControl.cancel:
# if there is a noEntry, we need to send a status of "available" before the ACM will accept "unavailable"
# send "available" right away as the VDM itself takes a few frames to acknowledge
interface_status = 1 if self.cancel_frames < 5 else 0
self.cancel_frames += 1
else:
self.cancel_frames = 0
can_sends.append(create_adas_status(self.packer, CS.vdm_adas_status, interface_status))
new_actuators = actuators.as_builder()
new_actuators.torque = apply_torque / steer_max
new_actuators.torqueOutputCan = apply_torque
self.frame += 1
return new_actuators, can_sends

View File

@@ -0,0 +1,103 @@
import copy
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.common.conversions import Conversions as CV
GearShifter = structs.CarState.GearShifter
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
self.last_speed = 30
self.acm_lka_hba_cmd = None
self.sccm_wheel_touch = None
self.vdm_adas_status = None
def update(self, can_parsers) -> structs.CarState:
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
cp_adas = can_parsers[Bus.adas]
ret = structs.CarState()
# Vehicle speed
ret.vEgoRaw = cp.vl["ESP_Status"]["ESP_Vehicle_Speed"] * CV.KPH_TO_MS
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = abs(ret.vEgoRaw) < 0.01
# Gas pedal
pedal_status = cp.vl["VDM_PropStatus"]["VDM_AcceleratorPedalPosition"]
ret.gas = pedal_status / 100.0
ret.gasPressed = pedal_status > 0
# Brake pedal
ret.brake = cp.vl["ESPiB3"]["ESPiB3_pMC1"] / 250.0 # pressure in Bar
ret.brakePressed = cp.vl["iBESP2"]["iBESP2_BrakePedalApplied"] == 1
# Steering wheel
ret.steeringAngleDeg = cp.vl["EPAS_AdasStatus"]["EPAS_InternalSas"]
ret.steeringRateDeg = cp.vl["EPAS_AdasStatus"]["EPAS_SteeringAngleSpeed"]
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
# 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
# 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 = True # cp.vl["VDM_AdasSts"]["VDM_AdasInterfaceStatus"] == 1
ret.cruiseState.standstill = cp.vl["VDM_AdasSts"]["VDM_AdasAccelRequestAcknowledged"] == 1
# TODO: log ACM_Unkown2=3 as a fault. need to filter it at the start and end of routes though
# ACM_FaultStatus hasn't been seen yet
ret.accFaulted = (cp_cam.vl["ACM_Status"]["ACM_FaultStatus"] == 1 or
# VDM_AdasFaultStatus=Brk_Intv is the default for some reason
# VDM_AdasFaultStatus=Imps_Cmd was seen when sending it rapidly changing ACC enable commands
# VDM_AdasFaultStatus=Cntr_Fault isn't fully understood, but we've seen it in the wild
cp.vl["VDM_AdasSts"]["VDM_AdasFaultStatus"] in (3,)) # 3=Imps_Cmd
# Gear
ret.gearShifter = GEAR_MAP.get(int(cp.vl["VDM_PropStatus"]["VDM_Prndl_Status"]), GearShifter.unknown)
# Doors
ret.doorOpen = (cp_adas.vl["IndicatorLights"]["RearDriverDoor"] != 2 or
cp_adas.vl["IndicatorLights"]["FrontPassengerDoor"] != 2 or
cp_adas.vl["IndicatorLights"]["DriverDoor"] != 2 or
cp_adas.vl["IndicatorLights"]["RearPassengerDoor"] != 2)
# 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"])
return ret
@staticmethod
def get_can_parsers(CP):
return {
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),
}

View File

@@ -0,0 +1,12 @@
from opendbc.car.structs import CarParams
from opendbc.car.rivian.values import CAR
Ecu = CarParams.Ecu
FW_VERSIONS = {
CAR.RIVIAN_R1_GEN1: {
(Ecu.eps, 0x733, None): [
b'R1TS_v3.4.1(51),3.4.1\x00',
],
},
}

View File

@@ -0,0 +1,37 @@
from opendbc.car import get_safety_config, structs
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
class CarInterface(CarInterfaceBase):
CarState = CarState
CarController = CarController
RadarInterface = RadarInterface
@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
ret.steerLimitTimer = 0.4
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
ret.steerControlType = structs.CarParams.SteerControlType.torque
ret.radarUnavailable = True
# TODO: pending finding/handling missing set speed and fixing up radar parser
ret.alphaLongitudinalAvailable = False
if alpha_long:
ret.openpilotLongitudinalControl = True
ret.safetyConfigs[0].safetyParam |= RivianSafetyFlags.LONG_CONTROL.value
ret.longitudinalActuatorDelay = 0.35
ret.vEgoStopping = 0.25
ret.stopAccel = 0
return ret

View File

@@ -0,0 +1,71 @@
import math
from opendbc.can import CANParser
from opendbc.car import Bus, structs
from opendbc.car.interfaces import RadarInterfaceBase
from opendbc.car.rivian.values import DBC
RADAR_START_ADDR = 0x500
RADAR_MSG_COUNT = 32
def get_radar_can_parser(CP):
messages = [(f"RADAR_TRACK_{addr:x}", 20) for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT)]
return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, 1)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.updated_messages = set()
self.trigger_msg = RADAR_START_ADDR + RADAR_MSG_COUNT - 1
self.track_id = 0
self.radar_off_can = CP.radarUnavailable
self.rcp = get_radar_can_parser(CP)
def update(self, can_strings):
if self.radar_off_can or (self.rcp is None):
return super().update(None)
vls = self.rcp.update(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
rr = self._update(self.updated_messages)
self.updated_messages.clear()
return rr
def _update(self, updated_messages):
ret = structs.RadarData()
if self.rcp is None:
return ret
if not self.rcp.can_valid:
ret.errors.canError = True
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
if valid:
azimuth = math.radians(msg['AZIMUTH'])
self.pts[addr].measured = True
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].vRel = msg['REL_SPEED']
self.pts[addr].aRel = float('nan')
self.pts[addr].yvRel = 0 #float('nan')
else:
del self.pts[addr]
ret.points = list(self.pts.values())
return ret

View File

@@ -0,0 +1,100 @@
def checksum(data, poly, xor_output):
crc = 0
for byte in data:
crc ^= byte
for _ in range(8):
if crc & 0x80:
crc = (crc << 1) ^ poly
else:
crc <<= 1
crc &= 0xFF
return crc ^ xor_output
def create_lka_steering(packer, frame, acm_lka_hba_cmd, apply_torque, enabled, active):
# forward auto high beam and speed limit status and nothing else
values = {s: acm_lka_hba_cmd[s] for s in (
"ACM_hbaSysState",
"ACM_hbaLamp",
"ACM_hbaOnOffState",
"ACM_slifOnOffState",
)}
values |= {
"ACM_lkaHbaCmd_Counter": frame % 15,
"ACM_lkaStrToqReq": apply_torque,
"ACM_lkaActToi": active,
"ACM_lkaLaneRecogState": 3 if enabled else 0,
"ACM_lkaSymbolState": 3 if enabled else 0,
# static values
"ACM_lkaElkRequest": 0,
"ACM_ldwlkaOnOffState": 2, # 2=LKAS+LDW on
"ACM_elkOnOffState": 1, # 1=LKAS on
# TODO: what are these used for?
"ACM_ldwWarnTypeState": 2, # always 2
"ACM_ldwWarnTimingState": 1, # always 1
#"ACM_lkaHandsoffDisplayWarning": 1, # TODO: we can send this when openpilot wants you to pay attention
}
data = packer.make_can_msg("ACM_lkaHbaCmd", 0, values)[1]
values["ACM_lkaHbaCmd_Checksum"] = checksum(data[1:], 0x1D, 0x63)
return packer.make_can_msg("ACM_lkaHbaCmd", 0, values)
def create_wheel_touch(packer, sccm_wheel_touch, enabled):
values = {s: sccm_wheel_touch[s] for s in (
"SCCM_WheelTouch_Counter",
"SCCM_WheelTouch_HandsOn",
"SCCM_WheelTouch_CapacitiveValue",
"SETME_X52",
)}
# When only using ACC without lateral, the ACM warns the driver to hold the steering wheel on engagement
# Tell the ACM that the user is holding the wheel to avoid this warning
if enabled:
values["SCCM_WheelTouch_HandsOn"] = 1
values["SCCM_WheelTouch_CapacitiveValue"] = 100 # only need to send this value, but both are set for consistency
data = packer.make_can_msg("SCCM_WheelTouch", 2, values)[1]
values["SCCM_WheelTouch_Checksum"] = checksum(data[1:], 0x1D, 0x97)
return packer.make_can_msg("SCCM_WheelTouch", 2, values)
def create_longitudinal(packer, frame, accel, enabled):
values = {
"ACM_longitudinalRequest_Counter": frame % 15,
"ACM_AccelerationRequest": accel if enabled else 0,
"ACM_VehicleHoldRequired": 0,
"ACM_PrndRequired": 0,
"ACM_longInterfaceEnable": 1 if enabled else 0,
"ACM_AccelerationRequestType": 0,
}
data = packer.make_can_msg("ACM_longitudinalRequest", 0, values)[1]
values["ACM_longitudinalRequest_Checksum"] = checksum(data[1:], 0x1D, 0x12)
return packer.make_can_msg("ACM_longitudinalRequest", 0, values)
def create_adas_status(packer, vdm_adas_status, interface_status):
values = {s: vdm_adas_status[s] for s in (
"VDM_AdasStatus_Checksum",
"VDM_AdasStatus_Counter",
"VDM_AdasDecelLimit",
"VDM_AdasDriverAccelPriorityStatus",
"VDM_AdasFaultStatus",
"VDM_AdasAccelLimit",
"VDM_AdasDriverModeStatus",
"VDM_AdasAccelRequest",
"VDM_AdasInterfaceStatus",
"VDM_AdasAccelRequestAcknowledged",
"VDM_AdasVehicleHoldStatus",
)}
if interface_status is not None:
values["VDM_AdasInterfaceStatus"] = interface_status
data = packer.make_can_msg("VDM_AdasSts", 2, values)[1]
values["VDM_AdasStatus_Checksum"] = checksum(data[1:], 0x1D, 0xD1)
return packer.make_can_msg("VDM_AdasSts", 2, values)

View File

@@ -0,0 +1,23 @@
from opendbc.car.rivian.fingerprints import FW_VERSIONS
from opendbc.car.rivian.values import CAR, FW_QUERY_CONFIG, WMI, ModelLine, ModelYear
class TestRivian:
def test_custom_fuzzy_fingerprinting(self, subtests):
for platform in CAR:
with subtests.test(platform=platform.name):
for wmi in WMI:
for line in ModelLine:
for year in ModelYear:
for bad in (True, False):
vin = ["0"] * 17
vin[:3] = wmi
vin[3] = line.value
vin[9] = year.value
if bad:
vin[3] = "Z"
vin = "".join(vin)
matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy({}, vin, FW_VERSIONS)
should_match = year != ModelYear.S_2025 and not bad
assert (matches == {platform}) == should_match, "Bad match"

View File

@@ -0,0 +1,138 @@
from dataclasses import dataclass, field
from enum import StrEnum, IntFlag
from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, structs, uds
from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts, Device
from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
from opendbc.car.vin import Vin
class WMI(StrEnum):
RIVIAN_TRUCK = "7FC"
RIVIAN_MPV = "7PD"
class ModelLine(StrEnum):
R1T = "T" # R1T 4-door Pickup Truck
R1S = "S" # R1S 4-door MPV
class ModelYear(StrEnum):
N_2022 = "N"
P_2023 = "P"
R_2024 = "R"
S_2025 = "S"
@dataclass
class RivianCarDocs(CarDocs):
package: str = "All"
car_parts: CarParts = field(default_factory=CarParts([Device.threex_angled_mount, CarHarness.rivian]))
setup_video_link: str = "https://youtu.be/uaISd1j7Z4U"
@dataclass
class RivianPlatformConfig(PlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'rivian_primary_actuator', Bus.radar: 'rivian_mando_front_radar_generated'})
wmis: set[WMI] = field(default_factory=set)
lines: set[ModelLine] = field(default_factory=set)
years: set[ModelYear] = field(default_factory=set)
class CAR(Platforms):
RIVIAN_R1_GEN1 = RivianPlatformConfig(
# TODO: verify this
[
RivianCarDocs("Rivian R1S 2022-24"),
RivianCarDocs("Rivian R1T 2022-24"),
],
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},
)
def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str]:
# Rivian VIN reference: https://www.rivianforums.com/forum/threads/rivian-vin-decoder.1546
vin_obj = Vin(vin)
line = vin_obj.vds[:1]
year = vin_obj.vis[:1]
candidates = set()
for platform in CAR:
if vin_obj.wmi in platform.config.wmis and line in platform.config.lines and year in platform.config.years:
candidates.add(platform)
return {str(c) for c in candidates}
RIVIAN_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(0xf1a0)
RIVIAN_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40])
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.SUPPLIER_SOFTWARE_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.SUPPLIER_SOFTWARE_VERSION_RESPONSE],
rx_offset=0x40,
bus=0,
),
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_ECU_HARDWARE_NUMBER_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_ECU_HARDWARE_NUMBER_RESPONSE],
rx_offset=0x40,
bus=0,
logging=True,
),
Request(
[StdQueries.TESTER_PRESENT_REQUEST, RIVIAN_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, RIVIAN_VERSION_RESPONSE],
rx_offset=0x40,
bus=0,
logging=True,
),
],
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
)
GEAR_MAP = {
0: structs.CarState.GearShifter.unknown,
1: structs.CarState.GearShifter.park,
2: structs.CarState.GearShifter.reverse,
3: structs.CarState.GearShifter.neutral,
4: structs.CarState.GearShifter.drive,
}
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]
STEER_STEP = 1
STEER_DELTA_UP = 3 # torque increase per refresh
STEER_DELTA_DOWN = 5 # torque decrease per refresh
STEER_DRIVER_ALLOWANCE = 100 # allowed driver torque before start limiting
STEER_DRIVER_MULTIPLIER = 2 # weight driver torque
STEER_DRIVER_FACTOR = 100
ACCEL_MIN = -3.5 # m/s^2
ACCEL_MAX = 2.0 # m/s^2
def __init__(self, CP):
pass
class RivianSafetyFlags(IntFlag):
LONG_CONTROL = 1
DBC = CAR.create_dbc_map()