mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-07-25 11:22:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/infiniteCable2/openpilot into rebel_legion

Browse Source
This commit is contained in:
infiniteCable2
2026-07-24 23:36:52 +02:00
parent be6af46a90 44bc7cd879
commit f2734d3673
30 changed files with 325 additions and 137 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo
+1 -1
Submodule opendbc_repo updated: 8859db75de...0614ef8314
openpilot/cereal/custom.capnp
+44 -5
View File
@@ -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 {
openpilot/cereal/log.capnp
+6 -9
View File
@@ -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 ***********
openpilot/cereal/services.py
+5
View File
@@ -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),
openpilot/common/params_keys.h
+3
View File
@@ -165,6 +165,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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"}},
openpilot/selfdrive/car/car_specific.py
+4 -4
View File
@@ -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)
openpilot/selfdrive/car/card.py
+47 -17
View File
@@ -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():
openpilot/selfdrive/car/cruise.py
+6 -6
View File
@@ -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
openpilot/selfdrive/car/helpers.py
+17 -1
View File
@@ -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
openpilot/selfdrive/car/tests/test_car_interfaces.py
+6 -3
View File
@@ -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
openpilot/selfdrive/car/tests/test_cruise_speed.py
+7 -6
View File
@@ -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:
openpilot/selfdrive/car/tests/test_models.py
+13 -9
View File
@@ -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
openpilot/selfdrive/controls/controlsd.py
+38 -20
View File
@@ -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
openpilot/selfdrive/controls/lib/longitudinal_planner.py
+7 -2
View File
@@ -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)
openpilot/selfdrive/controls/plannerd.py
+1 -1
View File
@@ -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)
openpilot/selfdrive/controls/tests/test_latcontrol.py
+2 -1
View File
@@ -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)
openpilot/selfdrive/controls/tests/test_latcontrol_torque_buffer.py
+2 -1
View File
@@ -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)
openpilot/selfdrive/locationd/curvatured.py
+42 -19
View File
@@ -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()
openpilot/selfdrive/locationd/test/test_curvatured.py
+20 -2
View File
@@ -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.
openpilot/selfdrive/selfdrived/selfdrived.py
+15 -6
View File
@@ -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
openpilot/selfdrive/test/process_replay/process_replay.py
+10 -8
View File
@@ -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,
openpilot/selfdrive/ui/mici/onroad/dynamic_steering_learner_graph.py
+2 -1
View File
@@ -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,
)
openpilot/selfdrive/ui/onroad/battery_details.py
+3 -3
View File
@@ -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)
openpilot/selfdrive/ui/onroad/dynamic_steering_learner_graph.py
+2 -1
View File
@@ -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:
openpilot/selfdrive/ui/ui_state.py
+9 -2
View File
@@ -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()
openpilot/sunnypilot/selfdrive/car/tests/test_custom_cruise.py
+5 -5
View File
@@ -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"""
openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_fingerprint.py
+2 -1
View File
@@ -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())
openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_load_model.py
+2 -1
View File
@@ -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)
openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py
+2 -1
View File
@@ -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)
openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py
+2 -1
View File
@@ -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