diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 8ae77875f..8a42f8b6a 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -223,7 +223,7 @@ struct BackupManagerSP @0xf98d843bfd7004a3 { } } -struct CustomReserved7 @0xb86e6369214c01c8 { +struct CarStateSP @0xb86e6369214c01c8 { } struct CustomReserved8 @0xf416ec09499d9d19 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 79f11f6bc..412660bc6 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2609,7 +2609,7 @@ struct Event { carParamsSP @111 :Custom.CarParamsSP; carControlSP @112 :Custom.CarControlSP; backupManagerSP @113 :Custom.BackupManagerSP; - customReserved7 @114 :Custom.CustomReserved7; + carStateSP @114 :Custom.CarStateSP; customReserved8 @115 :Custom.CustomReserved8; customReserved9 @116 :Custom.CustomReserved9; customReserved10 @136 :Custom.CustomReserved10; diff --git a/cereal/services.py b/cereal/services.py index c40538eec..4097e484f 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -83,6 +83,7 @@ _services: dict[str, tuple] = { "onroadEventsSP": (True, 1., 1), "carParamsSP": (True, 0.02, 1), "carControlSP": (True, 100., 10), + "carStateSP": (True, 100., 10), # debug "uiDebug": (True, 0., 1), diff --git a/opendbc_repo b/opendbc_repo index bb37b5678..a82c8d83c 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit bb37b56789abd13b6cdd9ff251df9c644a050223 +Subproject commit a82c8d83cd776dc36f84568f00e9adda73d8a563 diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py index d441c648f..151a8479a 100644 --- a/selfdrive/car/car_specific.py +++ b/selfdrive/car/car_specific.py @@ -1,4 +1,4 @@ -from cereal import car, log +from cereal import car, log, custom import cereal.messaging as messaging from opendbc.car import DT_CTRL, structs from opendbc.car.interfaces import MAX_CTRL_SPEED @@ -16,14 +16,14 @@ class MockCarState: def __init__(self): self.sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal']) - def update(self, CS: car.CarState): + def update(self, CS: car.CarState, CS_SP: custom.CarStateSP): self.sm.update(0) gps_sock = 'gpsLocationExternal' if self.sm.recv_frame['gpsLocationExternal'] > 1 else 'gpsLocation' CS.vEgo = self.sm[gps_sock].speed CS.vEgoRaw = self.sm[gps_sock].speed - return CS + return CS, CS_SP class CarSpecificEvents: diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 62dc818cd..1b9a1522a 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -73,12 +73,13 @@ class Car: def __init__(self, CI=None, RI=None) -> None: self.can_sock = messaging.sub_sock('can', timeout=20) self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP']) - self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP']) + self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP', 'carStateSP']) self.can_rcv_cum_timeout_counter = 0 self.CC_prev = car.CarControl.new_message() self.CS_prev = car.CarState.new_message() + self.CS_SP_prev = custom.CarStateSP.new_message() self.initialized_prev = False self.last_actuators_output = structs.CarControl.Actuators() @@ -188,16 +189,17 @@ class Car: # log fingerprint in sentry sunnypilot_interfaces.log_fingerprint(self.CP) - def state_update(self) -> tuple[car.CarState, structs.RadarDataT | None]: + def state_update(self) -> tuple[car.CarState, custom.CarStateSP, structs.RadarDataT | None]: """carState update loop, driven by can""" can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) can_list = can_capnp_to_list(can_strs) # Update carState from CAN - CS = self.CI.update(can_list) + CS, CS_SP = self.CI.update(can_list) + CS_SP = convert_to_capnp(CS_SP) if self.CP.brand == 'mock': - CS = self.mock_carstate.update(CS) + CS, CS_SP = self.mock_carstate.update(CS, CS_SP) # Update radar tracks from CAN RD: structs.RadarDataT | None = self.RI.update(can_list) @@ -222,9 +224,9 @@ class Car: CS.vCruise = float(self.v_cruise_helper.v_cruise_kph) CS.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph) - return CS, RD + return CS, CS_SP, RD - def state_publish(self, CS: car.CarState, RD: structs.RadarDataT | None): + def state_publish(self, CS: car.CarState, CS_SP: custom.CarStateSP, RD: structs.RadarDataT | None): """carState and carParams publish loop""" # carParams - logged every 50 seconds (> 1 per segment) @@ -261,6 +263,11 @@ class Car: cp_sp_send.carParamsSP = self.CP_SP_capnp self.pm.send('carParamsSP', cp_sp_send) + cs_sp_send = messaging.new_message('carStateSP') + cs_sp_send.valid = CS.canValid + cs_sp_send.carStateSP = CS_SP + self.pm.send('carStateSP', cs_sp_send) + def controls_update(self, CS: car.CarState, CC: car.CarControl, CC_SP: custom.CarControlSP): """control update loop, driven by carControl""" @@ -280,9 +287,9 @@ class Car: self.CC_prev = CC def step(self): - CS, RD = self.state_update() + CS, CS_SP, RD = self.state_update() - self.state_publish(CS, RD) + self.state_publish(CS, CS_SP, RD) initialized = (not any(e.name == EventName.selfdriveInitializing for e in self.sm['onroadEvents']) and self.sm.seen['onroadEvents']) @@ -291,6 +298,7 @@ class Car: self.initialized_prev = initialized self.CS_prev = CS + self.CS_SP_prev = CS_SP def params_thread(self, evt): while not evt.is_set(): diff --git a/selfdrive/car/helpers.py b/selfdrive/car/helpers.py index 6ebacfc97..6187be147 100644 --- a/selfdrive/car/helpers.py +++ b/selfdrive/car/helpers.py @@ -35,11 +35,13 @@ def asdictref(obj) -> dict[str, Any]: return _asdictref_inner(obj) -def convert_to_capnp(struct: structs.CarParamsSP) -> capnp.lib.capnp._DynamicStructBuilder: +def convert_to_capnp(struct: structs.CarParamsSP | structs.CarStateSP) -> capnp.lib.capnp._DynamicStructBuilder: struct_dict = asdictref(struct) if isinstance(struct, structs.CarParamsSP): struct_capnp = custom.CarParamsSP.new_message(**struct_dict) + elif isinstance(struct, structs.CarStateSP): + struct_capnp = custom.CarStateSP.new_message(**struct_dict) else: raise ValueError(f"Unsupported struct type: {type(struct)}") diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index a880e06fc..0fc6716dd 100644 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -204,7 +204,7 @@ class TestCarModelBase(unittest.TestCase): CC_SP = structs.CarControlSP() for i, msg in enumerate(self.can_msgs): - CS = self.CI.update(msg) + CS, _ = self.CI.update(msg) self.CI.apply(CC, CC_SP, msg[0]) if CS.canValid: @@ -349,7 +349,7 @@ class TestCarModelBase(unittest.TestCase): self.safety.safety_rx_hook(to_send) can = [(int(time.monotonic() * 1e9), [CanData(address=address, dat=dat, src=bus)])] - CS = self.CI.update(can) + CS, _ = self.CI.update(can) if self.safety.get_gas_pressed_prev() != prev_panda_gas: self.assertEqual(CS.gasPressed, self.safety.get_gas_pressed_prev()) @@ -409,7 +409,8 @@ class TestCarModelBase(unittest.TestCase): checks = defaultdict(int) vehicle_speed_seen = self.CP.steerControlType == SteerControlType.angle and not self.CP.notCar for idx, can in enumerate(self.can_msgs): - CS = self.CI.update(can).as_reader() + CS, _ = self.CI.update(can) + CS = CS.as_reader() for msg in filter(lambda m: m.src < 64, can[1]): to_send = libsafety_py.make_CANPacket(msg.address, msg.src % 4, msg.dat) ret = self.safety.safety_rx_hook(to_send)