diff --git a/opendbc_repo b/opendbc_repo index 8859db75d..0614ef831 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 8859db75de3b380f32eccf98c3082b946bf5b51f +Subproject commit 0614ef831420f11357aa28fb4d76d5278a9d11a2 diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index c3b60c2fb..d2fc9dcd8 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -468,19 +468,58 @@ struct CustomReserved12 @0x9ccdc8676701b412 { struct CustomReserved13 @0xcd96dafb67a082d0 { } -struct CustomReserved14 @0xb057204d7deadf3f { +struct ControlsStateIC @0xb057204d7deadf3f { + modelDesiredCurvature @0 :Float32; } -struct CustomReserved15 @0xbd443b539493bc68 { +struct LongitudinalPlanIC @0xbd443b539493bc68 { + leadDistance @0 :Float32; + vTarget @1 :Float32; } -struct CustomReserved16 @0xfc6241ed8877b611 { +struct CarStateIC @0xfc6241ed8877b611 { + steeringCurvature @0 :Float32; + steeringSlightlyPressed @1 :Bool; + steerFaultWarning @2 :Bool; + radarDisableFailed @3 :Bool; + batteryDetails @4 :BatteryDetails; + cruiseSpeedLimit @5 :Float32; + cruiseSpeedLimitPredicative @6 :Float32; + + struct BatteryDetails { + capacity @0 :Float32; + charge @1 :Float32; + soc @2 :Float32; + temperature @3 :Float32; + heaterActive @4 :Bool; + voltage @5 :Float32; + current @6 :Float32; + power @7 :Float32; + } } -struct CustomReserved17 @0xa30662f84033036c { +struct CarControlIC @0xa30662f84033036c { + curvatureControllerActive @0 :Bool; + rollCompensation @1 :Float32; + steerLimited @2 :Bool; + forceRHDForBSM @3 :Bool; + longComfortMode @4 :Bool; + disableCarSteerAlerts @5 :Bool; + cruiseSpeedLimit @6 :Bool; + cruiseSpeedLimitPredicative @7 :Bool; + cruiseSpeedLimitPredReactToSL @8 :Bool; + cruiseSpeedLimitPredReactToCurves @9 :Bool; + hudLeadFollowTime @10 :Float32; + hudLeadDistance @11 :Float32; } -struct CustomReserved18 @0xc86a3d38d13eb3ef { +struct CarParamsIC @0xc86a3d38d13eb3ef { + dashcamOnlyReason @0 :DashcamOnlyReason; + + enum DashcamOnlyReason { + unknown @0; + radarDisableEngineOn @1; + } } struct LiveCurvatureParameters @0xa4f1eb3323f5f582 { diff --git a/openpilot/cereal/log.capnp b/openpilot/cereal/log.capnp index f2c8bc932..61b806b35 100644 --- a/openpilot/cereal/log.capnp +++ b/openpilot/cereal/log.capnp @@ -852,7 +852,6 @@ struct ControlsState @0x97ff69c53601abf1 { uiAccelCmd @5 :Float32; ufAccelCmd @33 :Float32; curvature @37 :Float32; # path curvature from vehicle model - modelDesiredCurvature @67 :Float32; # raw desired curvature from modelV2 before smoothing/adaptation desiredCurvature @61 :Float32; # lag adjusted curvatures used by lateral controllers forceDecel @51 :Bool; @@ -866,7 +865,7 @@ struct ControlsState @0x97ff69c53601abf1 { lqrStateDEPRECATED @55 :Deprecated.LateralLQRState; indiStateDEPRECATED @52 :Deprecated.LateralINDIState; } - + struct LateralPIDState { active @0 :Bool; steeringAngleDeg @1 :Float32; @@ -1201,7 +1200,6 @@ struct LateralManeuverPlan { struct LongitudinalPlan @0xe00b5b3eba12876c { modelMonoTime @9 :UInt64; hasLead @7 :Bool; - leadDistance @40 :Float32; fcw @8 :Bool; longitudinalPlanSource @15 :LongitudinalPlanSource; processingDelay @29 :Float32; @@ -1211,7 +1209,6 @@ struct LongitudinalPlan @0xe00b5b3eba12876c { speeds @33 :List(Float32); jerks @34 :List(Float32); aTarget @18 :Float32; - vTarget @41 :Float32; shouldStop @37: Bool; allowThrottle @38: Bool; allowBrake @39: Bool; @@ -2628,11 +2625,11 @@ struct Event { customReserved11 @137 :Custom.CustomReserved11; customReserved12 @138 :Custom.CustomReserved12; customReserved13 @139 :Custom.CustomReserved13; - customReserved14 @140 :Custom.CustomReserved14; - customReserved15 @141 :Custom.CustomReserved15; - customReserved16 @142 :Custom.CustomReserved16; - customReserved17 @143 :Custom.CustomReserved17; - customReserved18 @144 :Custom.CustomReserved18; + controlsStateIC @140 :Custom.ControlsStateIC; + longitudinalPlanIC @141 :Custom.LongitudinalPlanIC; + carStateIC @142 :Custom.CarStateIC; + carControlIC @143 :Custom.CarControlIC; + carParamsIC @144 :Custom.CarParamsIC; liveCurvatureParameters @145 :Custom.LiveCurvatureParameters; # *********** legacy + deprecated *********** diff --git a/openpilot/cereal/services.py b/openpilot/cereal/services.py index 410f10050..360456494 100755 --- a/openpilot/cereal/services.py +++ b/openpilot/cereal/services.py @@ -97,6 +97,11 @@ _services: dict[str, tuple] = { # infiniteCable "liveCurvatureParameters": (True, 4., 1), + "carStateIC": (True, 100., 10), + "carControlIC": (True, 100., 10), + "carParamsIC": (True, 0.02, 1), + "controlsStateIC": (True, 100., 10), + "longitudinalPlanIC": (True, 20., 10), # debug "uiDebug": (True, 0., 1), diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index b6e42b739..b16a98090 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -165,6 +165,9 @@ inline static std::unordered_map keys = { {"CarParamsSP", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BYTES}}, {"CarParamsSPCache", {CLEAR_ON_MANAGER_START, BYTES}}, {"CarParamsSPPersistent", {PERSISTENT, BYTES}}, + {"CarParamsIC", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BYTES}}, + {"CarParamsICCache", {CLEAR_ON_MANAGER_START, BYTES}}, + {"CarParamsICPersistent", {PERSISTENT, BYTES}}, {"CarPlatformBundle", {PERSISTENT | BACKUP, JSON}}, {"ChevronInfo", {PERSISTENT | BACKUP, INT, "4"}}, {"CompletedSunnylinkConsentVersion", {PERSISTENT, STRING, "0"}}, diff --git a/openpilot/selfdrive/car/car_specific.py b/openpilot/selfdrive/car/car_specific.py index 87bb3dbda..7abbc7468 100644 --- a/openpilot/selfdrive/car/car_specific.py +++ b/openpilot/selfdrive/car/car_specific.py @@ -22,11 +22,11 @@ class CarSpecificEvents: self.no_steer_warning = False self.silent_steer_warning = True - def update(self, CS: car.CarState, CS_prev: car.CarState, CC: car.CarControl): + def update(self, CS: car.CarState, CS_prev: car.CarState, CC: car.CarControl, CS_IC: structs.CarStateIC): if self.CP.brand in ('body', 'mock'): return Events() - events = self.create_common_events(CS, CS_prev) + events = self.create_common_events(CS, CS_prev, CS_IC) if self.CP.brand == 'chrysler': # Low speed steer alert hysteresis logic @@ -93,7 +93,7 @@ class CarSpecificEvents: return events - def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState): + def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, CS_IC: structs.CarStateIC): events = Events() CI = interfaces[self.CP.carFingerprint] @@ -174,7 +174,7 @@ class CarSpecificEvents: else: self.no_steer_warning = False self.silent_steer_warning = False - if CS.steerFaultWarning: + if CS_IC.steerFaultWarning: events.add(EventName.steerFaultWarning) if CS.steerFaultPermanent: events.add(EventName.steerUnavailable) diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index db9fcc17f..2275a7fb3 100644 --- a/openpilot/selfdrive/car/card.py +++ b/openpilot/selfdrive/car/card.py @@ -20,7 +20,7 @@ from opendbc.car.car_helpers import get_car, interfaces from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.selfdrive.car.cruise import VCruiseHelper -from openpilot.selfdrive.car.helpers import convert_carControlSP, convert_to_capnp +from openpilot.selfdrive.car.helpers import convert_carControlSP, convert_carControlIC, convert_to_capnp from openpilot.sunnypilot.mads.helpers import set_alternative_experience, set_car_specific_params from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces @@ -67,18 +67,23 @@ class Car: RI: RadarInterfaceBase CP: car.CarParams CP_SP: structs.CarParamsSP + CP_IC: structs.CarParamsIC CP_SP_capnp: custom.CarParamsSP + CP_IC_capnp: custom.CarParamsIC 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', 'longitudinalPlanSP']) - self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP', 'carStateSP']) + ic_sm_services = ['carControlIC'] + ic_pm_services = ['carParamsIC', 'carStateIC'] + self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents'] + ['carControlSP', 'longitudinalPlanSP'] + ic_sm_services) + self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'] + ['carParamsSP', 'carStateSP'] + ic_pm_services) 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.CS_IC_prev = custom.CarStateIC.new_message() self.initialized_prev = False self.last_actuators_output = structs.CarControl.Actuators() @@ -115,16 +120,17 @@ class Car: self.RI = interfaces[self.CI.CP.carFingerprint].RadarInterface(self.CI.CP, self.CI.CP_SP) self.CP = self.CI.CP self.CP_SP = self.CI.CP_SP + self.CP_IC = self.CI.CP_IC # continue onto next fingerprinting step in pandad self.params.put_bool("FirmwareQueryDone", True, block=True) else: - self.CI, self.CP, self.CP_SP = CI, CI.CP, CI.CP_SP + self.CI, self.CP, self.CP_SP, self.CP_IC = CI, CI.CP, CI.CP_SP, CI.CP_IC self.RI = RI # supply a pre init method to set CP by checking data on the can bus # e.g. car is in a state where the radar can not be disabled -> set dashcam mode - self.CI.pre_init(self.CP, self.CP_SP, *self.can_callbacks) + self.CI.pre_init(self.CP, self.CP_SP, self.CP_IC, *self.can_callbacks) self.CP.alternativeExperience = 0 # mads @@ -182,6 +188,13 @@ class Car: self.params.put("CarParamsSPCache", cp_sp_bytes) self.params.put("CarParamsSPPersistent", cp_sp_bytes) + # Write CarParamsIC for controls + self.CP_IC_capnp = convert_to_capnp(self.CP_IC) + cp_ic_bytes = self.CP_IC_capnp.to_bytes() + self.params.put("CarParamsIC", cp_ic_bytes, block=True) + self.params.put("CarParamsICCache", cp_ic_bytes) + self.params.put("CarParamsICPersistent", cp_ic_bytes) + self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_SP) self.is_metric = self.params.get_bool("IsMetric") @@ -193,15 +206,14 @@ class Car: # log fingerprint in sentry sunnypilot_interfaces.log_fingerprint(self.CP) - def state_update(self) -> tuple[car.CarState, custom.CarStateSP, structs.RadarDataT | None]: + def state_update(self) -> tuple[car.CarState, custom.CarStateSP, custom.CarStateIC, 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, CS_SP = self.CI.update(can_list) - CS_SP = convert_to_capnp(CS_SP) + CS, CS_SP, CS_IC = self.CI.update(can_list) # Update radar tracks from CAN RD: structs.RadarDataT | None = self.RI.update(can_list) @@ -218,7 +230,9 @@ class Car: self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime self.v_cruise_helper.update_speed_limit_assist(self.is_metric, self.sm['longitudinalPlanSP']) - self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric, self.sm['carControl'].cruiseControl.speedLimit, self.sm['carControl'].cruiseControl.speedLimitPredicative) + self.v_cruise_helper.update_v_cruise(CS, CS_IC, self.sm['carControl'].enabled, self.is_metric, + self.sm['carControlIC'].cruiseSpeedLimit, + self.sm['carControlIC'].cruiseSpeedLimitPredicative) if self.sm['carControl'].enabled and not self.CC_prev.enabled: # Use CarState w/ buttons from the step selfdrived enables on self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode, self.dynamic_experimental_control) @@ -227,9 +241,12 @@ 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, CS_SP, RD + CS_SP_capnp = convert_to_capnp(CS_SP) + CS_IC_capnp = convert_to_capnp(CS_IC) - def state_publish(self, CS: car.CarState, CS_SP: custom.CarStateSP, RD: structs.RadarDataT | None): + return CS, CS_SP_capnp, CS_IC_capnp, RD + + def state_publish(self, CS: car.CarState, CS_SP: custom.CarStateSP, CS_IC: custom.CarStateIC, RD: structs.RadarDataT | None): """carState and carParams publish loop""" # carParams - logged every 50 seconds (> 1 per segment) @@ -266,42 +283,55 @@ class Car: cp_sp_send.carParamsSP = self.CP_SP_capnp self.pm.send('carParamsSP', cp_sp_send) + # carParamsIC - logged every 50 seconds (> 1 per segment) + if self.sm.frame % int(50. / DT_CTRL) == 0: + cp_ic_send = messaging.new_message('carParamsIC') + cp_ic_send.valid = True + cp_ic_send.carParamsIC = self.CP_IC_capnp + self.pm.send('carParamsIC', cp_ic_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): + cs_ic_send = messaging.new_message('carStateIC') + cs_ic_send.valid = CS.canValid + cs_ic_send.carStateIC = CS_IC + self.pm.send('carStateIC', cs_ic_send) + + def controls_update(self, CS: car.CarState, CC: car.CarControl, CC_SP: custom.CarControlSP, CC_IC: custom.CarControlIC): """control update loop, driven by carControl""" if not self.initialized_prev: # Initialize CarInterface, once controls are ready # TODO: this can make us miss at least a few cycles when doing an ECU knockout - self.CI.init(self.CP, self.CP_SP, *self.can_callbacks) + self.CI.init(self.CP, self.CP_SP, self.CP_IC, *self.can_callbacks) # signal pandad to switch to car safety mode self.params.put_bool("ControlsReady", True) if self.sm.all_alive(['carControl']): # send car controls over can now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) - self.last_actuators_output, can_sends = self.CI.apply(CC, convert_carControlSP(CC_SP), now_nanos) + self.last_actuators_output, can_sends = self.CI.apply(CC, convert_carControlSP(CC_SP), convert_carControlIC(CC_IC), now_nanos) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) self.CC_prev = CC def step(self): - CS, CS_SP, RD = self.state_update() + CS, CS_SP, CS_IC, RD = self.state_update() - self.state_publish(CS, CS_SP, RD) + self.state_publish(CS, CS_SP, CS_IC, RD) initialized = (not any(e.name == EventName.selfdriveInitializing for e in self.sm['onroadEvents']) and self.sm.seen['onroadEvents']) if not self.CP.passive and initialized: - self.controls_update(CS, self.sm['carControl'], self.sm['carControlSP']) + self.controls_update(CS, self.sm['carControl'], self.sm['carControlSP'], self.sm['carControlIC']) self.initialized_prev = initialized self.CS_prev = CS self.CS_SP_prev = CS_SP + self.CS_IC_prev = CS_IC def params_thread(self, evt): while not evt.is_set(): diff --git a/openpilot/selfdrive/car/cruise.py b/openpilot/selfdrive/car/cruise.py index 838169939..9ca8ed9d7 100644 --- a/openpilot/selfdrive/car/cruise.py +++ b/openpilot/selfdrive/car/cruise.py @@ -1,7 +1,7 @@ import math import numpy as np -from opendbc.car.structs import car +from opendbc.car.structs import car, CarStateIC from openpilot.common.constants import CV from openpilot.sunnypilot.selfdrive.car.cruise_ext import VCruiseHelperSP @@ -44,7 +44,7 @@ class VCruiseHelper(VCruiseHelperSP): def v_cruise_initialized(self): return self.v_cruise_kph != V_CRUISE_UNSET - def update_v_cruise(self, CS, enabled, is_metric, speed_limit_control=False, speed_limit_predicative=False): + def update_v_cruise(self, CS, CS_IC: CarStateIC, enabled, is_metric, speed_limit_control=False, speed_limit_predicative=False): self.v_cruise_kph_last = self.v_cruise_kph self.get_minimum_set_speed(is_metric) @@ -54,7 +54,7 @@ class VCruiseHelper(VCruiseHelperSP): if CS.cruiseState.available: if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled): # if stock cruise is completely disabled, then we can use our own set speed logic - self._update_v_speed_limit(CS, _enabled, speed_limit_control, speed_limit_predicative) + self._update_v_speed_limit(CS, CS_IC, _enabled, speed_limit_control, speed_limit_predicative) self._update_v_cruise_non_pcm(CS, _enabled, is_metric) self.update_speed_limit_assist_v_cruise_non_pcm() self.v_cruise_cluster_kph = self.v_cruise_kph @@ -74,12 +74,12 @@ class VCruiseHelper(VCruiseHelperSP): if not self.CP.pcmCruise or not self.CP_SP.pcmCruiseSpeed: self.update_button_timers(CS, enabled) - def _update_v_speed_limit(self, CS, enabled, speed_limit_control, predicative): + def _update_v_speed_limit(self, CS, CS_IC: CarStateIC, enabled, speed_limit_control, predicative): if not speed_limit_control: # or not enabled # always set speed limit return - speed_limit_current = CS.cruiseState.speedLimit * CV.MS_TO_KPH - speed_limit_predicative = CS.cruiseState.speedLimitPredicative * CV.MS_TO_KPH + speed_limit_current = CS_IC.cruiseSpeedLimit * CV.MS_TO_KPH + speed_limit_predicative = CS_IC.cruiseSpeedLimitPredicative * CV.MS_TO_KPH speed_limit = speed_limit_predicative if predicative and speed_limit_predicative != 0 else speed_limit_current diff --git a/openpilot/selfdrive/car/helpers.py b/openpilot/selfdrive/car/helpers.py index ec9e3efee..413f6c0e5 100644 --- a/openpilot/selfdrive/car/helpers.py +++ b/openpilot/selfdrive/car/helpers.py @@ -3,6 +3,7 @@ from typing import Any from openpilot.cereal import custom from opendbc.car import structs +from opendbc.car.structs import CarStateIC, CarControlIC, CarParamsIC _FIELDS = '__dataclass_fields__' # copy of dataclasses._FIELDS @@ -35,13 +36,17 @@ def asdictref(obj) -> dict[str, Any]: return _asdictref_inner(obj) -def convert_to_capnp(struct: structs.CarParamsSP | structs.CarStateSP) -> capnp.lib.capnp._DynamicStructBuilder: +def convert_to_capnp(struct: structs.CarParamsSP | structs.CarStateSP | CarParamsIC | CarStateIC) -> 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) + elif isinstance(struct, CarParamsIC): + struct_capnp = custom.CarParamsIC.new_message(**struct_dict) + elif isinstance(struct, CarStateIC): + struct_capnp = custom.CarStateIC.new_message(**struct_dict) else: raise ValueError(f"Unsupported struct type: {type(struct)}") @@ -65,3 +70,14 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct ) return struct_dataclass + + +def convert_carControlIC(struct: capnp.lib.capnp._DynamicStructReader) -> CarControlIC: + # TODO: recursively handle any car struct as needed + def remove_deprecated(s: dict) -> dict: + return {k: v for k, v in s.items() if not k.endswith('DEPRECATED')} + + struct_dict = struct.to_dict() + struct_dataclass = CarControlIC(**remove_deprecated({k: v for k, v in struct_dict.items() if not isinstance(k, dict)})) + + return struct_dataclass diff --git a/openpilot/selfdrive/car/tests/test_car_interfaces.py b/openpilot/selfdrive/car/tests/test_car_interfaces.py index 3c4587cbc..a1bbb9031 100644 --- a/openpilot/selfdrive/car/tests/test_car_interfaces.py +++ b/openpilot/selfdrive/car/tests/test_car_interfaces.py @@ -10,7 +10,7 @@ from opendbc.car.structs import CarParams from opendbc.car.tests.test_car_interfaces import get_fuzzy_car_interface from opendbc.car.mock.values import CAR as MOCK from opendbc.car.values import PLATFORMS -from openpilot.selfdrive.car.helpers import convert_carControlSP +from openpilot.selfdrive.car.helpers import convert_carControlSP, convert_carControlIC from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque @@ -37,15 +37,18 @@ class TestCarInterfaces: cc_msg = FuzzyGenerator.get_random_msg(data.draw, car.CarControl, real_floats=True) cc_sp_msg = FuzzyGenerator.get_random_msg(data.draw, custom.CarControlSP, real_floats=True) + cc_ic_msg = FuzzyGenerator.get_random_msg(data.draw, custom.CarControlIC, real_floats=True) # Run car interface now_nanos = 0 CC = car.CarControl.new_message(**cc_msg) CC = CC.as_reader() CC_SP = custom.CarControlSP.new_message(**cc_sp_msg) CC_SP = convert_carControlSP(CC_SP.as_reader()) + CC_IC = custom.CarControlIC.new_message(**cc_ic_msg) + CC_IC = convert_carControlIC(CC_IC.as_reader()) for _ in range(10): car_interface.update([]) - car_interface.apply(CC, CC_SP, now_nanos) + car_interface.apply(CC, CC_SP, CC_IC, now_nanos) now_nanos += DT_CTRL * 1e9 # 10 ms CC = car.CarControl.new_message(**cc_msg) @@ -55,7 +58,7 @@ class TestCarInterfaces: CC = CC.as_reader() for _ in range(10): car_interface.update([]) - car_interface.apply(CC, CC_SP, now_nanos) + car_interface.apply(CC, CC_SP, CC_IC, now_nanos) now_nanos += DT_CTRL * 1e9 # 10ms # Test controller initialization diff --git a/openpilot/selfdrive/car/tests/test_cruise_speed.py b/openpilot/selfdrive/car/tests/test_cruise_speed.py index c0b6ac979..84985606f 100644 --- a/openpilot/selfdrive/car/tests/test_cruise_speed.py +++ b/openpilot/selfdrive/car/tests/test_cruise_speed.py @@ -6,7 +6,7 @@ from openpilot.common.parameterized import parameterized_class from openpilot.cereal import log from openpilot.selfdrive.car.cruise import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT from openpilot.cereal import custom -from opendbc.car.structs import car +from opendbc.car.structs import car, CarStateIC from openpilot.common.constants import CV from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver @@ -51,13 +51,14 @@ class TestVCruiseHelper: def setup_method(self): self.CP = car.CarParams(pcmCruise=self.pcm_cruise) self.CP_SP = custom.CarParamsSP(pcmCruiseSpeed=self.pcm_cruise_speed) + self.CS_IC = CarStateIC() self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_SP) self.reset_cruise_speed_state() def reset_cruise_speed_state(self): # Two resets previous cruise speed for _ in range(2): - self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False) + self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), self.CS_IC, enabled=False, is_metric=False) def enable(self, v_ego, experimental_mode, dynamic_experimental_control): # Simulates user pressing set with a current speed @@ -75,7 +76,7 @@ class TestVCruiseHelper: CS = car.CarState(cruiseState={"available": True}) CS.buttonEvents = [ButtonEvent(type=btn, pressed=pressed)] - self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False) + self.v_cruise_helper.update_v_cruise(CS, self.CS_IC, enabled=True, is_metric=False) assert pressed == (self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last) def test_rising_edge_enable(self): @@ -90,7 +91,7 @@ class TestVCruiseHelper: (True, False)): CS = car.CarState(cruiseState={"available": True}) CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)] - self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False) + self.v_cruise_helper.update_v_cruise(CS, self.CS_IC, enabled=enabled, is_metric=False) if pressed: self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False) @@ -108,7 +109,7 @@ class TestVCruiseHelper: for pressed in (True, False): CS = car.CarState(cruiseState={"available": True, "standstill": standstill}) CS.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=pressed)] - self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False) + self.v_cruise_helper.update_v_cruise(CS, self.CS_IC, enabled=True, is_metric=False) # speed should only update if not at standstill and button falling edge should_equal = standstill or pressed @@ -131,7 +132,7 @@ class TestVCruiseHelper: CS = car.CarState(vEgo=float(v_ego), gasPressed=True, cruiseState={"available": True}) CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)] - self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False) + self.v_cruise_helper.update_v_cruise(CS, self.CS_IC, enabled=True, is_metric=False) # TODO: fix skipping first run due to enabled on rising edge exception if v_ego == 0.0: diff --git a/openpilot/selfdrive/car/tests/test_models.py b/openpilot/selfdrive/car/tests/test_models.py index adf050912..62112c999 100644 --- a/openpilot/selfdrive/car/tests/test_models.py +++ b/openpilot/selfdrive/car/tests/test_models.py @@ -159,8 +159,10 @@ class TestCarModelBase(unittest.TestCase): cls.CarInterface = interfaces[cls.platform] cls.CP = cls.CarInterface.get_params(cls.platform, cls.fingerprint, car_fw, alpha_long, False, docs=False) cls.CP_SP = cls.CarInterface.get_params_sp(cls.CP, cls.platform, cls.fingerprint, car_fw, alpha_long, False, docs=False) + cls.CP_IC = cls.CarInterface.get_params_ic(cls.CP, cls.platform, cls.fingerprint, car_fw, alpha_long, False, docs=False) assert cls.CP assert cls.CP_SP + assert cls.CP_IC assert cls.CP.carFingerprint == cls.platform os.environ["COMMA_CACHE"] = DEFAULT_DOWNLOAD_CACHE_ROOT @@ -170,7 +172,7 @@ class TestCarModelBase(unittest.TestCase): del cls.can_msgs def setUp(self): - self.CI = self.CarInterface(self.CP.copy(), copy.deepcopy(self.CP_SP)) + self.CI = self.CarInterface(self.CP.copy(), copy.deepcopy(self.CP_SP), copy.deepcopy(self.CP_IC)) assert self.CI # TODO: check safetyModel is in release panda build @@ -205,10 +207,11 @@ class TestCarModelBase(unittest.TestCase): can_invalid_cnt = 0 CC = structs.CarControl().as_reader() CC_SP = structs.CarControlSP() + CC_IC = structs.CarControlIC() for i, msg in enumerate(self.can_msgs): - CS, _ = self.CI.update(msg) - self.CI.apply(CC, CC_SP, msg[0]) + CS, _, _ = self.CI.update(msg) + self.CI.apply(CC, CC_SP, CC_IC, msg[0]) # wait max of 2s for low frequency msgs to be seen if i > 250: @@ -277,13 +280,13 @@ class TestCarModelBase(unittest.TestCase): if self.CP.notCar: self.skipTest("Skipping test for notCar") - def test_car_controller(car_control, car_control_sp): + def test_car_controller(car_control, car_control_sp, car_control_ic): now_nanos = 0 msgs_sent = 0 - CI = self.CarInterface(self.CP, self.CP_SP) + CI = self.CarInterface(self.CP, self.CP_SP, self.CP_IC) for _ in range(round(10.0 / DT_CTRL)): # make sure we hit the slowest messages CI.update([]) - _, sendcan = CI.apply(car_control, car_control_sp, now_nanos) + _, sendcan = CI.apply(car_control, car_control_sp, car_control_ic, now_nanos) now_nanos += DT_CTRL * 1e9 msgs_sent += len(sendcan) @@ -297,17 +300,18 @@ class TestCarModelBase(unittest.TestCase): # Make sure we can send all messages while inactive CC = structs.CarControl() CC_SP = structs.CarControlSP() - test_car_controller(CC.as_reader(), CC_SP) + CC_IC = structs.CarControlIC() + test_car_controller(CC.as_reader(), CC_SP, CC_IC) # Test cancel + general messages (controls_allowed=False & cruise_engaged=True) self.safety.set_cruise_engaged_prev(True) CC = structs.CarControl(cruiseControl=structs.CarControl.CruiseControl(cancel=True)) - test_car_controller(CC.as_reader(), CC_SP) + test_car_controller(CC.as_reader(), CC_SP, CC_IC) # Test resume + general messages (controls_allowed=True & cruise_engaged=True) self.safety.set_controls_allowed(True) CC = structs.CarControl(cruiseControl=structs.CarControl.CruiseControl(resume=True)) - test_car_controller(CC.as_reader(), CC_SP) + test_car_controller(CC.as_reader(), CC_SP, CC_IC) # Skip stdout/stderr capture with pytest, causes elevated memory usage @pytest.mark.nocapture diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index ce79efcdc..62755531e 100644 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -2,7 +2,7 @@ import math from numbers import Number -from openpilot.cereal import log +from openpilot.cereal import log, custom from opendbc.car.structs import car import openpilot.cereal.messaging as messaging from openpilot.common.constants import CV @@ -47,13 +47,19 @@ class Controls(ControlsExt): # Initialize sunnypilot controlsd extension and base model state ControlsExt.__init__(self, self.CP, self.params) - self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP) + cloudlog.info("controlsd is waiting for CarParamsIC") + self.CP_IC = messaging.log_from_bytes(self.params.get("CarParamsIC", block=True), custom.CarParamsIC) + cloudlog.info("controlsd got CarParamsIC") - self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'liveCurvatureParameters', 'modelV2', 'selfdriveState', + self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP, self.CP_IC) + + ic_sm_services = ['liveCurvatureParameters', 'longitudinalPlanIC'] + ic_pm_services = ['carControlIC', 'controlsStateIC'] + self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', 'liveCalibration', 'livePose', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput', - 'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + self.sm_services_ext, + 'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + ic_sm_services + self.sm_services_ext, poll='selfdriveState') - self.pm = messaging.PubMaster(['carControl', 'controlsState'] + self.pm_services_ext) + self.pm = messaging.PubMaster(['carControl', 'controlsState'] + ic_pm_services + self.pm_services_ext) self.steer_limited_by_safety = False self.curvature = 0.0 @@ -181,7 +187,7 @@ class Controls(ControlsExt): # accel PID loop pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, self.CP_SP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS) actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)) - actuators.speed = float(long_plan.vTarget) + actuators.speed = float(self.sm['longitudinalPlanIC'].vTarget) # Steering PID loop and lateral MPC # Reset desired curvature to current to avoid violating the limits on engage @@ -222,17 +228,22 @@ class Controls(ControlsExt): def publish(self, CC, lac_log): CS = self.sm['carState'] - CC.curvatureControllerActive = self.enable_curvature_controller # for car controller curvature correction activation - CC.steerLimited = self.steer_limited_by_safety - CC.forceRHDForBSM = self.force_rhd_for_bsm - CC.longComfortMode = self.enable_long_comfort_mode - CC.disableCarSteerAlerts = self.disable_car_steer_alerts + CC_IC = custom.CarControlIC.new_message() + CC_IC.curvatureControllerActive = self.enable_curvature_controller + CC_IC.steerLimited = self.steer_limited_by_safety + CC_IC.forceRHDForBSM = self.force_rhd_for_bsm + CC_IC.longComfortMode = self.enable_long_comfort_mode + CC_IC.disableCarSteerAlerts = self.disable_car_steer_alerts + CC_IC.cruiseSpeedLimit = self.enable_speed_limit_control + CC_IC.cruiseSpeedLimitPredicative = self.enable_speed_limit_predicative + CC_IC.cruiseSpeedLimitPredReactToSL = self.enable_pred_react_to_speed_limits + CC_IC.cruiseSpeedLimitPredReactToCurves = self.enable_pred_react_to_curves # Orientation and angle rates can be useful for carcontroller # Only calibrated (car) frame is relevant for the carcontroller CC.currentCurvature = self.curvature - CC.rollCompensation = self.roll_compensation - + CC_IC.rollCompensation = self.roll_compensation + if self.calibrated_pose is not None: CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist() CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist() @@ -240,19 +251,15 @@ class Controls(ControlsExt): CC.cruiseControl.override = CC.enabled and not CC.longActive and (self.CP.openpilotLongitudinalControl or not self.CP_SP.pcmCruiseSpeed) CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise) CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and not self.sm['longitudinalPlan'].shouldStop - CC.cruiseControl.speedLimit = self.enable_speed_limit_control - CC.cruiseControl.speedLimitPredicative = self.enable_speed_limit_predicative - CC.cruiseControl.speedLimitPredReactToSL = self.enable_pred_react_to_speed_limits - CC.cruiseControl.speedLimitPredReactToCurves = self.enable_pred_react_to_curves hudControl = CC.hudControl hudControl.setSpeed = float(CS.vCruiseCluster * CV.KPH_TO_MS) hudControl.speedVisible = CC.enabled hudControl.lanesVisible = CC.enabled hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead - hudControl.leadDistance = self.sm['longitudinalPlan'].leadDistance + CC_IC.hudLeadDistance = self.sm['longitudinalPlanIC'].leadDistance hudControl.leadDistanceBars = self.sm['selfdriveState'].personality.raw + 1 - hudControl.leadFollowTime = get_T_FOLLOW(hudControl.leadDistanceBars - 1) + CC_IC.hudLeadFollowTime = get_T_FOLLOW(hudControl.leadDistanceBars - 1) hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual hudControl.rightLaneVisible = True @@ -280,7 +287,6 @@ class Controls(ControlsExt): cs.curvature = self.curvature cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2'] - cs.modelDesiredCurvature = self.model_desired_curvature cs.desiredCurvature = self.desired_curvature cs.longControlState = self.LoC.long_control_state cs.upAccelCmd = float(self.LoC.pid.p) @@ -304,6 +310,18 @@ class Controls(ControlsExt): self.pm.send('controlsState', dat) + # carControlIC + cc_ic_send = messaging.new_message('carControlIC') + cc_ic_send.valid = CS.canValid + cc_ic_send.carControlIC = CC_IC + self.pm.send('carControlIC', cc_ic_send) + + # controlsStateIC + cs_ic_send = messaging.new_message('controlsStateIC') + cs_ic_send.valid = CS.canValid + cs_ic_send.controlsStateIC.modelDesiredCurvature = self.model_desired_curvature + self.pm.send('controlsStateIC', cs_ic_send) + # carControl cc_send = messaging.new_message('carControl') cc_send.valid = CS.canValid diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index b75d6458d..d715ee682 100644 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -182,16 +182,21 @@ class LongitudinalPlanner(LongitudinalPlannerSP): longitudinalPlan.jerks = self.j_desired_trajectory.tolist() longitudinalPlan.hasLead = sm['radarState'].leadOne.present - longitudinalPlan.leadDistance = get_lead_distance(sm['radarState']) longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw longitudinalPlan.aTarget = float(self.output_a_target) longitudinalPlan.shouldStop = bool(self.output_should_stop) - longitudinalPlan.vTarget = float(self.output_v_target) longitudinalPlan.allowBrake = True longitudinalPlan.allowThrottle = bool(self.allow_throttle) pm.send('longitudinalPlan', plan_send) + # infiniteCable longitudinal plan extension + plan_ic_send = messaging.new_message('longitudinalPlanIC') + plan_ic_send.valid = sm.all_checks() + plan_ic_send.longitudinalPlanIC.leadDistance = get_lead_distance(sm['radarState']) + plan_ic_send.longitudinalPlanIC.vTarget = float(self.output_v_target) + pm.send('longitudinalPlanIC', plan_ic_send) + self.publish_longitudinal_plan_sp(sm, pm) diff --git a/openpilot/selfdrive/controls/plannerd.py b/openpilot/selfdrive/controls/plannerd.py index 5908e141f..657bd1dd9 100755 --- a/openpilot/selfdrive/controls/plannerd.py +++ b/openpilot/selfdrive/controls/plannerd.py @@ -27,7 +27,7 @@ def main(): ldw = LaneDepartureWarning() longitudinal_planner = LongitudinalPlanner(CP, CP_SP) - pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP']) + pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP', 'longitudinalPlanIC']) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState', 'liveMapDataSP', 'carStateSP', gps_location_service], poll='carState', ignore_alive=ignore_services, ignore_avg_freq=ignore_services, ignore_valid=ignore_services) diff --git a/openpilot/selfdrive/controls/tests/test_latcontrol.py b/openpilot/selfdrive/controls/tests/test_latcontrol.py index 52e6912d6..89ef425eb 100644 --- a/openpilot/selfdrive/controls/tests/test_latcontrol.py +++ b/openpilot/selfdrive/controls/tests/test_latcontrol.py @@ -26,7 +26,8 @@ class TestLatControl: CarInterface = interfaces[car_name] CP = CarInterface.get_non_essential_params(car_name) CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name) - CI = CarInterface(CP, CP_SP) + CP_IC = CarInterface.get_non_essential_params_ic(CP, car_name) + CI = CarInterface(CP, CP_SP, CP_IC) sunnypilot_interfaces.setup_interfaces(CI) CP_SP = convert_to_capnp(CP_SP) VM = VehicleModel(CP) diff --git a/openpilot/selfdrive/controls/tests/test_latcontrol_torque_buffer.py b/openpilot/selfdrive/controls/tests/test_latcontrol_torque_buffer.py index dc634f20f..a68527371 100644 --- a/openpilot/selfdrive/controls/tests/test_latcontrol_torque_buffer.py +++ b/openpilot/selfdrive/controls/tests/test_latcontrol_torque_buffer.py @@ -17,7 +17,8 @@ def get_controller(car_name): CarInterface = interfaces[car_name] CP = CarInterface.get_non_essential_params(car_name) CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name) - CI = CarInterface(CP, CP_SP) + CP_IC = CarInterface.get_non_essential_params_ic(CP, car_name) + CI = CarInterface(CP, CP_SP, CP_IC) sunnypilot_interfaces.setup_interfaces(CI) CP_SP = convert_to_capnp(CP_SP) VM = VehicleModel(CP) diff --git a/openpilot/selfdrive/locationd/curvatured.py b/openpilot/selfdrive/locationd/curvatured.py index 5b610f338..4d0f2b724 100644 --- a/openpilot/selfdrive/locationd/curvatured.py +++ b/openpilot/selfdrive/locationd/curvatured.py @@ -39,6 +39,8 @@ PREVIEW_REFRESH_EVERY_N_UPDATES = 20 # # Safety / scope: # - Learning is gated by valid upstream pose/calibration, low roll, low yaw uncertainty, and no steering override. +# - TODO: Track 10 s override-free data quality and use it to weight sample counts/EMA updates after the hard override gate. +# - TODO: Latch override events/durations so brief overrides cannot be missed by conflated messaging. # - Corrections are bounded by a relative cap envelope over the speed-available buckets: # - up to 50% of local curvature through the last still-supported bucket # - from there, the cap fades toward 0 at the next outer bucket center @@ -515,11 +517,14 @@ class CurvatureEstimator(CurvatureDLookup): self.car_control_t = deque(maxlen=self.hist_len) self.lat_active = deque(maxlen=self.hist_len) + self.car_control_ic_t = deque(maxlen=self.hist_len) self.roll_compensation = deque(maxlen=self.hist_len) self.car_state_t = deque(maxlen=self.hist_len) self.vego = deque(maxlen=self.hist_len) self.steering_pressed = deque(maxlen=self.hist_len) - self.controls_state_t = deque(maxlen=self.hist_len) + self.car_state_ic_t = deque(maxlen=self.hist_len) + self.steering_slightly_pressed = deque(maxlen=self.hist_len) + self.controls_state_ic_t = deque(maxlen=self.hist_len) self.model_desired_curvature = deque(maxlen=self.hist_len) self.last_lat_inactive_t = 0.0 @@ -601,16 +606,19 @@ class CurvatureEstimator(CurvatureDLookup): self.current_bias = 0.0 self.current_bucket_points = 0 if self.prev_use_params: - for d in [self.car_control_t, self.lat_active, self.roll_compensation, + for d in [self.car_control_t, self.lat_active, self.car_control_ic_t, self.roll_compensation, self.car_state_t, self.vego, self.steering_pressed, - self.controls_state_t, self.model_desired_curvature]: + self.car_state_ic_t, self.steering_slightly_pressed, + self.controls_state_ic_t, self.model_desired_curvature]: d.clear() self.last_lat_inactive_t = 0.0 self.last_override_t = 0.0 self.frame += 1 def _history_ready(self) -> bool: - return min(len(self.car_control_t), len(self.car_state_t), len(self.controls_state_t)) == self.hist_len + histories = (self.car_control_t, self.car_control_ic_t, self.car_state_t, + self.car_state_ic_t, self.controls_state_ic_t) + return min(map(len, histories)) == self.hist_len @staticmethod def _sample_at_or_before(target_t: float, ts: deque, values: deque): @@ -624,6 +632,13 @@ class CurvatureEstimator(CurvatureDLookup): return values[i] return None + def _steering_override_at(self, t: float) -> bool | None: + steering_pressed = self._sample_at_or_before(t, self.car_state_t, self.steering_pressed) + steering_slightly_pressed = self._sample_at_or_before(t, self.car_state_ic_t, self.steering_slightly_pressed) + if steering_pressed is None or steering_slightly_pressed is None: + return None + return bool(steering_pressed or steering_slightly_pressed) + def add_measurement(self, desired_curvature: float, actual_curvature: float, v_ego: float, schedule_only: bool = False) -> None: curvature_idx = self.curvature_index(desired_curvature) @@ -840,21 +855,28 @@ class CurvatureEstimator(CurvatureDLookup): if which == "carControl": self.car_control_t.append(t) self.lat_active.append(msg.latActive) - self.roll_compensation.append(msg.rollCompensation) if not msg.latActive: self.last_lat_inactive_t = t + elif which == "carControlIC": + self.car_control_ic_t.append(t) + self.roll_compensation.append(float(msg.rollCompensation)) elif which == "carState": - steering_override = bool(msg.steeringPressed or msg.steeringSlightlyPressed) self.car_state_t.append(t) self.vego.append(msg.vEgo) - self.steering_pressed.append(steering_override) - if steering_override: + self.steering_pressed.append(bool(msg.steeringPressed)) + if msg.steeringPressed: self.last_override_t = t - elif which == "controlsState": - self.controls_state_t.append(t) + elif which == "carStateIC": + self.car_state_ic_t.append(t) + self.steering_slightly_pressed.append(bool(msg.steeringSlightlyPressed)) + if msg.steeringSlightlyPressed: + self.last_override_t = t + elif which == "controlsStateIC": + self.controls_state_ic_t.append(t) self.model_desired_curvature.append(msg.modelDesiredCurvature) - if self.car_state_t: - self._update_current_lookup(self.model_desired_curvature[-1], self.vego[-1]) + v_ego = self._sample_at_or_before(t, self.car_state_t, self.vego) + if v_ego is not None: + self._update_current_lookup(self.model_desired_curvature[-1], v_ego) elif which == "liveCalibration": self.calibrator.feed_live_calib(msg) elif which == "liveDelay": @@ -870,15 +892,16 @@ class CurvatureEstimator(CurvatureDLookup): target_t = t - self.lag lat_active = self._sample_at_or_before(target_t, self.car_control_t, self.lat_active) - roll_comp = self._sample_at_or_before(target_t, self.car_control_t, self.roll_compensation) - steering_pressed = self._sample_at_or_before(target_t, self.car_state_t, self.steering_pressed) + roll_comp = self._sample_at_or_before(target_t, self.car_control_ic_t, self.roll_compensation) v_ego = self._sample_at_or_before(target_t, self.car_state_t, self.vego) - desired_curvature = self._sample_at_or_before(target_t, self.controls_state_t, self.model_desired_curvature) + desired_curvature = self._sample_at_or_before(target_t, self.controls_state_ic_t, self.model_desired_curvature) + # Driver overrides apply immediately; only command-derived signals are lag aligned. + steering_override = self._steering_override_at(t) - if any(x is None for x in (lat_active, roll_comp, steering_pressed, v_ego, desired_curvature)): + if any(x is None for x in (lat_active, roll_comp, v_ego, desired_curvature, steering_override)): return - if not bool(lat_active) or bool(steering_pressed) or float(v_ego) < self.MIN_SPEED: + if not bool(lat_active) or bool(steering_override) or float(v_ego) < self.MIN_SPEED: return device_pose = Pose.from_live_pose(msg) @@ -948,7 +971,8 @@ def main(): config_realtime_process([0, 1, 2, 3], 5) pm = messaging.PubMaster(['liveCurvatureParameters']) - sm = messaging.SubMaster(['carControl', 'carState', 'liveCalibration', 'livePose', 'liveDelay', 'controlsState'], poll='livePose') + sm = messaging.SubMaster(['carControlIC', 'carControl', 'carState', 'carStateIC', 'liveCalibration', 'livePose', + 'liveDelay', 'controlsStateIC'], poll='livePose') params = Params() CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) @@ -990,4 +1014,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/openpilot/selfdrive/locationd/test/test_curvatured.py b/openpilot/selfdrive/locationd/test/test_curvatured.py index 188c4ffe5..62145f20d 100644 --- a/openpilot/selfdrive/locationd/test/test_curvatured.py +++ b/openpilot/selfdrive/locationd/test/test_curvatured.py @@ -337,11 +337,29 @@ class TestCurvatureEstimator: estimator = get_estimator() estimator.use_params = True - estimator.handle_log(12.0, "carState", car.CarState(vEgo=20.0, steeringPressed=False, steeringSlightlyPressed=True)) + estimator.handle_log(11.0, "carState", car.CarState(vEgo=20.0, steeringPressed=False)) + estimator.handle_log(11.0, "carStateIC", custom.CarStateIC(steeringSlightlyPressed=False)) + estimator.handle_log(12.0, "carState", car.CarState(vEgo=20.0, steeringPressed=False)) + estimator.handle_log(12.0, "carStateIC", custom.CarStateIC(steeringSlightlyPressed=True)) - assert estimator.steering_pressed[-1] + assert not estimator.steering_pressed[-1] + assert estimator.steering_slightly_pressed[-1] + assert not estimator._steering_override_at(11.99) + assert estimator._steering_override_at(12.0) assert estimator.last_override_t == 12.0 + def test_ic_values_keep_their_own_timestamps(self): + estimator = get_estimator() + estimator.use_params = True + + estimator.handle_log(10.0, "carControl", car.CarControl(latActive=True)) + estimator.handle_log(10.01, "carControlIC", custom.CarControlIC(rollCompensation=4.0e-4)) + + assert estimator.car_control_t[-1] == 10.0 + assert estimator.car_control_ic_t[-1] == 10.01 + assert estimator._sample_at_or_before(10.0, estimator.car_control_ic_t, estimator.roll_compensation) is None + assert np.isclose(estimator._sample_at_or_before(10.01, estimator.car_control_ic_t, estimator.roll_compensation), 4.0e-4) + def test_interp_curve_value_matches_interp_curve_samples(self): """Verifies the unified interp_curve_value API returns the same result for scalar and array input, and that both branches share the same code path. diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py index 65130100a..aff7d5e2f 100644 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/openpilot/selfdrive/selfdrived/selfdrived.py @@ -46,7 +46,7 @@ LaneChangeDirection = log.LaneChangeDirection EventName = log.OnroadEvent.EventName ButtonType = car.CarState.ButtonEvent.Type SafetyModel = car.CarParams.SafetyModel -DashcamOnlyReason = car.CarParams.DashcamOnlyReason + AlertLevel = log.DriverMonitoringState.AlertLevel MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy TurnDirection = custom.ModelDataV2SP.TurnDirection @@ -55,7 +55,7 @@ IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) class SelfdriveD(CruiseHelper): - def __init__(self, CP=None, CP_SP=None): + def __init__(self, CP=None, CP_SP=None, CP_IC=None): self.params = Params() # Ensure the current branch is cached, otherwise the first cycle lags @@ -75,6 +75,13 @@ class SelfdriveD(CruiseHelper): else: self.CP_SP = CP_SP + if CP_IC is None: + cloudlog.info("selfdrived is waiting for CarParamsIC") + self.CP_IC = messaging.log_from_bytes(self.params.get("CarParamsIC", block=True), custom.CarParamsIC) + cloudlog.info("selfdrived got CarParamsIC") + else: + self.CP_IC = CP_IC + self.car_events = CarSpecificEvents(self.CP) self.pose_calibrator = PoseCalibrator() @@ -105,7 +112,7 @@ class SelfdriveD(CruiseHelper): 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'liveCurvatureParameters', 'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback', - 'lateralManeuverPlan', 'modelDataV2SP', 'longitudinalPlanSP'] + \ + 'lateralManeuverPlan', 'modelDataV2SP', 'longitudinalPlanSP', 'carStateIC'] + \ self.camera_packets + self.sensor_packets + self.gps_packets, ignore_alive=ignore, ignore_avg_freq=ignore, ignore_valid=ignore, frequency=int(1/DT_CTRL)) @@ -169,7 +176,8 @@ class SelfdriveD(CruiseHelper): self.events.add(EventName.carUnrecognized, static=True) set_offroad_alert("Offroad_CarUnrecognized", True) elif self.CP.passive: - if self.CP.dashcamOnlyReason == DashcamOnlyReason.radarDisableEngineOn: + dashcam_reason = self.CP_IC.dashcamOnlyReason + if dashcam_reason == custom.CarParamsIC.DashcamOnlyReason.radarDisableEngineOn: self.events.add(EventName.dashcamModeRadDisEngOn, static=True) else: self.events.add(EventName.dashcamMode, static=True) @@ -255,7 +263,7 @@ class SelfdriveD(CruiseHelper): # Add car events, ignore if CAN isn't valid if CS.canValid: - car_events = self.car_events.update(CS, self.CS_prev, self.sm['carControl']).to_msg() + car_events = self.car_events.update(CS, self.CS_prev, self.sm['carControl'], self.sm['carStateIC']).to_msg() self.events.add_from_msg(car_events) car_events_sp = self.car_events_sp.update(CS, self.events).to_msg() @@ -273,7 +281,8 @@ class SelfdriveD(CruiseHelper): (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)): self.events.add(EventName.pedalPressed) - if CS.radarDisableFailed: + cs_ic = self.sm['carStateIC'] + if cs_ic.radarDisableFailed: self.events.add(EventName.radarDisableFailed) # Create events for temperature, disk space, and memory diff --git a/openpilot/selfdrive/test/process_replay/process_replay.py b/openpilot/selfdrive/test/process_replay/process_replay.py index 409c18e4a..0016fce6c 100755 --- a/openpilot/selfdrive/test/process_replay/process_replay.py +++ b/openpilot/selfdrive/test/process_replay/process_replay.py @@ -357,6 +357,7 @@ def get_car_params_callback(rc, pm, msgs, fingerprint): CarInterface = interfaces[fingerprint] CP = CarInterface.get_non_essential_params(fingerprint) CP_SP = CarInterface.get_non_essential_params_sp(CP, fingerprint) + CP_IC = CarInterface.get_non_essential_params_ic(CP, fingerprint) else: can_msgs = ([CanData(can.address, can.dat, can.src) for can in m.can] for m in msgs if m.which() == "can") cached_params_raw = params.get("CarParamsCache") @@ -373,10 +374,11 @@ def get_car_params_callback(rc, pm, msgs, fingerprint): cached_params = _cached_params _CI = get_car(can_recv, lambda _msgs: None, lambda obd: None, params.get_bool("AlphaLongitudinalEnabled"), False, cached_params=cached_params) - CP, CP_SP = _CI.CP, _CI.CP_SP + CP, CP_SP, CP_IC = _CI.CP, _CI.CP_SP, _CI.CP_IC params.put("CarParams", CP.to_bytes(), block=True) params.put("CarParamsSP", convert_to_capnp(CP_SP).to_bytes(), block=True) + params.put("CarParamsIC", convert_to_capnp(CP_IC).to_bytes(), block=True) def card_rcv_callback(msg, cfg, frame): @@ -442,7 +444,7 @@ CONFIGS = [ "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState", "liveTorqueParameters", "liveCurvatureParameters", "accelerometer", "gyroscope", "carOutput", "gpsLocationExternal", "gpsLocation", "controlsState", - "carControl", "driverAssistance", "alertDebug", "audioFeedback", + "carControl", "carStateIC", "driverAssistance", "alertDebug", "audioFeedback", ], subs=["selfdriveState", "onroadEvents"], ignore=["logMonoTime"], @@ -455,9 +457,9 @@ CONFIGS = [ ProcessConfig( proc_name="controlsd", pubs=["liveParameters", "liveTorqueParameters", "liveCurvatureParameters", "modelV2", "selfdriveState", - "liveCalibration", "livePose", "longitudinalPlan", "carState", "carOutput", + "liveCalibration", "livePose", "longitudinalPlan", "longitudinalPlanIC", "carState", "carOutput", "driverMonitoringState", "onroadEvents", "driverAssistance"], - subs=["carControl", "controlsState"], + subs=["carControl", "carControlIC", "controlsState", "controlsStateIC"], ignore=["logMonoTime", ], init_callback=get_car_params_callback, should_recv_callback=MessageBasedRcvCallback("selfdriveState"), @@ -465,8 +467,8 @@ CONFIGS = [ ), ProcessConfig( proc_name="card", - pubs=["pandaStates", "carControl", "onroadEvents", "can"], - subs=["sendcan", "carState", "carParams", "carOutput", "liveTracks"], + pubs=["pandaStates", "carControl", "carControlIC", "onroadEvents", "can"], + subs=["sendcan", "carState", "carStateIC", "carParams", "carParamsIC", "carOutput", "liveTracks"], ignore=["logMonoTime", "carState.cumLagMs"], init_callback=card_fingerprint_callback, should_recv_callback=card_rcv_callback, @@ -486,7 +488,7 @@ CONFIGS = [ ProcessConfig( proc_name="plannerd", pubs=["modelV2", "carControl", "carState", "controlsState", "liveParameters", "radarState", "selfdriveState"], - subs=["longitudinalPlan", "driverAssistance"], + subs=["longitudinalPlan", "longitudinalPlanIC", "driverAssistance"], ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"], init_callback=get_car_params_callback, should_recv_callback=MessageBasedRcvCallback("modelV2"), @@ -555,7 +557,7 @@ CONFIGS = [ ), ProcessConfig( proc_name="curvatured", - pubs=["livePose", "liveCalibration", "liveDelay", "carState", "carControl", "controlsState"], + pubs=["livePose", "liveCalibration", "liveDelay", "carState", "carStateIC", "carControl", "carControlIC", "controlsStateIC"], subs=["liveCurvatureParameters"], ignore=["logMonoTime"], init_callback=get_car_params_callback, diff --git a/openpilot/selfdrive/ui/mici/onroad/dynamic_steering_learner_graph.py b/openpilot/selfdrive/ui/mici/onroad/dynamic_steering_learner_graph.py index 172b01984..2bc312d8c 100644 --- a/openpilot/selfdrive/ui/mici/onroad/dynamic_steering_learner_graph.py +++ b/openpilot/selfdrive/ui/mici/onroad/dynamic_steering_learner_graph.py @@ -152,13 +152,14 @@ class DynamicSteeringLearnerGraphMici(Widget): lcp_frame, preview_corrections, preview_valid, fit_corrections, fit_valid, float(car_state.vEgo) ) + cs_ic = sm["controlsStateIC"] self._draw_plot( plot_rect, preview_curve, corrections, min_y, max_y, - float(controls_state.modelDesiredCurvature), + float(cs_ic.modelDesiredCurvature), payload_valid, ) diff --git a/openpilot/selfdrive/ui/onroad/battery_details.py b/openpilot/selfdrive/ui/onroad/battery_details.py index 13cbb9b22..8b936c6b4 100644 --- a/openpilot/selfdrive/ui/onroad/battery_details.py +++ b/openpilot/selfdrive/ui/onroad/battery_details.py @@ -55,9 +55,9 @@ class BatteryDetails(Widget): self._reset_values() return - car_state = sm["carState"] - - battery_data = car_state.batteryDetails + car_state_ic = sm["carStateIC"] + + battery_data = car_state_ic.batteryDetails self._capacity = float(battery_data.capacity) self._charge = float(battery_data.charge) self._soc = float(battery_data.soc) diff --git a/openpilot/selfdrive/ui/onroad/dynamic_steering_learner_graph.py b/openpilot/selfdrive/ui/onroad/dynamic_steering_learner_graph.py index 7a8322b6a..2055d8019 100644 --- a/openpilot/selfdrive/ui/onroad/dynamic_steering_learner_graph.py +++ b/openpilot/selfdrive/ui/onroad/dynamic_steering_learner_graph.py @@ -165,7 +165,8 @@ class DynamicSteeringLearnerGraph(Widget): _, _, min_y, max_y = self._draw_plot( plot_rect, preview_curve, corrections, min_y, max_y, transport_valid and payload_valid ) - self._draw_overlay_info(graph_rect, lcp, float(car_state.vEgo), float(controls_state.modelDesiredCurvature), + cs_ic = sm["controlsStateIC"] + self._draw_overlay_info(graph_rect, lcp, float(car_state.vEgo), float(cs_ic.modelDesiredCurvature), fit_corrections, fit_valid, min_y, max_y, transport_valid, payload_valid) def _build_graph_rect(self, rect: rl.Rectangle) -> rl.Rectangle: diff --git a/openpilot/selfdrive/ui/ui_state.py b/openpilot/selfdrive/ui/ui_state.py index 7035b7d39..884fc1bc8 100644 --- a/openpilot/selfdrive/ui/ui_state.py +++ b/openpilot/selfdrive/ui/ui_state.py @@ -39,6 +39,14 @@ class UIState(UIStateSP): def _initialize(self): UIStateSP.__init__(self) self.params = Params() + ic_services = [ + "liveCurvatureParameters", + "controlsStateIC", + "carStateIC", + "carControlIC", + "carParamsIC", + "longitudinalPlanIC", + ] self.sm = messaging.SubMaster( [ "modelV2", @@ -61,10 +69,9 @@ class UIState(UIStateSP): "carOutput", "carControl", "liveParameters", - "liveCurvatureParameters", "testJoystick", "rawAudioData", - ] + self.sm_services_ext + ] + ic_services + self.sm_services_ext ) self.prime_state = PrimeState() diff --git a/openpilot/sunnypilot/selfdrive/car/tests/test_custom_cruise.py b/openpilot/sunnypilot/selfdrive/car/tests/test_custom_cruise.py index ce5d7dc12..4ca1f3e6c 100644 --- a/openpilot/sunnypilot/selfdrive/car/tests/test_custom_cruise.py +++ b/openpilot/sunnypilot/selfdrive/car/tests/test_custom_cruise.py @@ -30,24 +30,24 @@ class TestCustomAccIncrements(TestVCruiseHelper): """Simulate a short button press (press + release)""" CS = car.CarState(cruiseState={"available": True}) CS.buttonEvents = [ButtonEvent(type=button_type, pressed=True)] - self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True) + self.v_cruise_helper.update_v_cruise(CS, self.CS_IC, enabled=True, is_metric=True) CS.buttonEvents = [ButtonEvent(type=button_type, pressed=False)] - self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True) + self.v_cruise_helper.update_v_cruise(CS, self.CS_IC, enabled=True, is_metric=True) def press_button_long(self, button_type: car.CarState.ButtonEvent.Type) -> None: """Simulate a long button press (50+ frames)""" CS = car.CarState(cruiseState={"available": True}) CS.buttonEvents = [ButtonEvent(type=button_type, pressed=True)] - self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True) + self.v_cruise_helper.update_v_cruise(CS, self.CS_IC, enabled=True, is_metric=True) # Hold for 50 frames to trigger long press CS.buttonEvents = [] for _ in range(50): - self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True) + self.v_cruise_helper.update_v_cruise(CS, self.CS_IC, enabled=True, is_metric=True) CS.buttonEvents = [ButtonEvent(type=button_type, pressed=False)] - self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True) + self.v_cruise_helper.update_v_cruise(CS, self.CS_IC, enabled=True, is_metric=True) def set_custom_increments(self, enabled: bool, short_inc: int, long_inc: int) -> None: """Set custom ACC increment parameters""" diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_fingerprint.py b/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_fingerprint.py index 07e7d2852..d424bf0cd 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_fingerprint.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_fingerprint.py @@ -21,7 +21,8 @@ class TestNNLCFingerprintBase: CarInterface = interfaces[car_name] CP = CarInterface.get_non_essential_params(car_name) CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name) - CI = CarInterface(CP, CP_SP) + CP_IC = CarInterface.get_non_essential_params_ic(CP, car_name) + CI = CarInterface(CP, CP_SP, CP_IC) sunnypilot_interfaces.setup_interfaces(CI, Params()) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_load_model.py b/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_load_model.py index 2f4e0d699..cd66ea0ce 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_load_model.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_load_model.py @@ -20,7 +20,8 @@ class TestNNTorqueModel: CarInterface = interfaces[car_name] CP = CarInterface.get_non_essential_params(car_name) CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name) - CI = CarInterface(CP, CP_SP) + CP_IC = CarInterface.get_non_essential_params_ic(CP, car_name) + CI = CarInterface(CP, CP_SP, CP_IC) sunnypilot_interfaces.setup_interfaces(CI, params) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py b/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py index 56fd2f9ce..67438e1cb 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py @@ -52,7 +52,8 @@ class TestNeuralNetworkLateralControl: CarInterface = interfaces[car_name] CP = CarInterface.get_non_essential_params(car_name) CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name) - CI = CarInterface(CP, CP_SP) + CP_IC = CarInterface.get_non_essential_params_ic(CP, car_name) + CI = CarInterface(CP, CP_SP, CP_IC) sunnypilot_interfaces.setup_interfaces(CI, params) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py b/openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py index 7cd6fef52..1f02ff6b3 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py @@ -69,7 +69,8 @@ class TestSpeedLimitAssist: CarInterface = interfaces[car_name] CP = CarInterface.get_non_essential_params(car_name) CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name) - CI = CarInterface(CP, CP_SP) + CP_IC = CarInterface.get_non_essential_params_ic(CP, car_name) + CI = CarInterface(CP, CP_SP, CP_IC) CI.CP.openpilotLongitudinalControl = True # always assume it's openpilot longitudinal sunnypilot_interfaces.setup_interfaces(CI, self.params) return CI