mirror of
https://github.com/infiniteCable2/openpilot.git
synced 2026-07-25 19:32:03 +08:00
Upstream-compliant extensibility (#22)
* custom data handling * cleanups * cleanup * Update opendbc_repo * fix missing CarParamsIC handling * ic srv list and duplicate sp cleanup * divide lists for better visibility * fix curvatured, missing services, split lists, cleanup unneccessary * curvatured time correctness after custom IC structure introduction * hint for future data quality adaption * point to master before merge
This commit is contained in:
infiniteCable
+1
-1
Submodule opendbc_repo updated: 8859db75de...0614ef8314
@@ -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 {
|
||||
|
||||
@@ -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 ***********
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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"}},
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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():
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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()
|
||||
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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,
|
||||
)
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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"""
|
||||
|
||||
@@ -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())
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
+2
-1
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user