diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 8ae77875f2..cc63ee7e08 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -223,7 +223,8 @@ struct BackupManagerSP @0xf98d843bfd7004a3 { } } -struct CustomReserved7 @0xb86e6369214c01c8 { +struct CarStateSP @0xb86e6369214c01c8 { + speedLimit @0 :Float32; # m/s } struct CustomReserved8 @0xf416ec09499d9d19 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 79f11f6bcf..412660bc6a 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 c40538eec0..4097e484f6 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 bb37b56789..9b97ee44d6 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit bb37b56789abd13b6cdd9ff251df9c644a050223 +Subproject commit 9b97ee44d64b0213e79d95dd28258748dd1febee diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py index d441c648ff..151a8479a7 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 62dc818cd9..1b9a1522a1 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 6ebacfc976..6187be1474 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)}")