mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-23 02:23:47 +08:00
Compare commits
1 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 github-actions[bot]
|
10e05bbf32 |
@@ -11,8 +11,7 @@ from opendbc.car.toyota import toyotacan
|
||||
from opendbc.car.toyota.values import CAR, NO_STOP_TIMER_CAR, TSS2_CAR, \
|
||||
CarControllerParams, ToyotaFlags
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.sunnypilot.car.toyota.auto_brake_hold import AutoBrakeHoldCarController
|
||||
from opendbc.sunnypilot.car.toyota.enhanced_bsm import EnhancedBsmCarController
|
||||
|
||||
from opendbc.sunnypilot.car.toyota.gas_interceptor import GasInterceptorCarController
|
||||
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
|
||||
|
||||
@@ -20,7 +19,6 @@ Ecu = structs.CarParams.Ecu
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
SteerControlType = structs.CarParams.SteerControlType
|
||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||
GearShifter = structs.CarState.GearShifter
|
||||
|
||||
# The up limit allows the brakes/gas to unwind quickly leaving a stop,
|
||||
# the down limit roughly matches the rate of ACCEL_NET, reducing PCM compensation windup
|
||||
@@ -38,20 +36,11 @@ MAX_STEER_RATE_FRAMES = 17 # tx control frames needed before torque can be cut
|
||||
# EPS allows user torque above threshold for 50 frames before permanently faulting
|
||||
MAX_USER_TORQUE = 500
|
||||
|
||||
CRUISE_CANCEL_DELAY_FRAMES = 10
|
||||
|
||||
def get_long_tune(CP, CP_SP, params):
|
||||
def get_long_tune(CP, params):
|
||||
if CP.flags & ToyotaFlags.TSS2:
|
||||
if CP_SP.flags & ToyotaFlagsSP.TSS2_LONG_TUNING:
|
||||
#kiBP = [0., 2.0, 9.0, 14., 20., 27.]
|
||||
#kiV = [0.25, 0.25, 0.15, 0.12, 0.12, 0.12]
|
||||
#kiBP= [0., 1.0, 2.0, 3.0, 4.0, 5.0, 7., 20., 27., 36.]
|
||||
#kiV = [0.31, 0.32, 0.301, 0.280, 0.259, 0.226, 0.15, 0.15, 0.101, 0.10]
|
||||
kiBP = [0.0, 1.0, 3.0, 5.0, 12., 36.]
|
||||
kiV = [0.30, 0.35, 0.32, 0.28, 0.24, 0.20]
|
||||
else:
|
||||
kiBP = [2., 5.]
|
||||
kiV = [0.5, 0.25]
|
||||
kiBP = [2., 5.]
|
||||
kiV = [0.5, 0.25]
|
||||
else:
|
||||
kiBP = [0., 5., 35.]
|
||||
kiV = [3.6, 2.4, 1.5]
|
||||
@@ -74,10 +63,9 @@ class CarController(CarControllerBase, GasInterceptorCarController):
|
||||
self.permit_braking = True
|
||||
self.steer_rate_counter = 0
|
||||
self.distance_button = 0
|
||||
self.cancel_counter = 0
|
||||
|
||||
# *** start long control state ***
|
||||
self.long_pid = get_long_tune(self.CP, self.CP_SP, self.params)
|
||||
self.long_pid = get_long_tune(self.CP, self.params)
|
||||
self.aego = FirstOrderFilter(0.0, 0.25, DT_CTRL * 3)
|
||||
self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL)
|
||||
self.pitch_hp = HighPassFilter(0.0, 0.25, 1.5, DT_CTRL)
|
||||
@@ -93,20 +81,11 @@ class CarController(CarControllerBase, GasInterceptorCarController):
|
||||
self.secoc_acc_message_counter = 0
|
||||
self.secoc_prev_reset_counter = 0
|
||||
|
||||
self.enhanced_bsm = EnhancedBsmCarController(CP, CP_SP)
|
||||
self.auto_brake_hold = AutoBrakeHoldCarController(CP, CP_SP)
|
||||
|
||||
self._auto_lock_speed = 0.0
|
||||
|
||||
self._auto_lock_once = False
|
||||
self._gear_prev = GearShifter.park
|
||||
|
||||
def update(self, CC, CC_SP, CS, now_nanos):
|
||||
actuators = CC.actuators
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
hud_control = CC.hudControl
|
||||
self.cancel_counter = self.cancel_counter + 1 if CC.cruiseControl.cancel else 0
|
||||
pcm_cancel_cmd = self.cancel_counter > CRUISE_CANCEL_DELAY_FRAMES
|
||||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE
|
||||
|
||||
if len(CC.orientationNED) == 3:
|
||||
@@ -217,9 +196,6 @@ class CarController(CarControllerBase, GasInterceptorCarController):
|
||||
|
||||
self.last_standstill = CS.out.standstill
|
||||
|
||||
if self.auto_brake_hold.enabled:
|
||||
can_sends.extend(self.auto_brake_hold.update(CS, self.frame, self.packer))
|
||||
|
||||
# handle UI messages
|
||||
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
|
||||
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
|
||||
@@ -343,9 +319,6 @@ class CarController(CarControllerBase, GasInterceptorCarController):
|
||||
if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
|
||||
can_sends.append(make_tester_present_msg(0x750, 0, 0xF))
|
||||
|
||||
if self.enhanced_bsm.enabled:
|
||||
can_sends.extend(self.enhanced_bsm.update(CS, self.frame))
|
||||
|
||||
new_actuators = actuators.as_builder()
|
||||
new_actuators.torque = apply_torque / self.params.STEER_MAX
|
||||
new_actuators.torqueOutputCan = apply_torque
|
||||
|
||||
@@ -1,6 +1,4 @@
|
||||
import copy
|
||||
from enum import IntEnum
|
||||
import importlib
|
||||
|
||||
from opendbc.can import CANDefine, CANParser
|
||||
from opendbc.car import Bus, DT_CTRL, create_button_events, structs
|
||||
@@ -10,34 +8,11 @@ from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
|
||||
TSS2_CAR, EPS_SCALE
|
||||
from opendbc.sunnypilot.car.toyota.carstate_ext import CarStateExt
|
||||
from opendbc.sunnypilot.car.toyota.enhanced_bsm import EnhancedBsmCarState
|
||||
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
SteerControlType = structs.CarParams.SteerControlType
|
||||
|
||||
|
||||
class AccelPersonality(IntEnum):
|
||||
eco = 0
|
||||
normal = 1
|
||||
sport = 2
|
||||
|
||||
|
||||
def get_accel_personality(sport_mode: int, eco_mode: int) -> AccelPersonality:
|
||||
if sport_mode == 1:
|
||||
return AccelPersonality.sport
|
||||
if eco_mode == 1:
|
||||
return AccelPersonality.eco
|
||||
return AccelPersonality.normal
|
||||
|
||||
|
||||
def get_host_params():
|
||||
"""Return sunnypilot's Params store when opendbc is embedded in openpilot."""
|
||||
try:
|
||||
return importlib.import_module("openpilot.common.params").Params()
|
||||
except ModuleNotFoundError:
|
||||
return None
|
||||
|
||||
# These steering fault definitions seem to be common across LKA (torque) and LTA (angle):
|
||||
# - high steer rate fault: goes to 21 or 25 for 1 frame, then 9 for 2 seconds
|
||||
# - lka/lta msg drop out: goes to 9 then 11 for a combined total of 2 seconds, then 3.
|
||||
@@ -80,21 +55,6 @@ class CarState(CarStateBase, CarStateExt):
|
||||
self.gvc = 0.0
|
||||
self.secoc_synchronization = None
|
||||
|
||||
self.enhanced_bsm = EnhancedBsmCarState(CP, CP_SP)
|
||||
|
||||
if CP_SP.flags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
|
||||
self.pre_collision_2 = {}
|
||||
|
||||
self._host_params = get_host_params()
|
||||
self.toyota_drive_mode = self._host_params is not None and self._host_params.get_bool('ToyotaDriveMode')
|
||||
self._drive_mode_signals_checked = False
|
||||
self._sport_signal_available = False
|
||||
self._eco_signal_available = False
|
||||
self._prev_accel_profile = None
|
||||
self._accel_profile_init = False
|
||||
|
||||
self.frame = 0
|
||||
|
||||
def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]:
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
@@ -112,47 +72,18 @@ class CarState(CarStateBase, CarStateExt):
|
||||
ret.parkingBrake = cp.vl["BODY_CONTROL_STATE"]["PARKING_BRAKE"] == 1
|
||||
|
||||
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
|
||||
#ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1
|
||||
ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1
|
||||
|
||||
if self.CP.flags & ToyotaFlags.SECOC.value:
|
||||
self.secoc_synchronization = copy.copy(cp.vl["SECOC_SYNCHRONIZATION"])
|
||||
ret.gasPressed = cp.vl["GAS_PEDAL"]["GAS_PEDAL_USER"] > 0
|
||||
can_gear = int(cp.vl["GEAR_PACKET_HYBRID"]["GEAR"])
|
||||
else:
|
||||
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 # TODO: these also have GAS_PEDAL, come back and unify
|
||||
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
|
||||
can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
|
||||
if not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
|
||||
ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
|
||||
|
||||
if self.toyota_drive_mode: # and not self.CP.flags & ToyotaFlags.SECOC.value:
|
||||
sport_signal = 'SPORT_ON_2' if self.CP.carFingerprint in (CAR.TOYOTA_RAV4_TSS2, CAR.LEXUS_ES_TSS2,
|
||||
CAR.TOYOTA_HIGHLANDER_TSS2) else 'SPORT_ON'
|
||||
|
||||
if not self._drive_mode_signals_checked:
|
||||
self._drive_mode_signals_checked = True
|
||||
try:
|
||||
sport_mode = cp.vl["GEAR_PACKET"][sport_signal]
|
||||
self._sport_signal_available = True
|
||||
except KeyError:
|
||||
sport_mode = 0
|
||||
self._sport_signal_available = False
|
||||
try:
|
||||
eco_mode = cp.vl["GEAR_PACKET"]['ECON_ON']
|
||||
self._eco_signal_available = True
|
||||
except KeyError:
|
||||
eco_mode = 0
|
||||
self._eco_signal_available = False
|
||||
else:
|
||||
sport_mode = cp.vl["GEAR_PACKET"][sport_signal] if self._sport_signal_available else 0
|
||||
eco_mode = cp.vl["GEAR_PACKET"]['ECON_ON'] if self._eco_signal_available else 0
|
||||
|
||||
accel_profile = get_accel_personality(sport_mode, eco_mode)
|
||||
|
||||
if not self._accel_profile_init or accel_profile != self._prev_accel_profile:
|
||||
self._host_params.put('AccelPersonality', int(accel_profile))
|
||||
self._accel_profile_init = True
|
||||
self._prev_accel_profile = accel_profile
|
||||
|
||||
self.parse_wheel_speeds(ret,
|
||||
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
|
||||
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"],
|
||||
@@ -281,14 +212,6 @@ class CarState(CarStateBase, CarStateExt):
|
||||
|
||||
ret.buttonEvents = buttonEvents
|
||||
|
||||
if self.enhanced_bsm.enabled and self.frame > 199:
|
||||
ret.leftBlindspot, ret.rightBlindspot = self.enhanced_bsm.update(cp, self.frame)
|
||||
|
||||
if self.CP_SP.flags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
|
||||
self.pre_collision_2 = copy.copy(cp_cam.vl["PRE_COLLISION_2"])
|
||||
|
||||
self.frame += 1
|
||||
|
||||
CarStateExt.update(self, ret, ret_sp, can_parsers)
|
||||
|
||||
return ret, ret_sp
|
||||
|
||||
@@ -3,7 +3,7 @@ from opendbc.car.toyota.carstate import CarState
|
||||
from opendbc.car.toyota.carcontroller import CarController
|
||||
from opendbc.car.toyota.radar_interface import RadarInterface
|
||||
from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
|
||||
MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ToyotaSafetyFlags, UNSUPPORTED_DSU_CAR, SECOC_CAR
|
||||
MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ToyotaSafetyFlags, UNSUPPORTED_DSU_CAR
|
||||
from opendbc.car.disable_ecu import disable_ecu
|
||||
from opendbc.car.interfaces import CarInterfaceBase
|
||||
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP, ToyotaSafetyFlagsSP
|
||||
@@ -65,9 +65,6 @@ class CarInterface(CarInterfaceBase):
|
||||
if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
|
||||
ret.steerActuatorDelay = 0.25
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
|
||||
# 2021+ TSS2 steering rack swapped into a TSS-P car, not supported
|
||||
if fw.ecu == "eps" and fw.fwVersion == b'8965B47070\x00\x00\x00\x00\x00\x00':
|
||||
ret.dashcamOnly = True
|
||||
|
||||
elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2):
|
||||
stop_and_go = True
|
||||
@@ -133,9 +130,6 @@ class CarInterface(CarInterfaceBase):
|
||||
if candidate in UNSUPPORTED_DSU_CAR:
|
||||
ret.safetyParam |= ToyotaSafetyFlagsSP.UNSUPPORTED_DSU
|
||||
|
||||
if candidate == CAR.TOYOTA_PRIUS_TSS2:
|
||||
ret.flags |= ToyotaFlagsSP.SP_NEED_DEBUG_BSM.value
|
||||
|
||||
# Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it
|
||||
# 0x2AA is sent by a similar device which intercepts the radar instead of DSU on NO_DSU_CARs
|
||||
if 0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR):
|
||||
|
||||
@@ -1,5 +1,4 @@
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.can_definitions import CanData
|
||||
|
||||
SteerControlType = CarParams.SteerControlType
|
||||
|
||||
@@ -165,47 +164,3 @@ def toyota_checksum(address: int, sig, d: bytearray) -> int:
|
||||
for i in range(len(d) - 1):
|
||||
s += d[i]
|
||||
return s & 0xFF
|
||||
|
||||
def create_set_bsm_debug_mode(lr_blindspot, enabled):
|
||||
dat = b"\x02\x10\x60\x00\x00\x00\x00" if enabled else b"\x02\x10\x01\x00\x00\x00\x00"
|
||||
dat = lr_blindspot + dat
|
||||
|
||||
return CanData(0x750, dat, 0)
|
||||
|
||||
|
||||
def create_bsm_polling_status(lr_blindspot):
|
||||
return CanData(0x750, lr_blindspot + b"\x02\x21\x69\x00\x00\x00\x00", 0)
|
||||
|
||||
|
||||
# auto brake hold
|
||||
def create_brake_hold_command(packer, frame, pre_collision_2, brake_hold_active):
|
||||
# forward PRE_COLLISION_2 when auto brake hold is not active
|
||||
values = {s: pre_collision_2[s] for s in [
|
||||
"DSS1GDRV",
|
||||
"DS1STAT2",
|
||||
"DS1STBK2",
|
||||
"PCSWAR",
|
||||
"PCSALM",
|
||||
"PCSOPR",
|
||||
"PCSABK",
|
||||
"PBATRGR",
|
||||
"PPTRGR",
|
||||
"IBTRGR",
|
||||
"CLEXTRGR",
|
||||
"IRLT_REQ",
|
||||
"BRKHLD",
|
||||
"AVSTRGR",
|
||||
"VGRSTRGR",
|
||||
"PREFILL",
|
||||
"PBRTRGR",
|
||||
"PCSDIS",
|
||||
"PBPREPMP",
|
||||
]}
|
||||
|
||||
if brake_hold_active:
|
||||
values = {
|
||||
"DSS1GDRV": 0x3FF,
|
||||
"PBRTRGR": frame % 730 < 727, # cut actuation for 3 frames
|
||||
}
|
||||
|
||||
return packer.make_can_msg("PRE_COLLISION_2", 0, values)
|
||||
|
||||
@@ -1,79 +0,0 @@
|
||||
BO_ 1880 DEBUG: 8 XXX
|
||||
SG_ BLINDSPOTSIDE : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ BLINDSPOT : 38|1@0+ (1,0) [0|15] "" XXX
|
||||
SG_ BLINDSPOTD1 : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ BLINDSPOTD2 : 55|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 836 PRE_COLLISION_2: 8 DSU
|
||||
SG_ DSS1GDRV : 7|10@0- (0.1,0) [0|0] "m/s^2" Vector__XXX
|
||||
SG_ DS1STAT2 : 13|3@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ DS1STBK2 : 10|3@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ PCSWAR : 18|1@0+ (1,0) [0|0] "" FCM
|
||||
SG_ PCSALM : 17|1@0+ (1,0) [0|0] "" FCM
|
||||
SG_ PCSOPR : 16|1@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ PCSABK : 31|1@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ PBATRGR : 30|2@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ PPTRGR : 28|1@0+ (1,0) [0|0] "" FCM
|
||||
SG_ IBTRGR : 27|1@0+ (1,0) [0|0] "" FCM
|
||||
SG_ CLEXTRGR : 26|1@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ IRLT_REQ : 25|2@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ BRKHLD : 37|1@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ AVSTRGR : 36|1@0+ (1,0) [0|0] "" SCS
|
||||
SG_ VGRSTRGR : 35|2@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ PREFILL : 33|1@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ PBRTRGR : 32|1@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ PCSDIS : 43|1@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ PBPREPMP : 40|1@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
|
||||
|
||||
BO_ 713 HYBRID_POWERTRAIN: 8 XXX
|
||||
SG_ COAST_FUEL_CUT : 5|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ HYBRID_DRIVE_STATE : 11|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
BO_ 814 BRAKE_HYDRAULIC: 8 XXX
|
||||
SG_ BRAKE_PRESSURE : 15|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ BRAKE_PRESSURE_COPY1 : 31|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ BRAKE_PRESSURE_COPY2 : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 971 MOTOR_SPEED_CLUSTER: 7 XXX
|
||||
SG_ CLUSTER_SPEED : 55|8@0+ (1,-21) [0|255] "km/h" XXX
|
||||
|
||||
BO_ 896 HYBRID_BATTERY: 8 XXX
|
||||
SG_ HV_SOC_PCT : 55|8@0+ (1,0) [0|100] "%" XXX
|
||||
|
||||
BO_ 1654 HV_POWER_INTEGRATOR: 8 XXX
|
||||
SG_ HV_POWER_ACCUM : 39|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 975 HV_INVERTER: 5 XXX
|
||||
SG_ HV_VOLTAGE_LO : 23|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ HV_CURRENT_LO : 39|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 955 BRAKE_REDUNDANT: 8 XXX
|
||||
SG_ BRAKE_PRESSED_REDUNDANT : 0|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 918 VEHICLE_DRIVE_MODE: 8 XXX
|
||||
SG_ DRIVE_MODE_STATE : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
CM_ "Sunnypilot Prius TSS2 reverse-engineered signals — route 550a71ee4c7a7fbe/0000038f--6b48497966, 2026-05-19. Confidence: high unless noted.";
|
||||
CM_ BO_ 713 "Hybrid powertrain status. 100Hz on bus 0.";
|
||||
CM_ SG_ 713 COAST_FUEL_CUT "byte0 bit5. 1 when accelerator lifted at speed -> ICE fuel cut / decel-fuel-cut active. P(=1|coast)=0.95, P(=1|accel)=0.008, P(=1|brake)=0.0. Use: lift detection for accel feedforward, EV-only mode hint.";
|
||||
CM_ SG_ 713 HYBRID_DRIVE_STATE "byte1 lower-nibble (bits 8-11). Enum (validated): 4=brake/regen-active, 5-8=coast/cruise levels, 10/12=ICE running active, 11=idle-stop. Not a scalar magnitude. Use: distinguish ICE-on vs EV for accel command shaping.";
|
||||
CM_ BO_ 814 "Brake hydraulic. 5Hz on bus 0. Bytes 0,2,4,6 are status/flag bytes (not yet decoded). Saturates 255 under hard brake.";
|
||||
CM_ SG_ 814 BRAKE_PRESSURE "byte1. Master cylinder pressure proxy. 0 during coast, 33-45 mean during brake, 254-255 max. NOT same as BRAKE_MODULE.BRAKE_PRESSURE.";
|
||||
CM_ SG_ 814 BRAKE_PRESSURE_COPY1 "byte3. Identical to BRAKE_PRESSURE 90% of brake samples. Redundant copy (CRC/safety).";
|
||||
CM_ SG_ 814 BRAKE_PRESSURE_COPY2 "byte5. Identical to BRAKE_PRESSURE 90% of brake samples. Redundant copy.";
|
||||
CM_ BO_ 971 "Cluster speedometer feed. 10Hz on bus 0.";
|
||||
CM_ SG_ 971 CLUSTER_SPEED "byte6. Encoded as raw_byte = v_kph + 21. After (1,-21) factor/offset: km/h matching dashboard speedometer. Corr 0.998 w/ WHEEL_SPEEDS. ~5km/h higher than wheel mean = cluster display bias.";
|
||||
CM_ BO_ 896 "HV battery status. 5Hz on bus 0.";
|
||||
CM_ SG_ 896 HV_SOC_PCT "byte6. HV battery state-of-charge percent. Validated range 53-62% on route 0000038f (Prius hybrid SoC normal band 40-80%). Use: energy-aware planner — reduce regen demand at high SoC (>70%), lift accel demand at low SoC (<50%).";
|
||||
CM_ BO_ 1654 "HV power integrator. 1Hz on bus 0. Slow drift signal, semantic not fully confirmed.";
|
||||
CM_ SG_ 1654 HV_POWER_ACCUM "byte4. Slow-drifting integer 0-59, independent of HV_SOC_PCT. Hypothesis: HV power flow accumulator or charging-cycle counter. Needs more routes.";
|
||||
CM_ BO_ 975 "HV inverter telemetry. 10Hz on bus 0. 5-byte message.";
|
||||
CM_ SG_ 975 HV_VOLTAGE_LO "byte2. Sequential integer 40-53 range. Hypothesis: HV bus voltage low-byte scaled. Moderate corr w/ brake (regen-loaded voltage).";
|
||||
CM_ SG_ 975 HV_CURRENT_LO "byte4. Sequential integer 17-36 range. JUMPS -16 at brake-release (regen current cutoff signature). Use: real-time regen current readback for brake-blend tuning.";
|
||||
CM_ BO_ 955 "Brake redundant signal. 10Hz on bus 0.";
|
||||
CM_ SG_ 955 BRAKE_PRESSED_REDUNDANT "byte0 bit0. 99.9% agreement with BRAKE_MODULE.BRAKE_PRESSED. Safety-redundant copy. Use: cross-check brake state for fault detection.";
|
||||
CM_ BO_ 918 "Vehicle drive mode state. 5Hz on bus 0.";
|
||||
CM_ SG_ 918 DRIVE_MODE_STATE "byte0. 3 states observed: 185=parked-with-brake, 189=ready-to-drive, 191=ready-no-brake. Transitions on GEAR shifts + brake. Use: more reliable drive-state machine than gear+brake combo.";
|
||||
VAL_ 713 HYBRID_DRIVE_STATE 4 "brake_regen_active" 5 "coast_5" 6 "coast_6" 7 "coast_7" 8 "coast_cruise" 10 "ice_active_10" 11 "idle_stop" 12 "ice_active_12";
|
||||
VAL_ 918 DRIVE_MODE_STATE 185 "parked_brake" 189 "ready" 191 "ready_no_brake";
|
||||
@@ -234,7 +234,6 @@ BO_ 956 GEAR_PACKET: 8 XXX
|
||||
SG_ SPORT_GEAR_ON : 33|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SPORT_GEAR : 38|3@0+ (1,0) [0|7] "" XXX
|
||||
SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SPORT_ON_2 : 55|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ B_GEAR_ENGAGED : 41|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ DRIVE_ENGAGED : 47|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
@@ -546,7 +545,6 @@ VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P";
|
||||
VAL_ 956 SPORT_GEAR_ON 0 "off" 1 "on";
|
||||
VAL_ 956 SPORT_GEAR 1 "S1" 2 "S2" 3 "S3" 4 "S4" 5 "S5" 6 "S6";
|
||||
VAL_ 956 ECON_ON 0 "off" 1 "on";
|
||||
VAL_ 956 SPORT_ON_2 0 "off" 1 "on";
|
||||
VAL_ 956 B_GEAR_ENGAGED 0 "off" 1 "on";
|
||||
VAL_ 956 DRIVE_ENGAGED 0 "off" 1 "on";
|
||||
VAL_ 1005 REVERSE_CAMERA_GUIDELINES 3 "No guidelines" 2 "Static guidelines" 1 "Active guidelines";
|
||||
|
||||
@@ -35,6 +35,14 @@ BO_ 740 STEERING_LKA: 5 XXX
|
||||
SG_ STEER_TORQUE_CMD : 15|16@0- (1,0) [0|65535] "" XXX
|
||||
SG_ CHECKSUM : 39|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 836 PRE_COLLISION_2: 8 DSU
|
||||
SG_ DSS1GDRV : 7|10@0- (0.1,0) [0|0] "m/s^2" Vector__XXX
|
||||
SG_ PCSALM : 17|1@0+ (1,0) [0|0] "" FCM
|
||||
SG_ IBTRGR : 27|1@0+ (1,0) [0|0] "" FCM
|
||||
SG_ PBATRGR : 30|2@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ PREFILL : 33|1@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ AVSTRGR : 36|1@0+ (1,0) [0|0] "" SCS
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX
|
||||
|
||||
CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input";
|
||||
CM_ SG_ 466 ACC_BRAKING "whether brakes are being actuated from ACC command";
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
CM_ "IMPORT _community.dbc";
|
||||
CM_ "IMPORT _toyota_2017.dbc";
|
||||
CM_ "IMPORT _toyota_adas_standard.dbc";
|
||||
CM_ "IMPORT _sp_debug_toyota.dbc";
|
||||
|
||||
BO_ 548 BRAKE_MODULE: 8 XXX
|
||||
SG_ BRAKE_PRESSURE : 43|12@0+ (1,0) [0|4047] "" XXX
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
CM_ "IMPORT _community.dbc";
|
||||
CM_ "IMPORT _toyota_2017.dbc";
|
||||
CM_ "IMPORT _toyota_adas_standard.dbc";
|
||||
CM_ "IMPORT _sp_debug_toyota.dbc";
|
||||
|
||||
BO_ 401 STEERING_LTA: 8 XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
CM_ "IMPORT _community.dbc";
|
||||
CM_ "IMPORT _toyota_2017.dbc";
|
||||
CM_ "IMPORT _toyota_adas_standard.dbc";
|
||||
CM_ "IMPORT _sp_debug_toyota.dbc";
|
||||
|
||||
BO_ 550 BRAKE_MODULE: 8 XXX
|
||||
SG_ BRAKE_PRESSURE : 0|9@0+ (1,0) [0|511] "" XXX
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
|
||||
// Stock longitudinal
|
||||
#define TOYOTA_BASE_TX_MSGS \
|
||||
{0x191, 0, 8, .check_relay = true}, {0x412, 0, 8, .check_relay = true}, {0x1D2, 0, 8, .check_relay = false}, {0x750, 0, 8, .check_relay = false}, /* LKAS + LTA + PCM cancel cmd */ \
|
||||
{0x191, 0, 8, .check_relay = true}, {0x412, 0, 8, .check_relay = true}, {0x1D2, 0, 8, .check_relay = false}, /* LKAS + LTA + PCM cancel cmd */ \
|
||||
|
||||
#define TOYOTA_COMMON_TX_MSGS \
|
||||
TOYOTA_BASE_TX_MSGS \
|
||||
@@ -69,7 +69,6 @@ static bool toyota_secoc = false;
|
||||
static bool toyota_alt_brake = false;
|
||||
static bool toyota_stock_longitudinal = false;
|
||||
static bool toyota_lta = false;
|
||||
static bool toyota_cruise_engaged = false; // SP: PCM_CRUISE.CRUISE_ACTIVE, narrows the auto brake hold AEB window below
|
||||
static int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file
|
||||
|
||||
static uint32_t toyota_compute_checksum(const CANPacket_t *msg) {
|
||||
@@ -152,7 +151,6 @@ static void toyota_rx_hook(const CANPacket_t *msg) {
|
||||
if (msg->addr == 0x176U) {
|
||||
bool cruise_engaged = GET_BIT(msg, 5U); // PCM_CRUISE.CRUISE_ACTIVE
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
toyota_cruise_engaged = cruise_engaged;
|
||||
}
|
||||
if (msg->addr == 0x116U) {
|
||||
gas_pressed = msg->data[1] != 0U; // GAS_PEDAL.GAS_PEDAL_USER
|
||||
@@ -164,7 +162,6 @@ static void toyota_rx_hook(const CANPacket_t *msg) {
|
||||
if (msg->addr == 0x1D2U) {
|
||||
bool cruise_engaged = GET_BIT(msg, 5U); // PCM_CRUISE.CRUISE_ACTIVE
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
toyota_cruise_engaged = cruise_engaged;
|
||||
|
||||
if (!enable_gas_interceptor) {
|
||||
gas_pressed = !GET_BIT(msg, 4U); // PCM_CRUISE.GAS_RELEASED
|
||||
@@ -389,35 +386,13 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// SP: auto brake hold https://github.com/AlexandreSato
|
||||
if ((msg->addr == 0x344U) && (alternative_experience & ALT_EXP_ALLOW_AEB)) {
|
||||
if (vehicle_moving || gas_pressed || !acc_main_on || toyota_cruise_engaged) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// UDS: Only tester present ("\x0F\x02\x3E\x00\x00\x00\x00\x00") allowed on diagnostics address
|
||||
if (msg->addr == 0x750U) {
|
||||
// this address is sub-addressed. only allow tester present to radar (0xF)
|
||||
bool invalid_uds_msg = (GET_BYTES(msg, 0, 4) != 0x003E020FU) || (GET_BYTES(msg, 4, 4) != 0x0U);
|
||||
// SP: Secret sauce from dp. (ask @rav4kumar prior to modifying)
|
||||
// Enhanced BSM
|
||||
bool sp_valid_uds_msgs = ((GET_BYTES(msg, 0, 4) == 0x01100241U) || // disable left BSM debug
|
||||
(GET_BYTES(msg, 0, 4) == 0x60100241U) || // enable left BSM debug
|
||||
(GET_BYTES(msg, 0, 4) == 0x69210241U) || // poll left BSM status
|
||||
(GET_BYTES(msg, 0, 4) == 0x01100242U) || // disable right BSM debug
|
||||
(GET_BYTES(msg, 0, 4) == 0x60100242U) || // enable right BSM debug
|
||||
(GET_BYTES(msg, 0, 4) == 0x69210242U)) // poll right BSM status
|
||||
&& (GET_BYTES(msg, 4, 4) == 0x0U);
|
||||
|
||||
sp_valid_uds_msgs |= (GET_BYTES(msg, 0, 4) == 0x11300540U) && // automatic door locking and unlocking
|
||||
((GET_BYTES(msg, 4, 4) == 0x00004000U) || // unlock
|
||||
(GET_BYTES(msg, 4, 4) == 0x00008000U)); // lock
|
||||
|
||||
bool valid_tester_present = !invalid_uds_msg && !toyota_stock_longitudinal && !toyota_secoc;
|
||||
if (!valid_tester_present && !sp_valid_uds_msgs) {
|
||||
if (invalid_uds_msg) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
@@ -591,27 +566,10 @@ static safety_config toyota_init(uint16_t param) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
static bool toyota_fwd_hook(int bus_num, int addr) {
|
||||
bool block_msg = false;
|
||||
if (bus_num == 2) {
|
||||
// SP: block AEB when auto brake hold is active, unblock AEB when auto brake hold is not active.
|
||||
// Narrowed to match auto brake hold's own precondition (cruise must be off) - previously this
|
||||
// blocked native AEB forwarding, forcing a slower software relay, any time the car was simply
|
||||
// stopped with the gas released and ACC main on, even while cruise was actively engaged and
|
||||
// auto brake hold couldn't be active at all.
|
||||
bool is_aeb_msg = (addr == 0x344);
|
||||
block_msg = (is_aeb_msg && (alternative_experience & ALT_EXP_ALLOW_AEB) && !vehicle_moving && !gas_pressed && acc_main_on &&
|
||||
!toyota_cruise_engaged);
|
||||
}
|
||||
|
||||
return block_msg;
|
||||
}
|
||||
|
||||
const safety_hooks toyota_hooks = {
|
||||
.init = toyota_init,
|
||||
.rx = toyota_rx_hook,
|
||||
.tx = toyota_tx_hook,
|
||||
.fwd = toyota_fwd_hook,
|
||||
.get_checksum = toyota_get_checksum,
|
||||
.compute_checksum = toyota_compute_checksum,
|
||||
.get_quality_flag_valid = toyota_get_quality_flag_valid,
|
||||
|
||||
@@ -101,29 +101,6 @@ class TestToyotaSafetyBase(common.CarSafetyTest, common.LongitudinalAccelSafetyT
|
||||
tester_present = libsafety_py.make_CANPacket(0x750, 0, msg)
|
||||
self.assertEqual(should_tx and ecu_disabled and not stock_longitudinal, self._tx(tester_present))
|
||||
|
||||
def test_enhanced_bsm(self):
|
||||
# SP: enable/disable/poll left+right blind spot debug mode, sent to the radar diagnostic address
|
||||
valid_msgs = [
|
||||
b"\x41\x02\x10\x60\x00\x00\x00\x00", # enable left
|
||||
b"\x41\x02\x10\x01\x00\x00\x00\x00", # disable left
|
||||
b"\x41\x02\x21\x69\x00\x00\x00\x00", # poll left
|
||||
b"\x42\x02\x10\x60\x00\x00\x00\x00", # enable right
|
||||
b"\x42\x02\x10\x01\x00\x00\x00\x00", # disable right
|
||||
b"\x42\x02\x21\x69\x00\x00\x00\x00", # poll right
|
||||
]
|
||||
for msg in valid_msgs:
|
||||
pkt = libsafety_py.make_CANPacket(0x750, 0, msg)
|
||||
self.assertTrue(self._tx(pkt), msg.hex())
|
||||
|
||||
invalid_msgs = [
|
||||
b"\x41\x02\x10\x61\x00\x00\x00\x00", # wrong subfunction
|
||||
b"\x43\x02\x10\x60\x00\x00\x00\x00", # wrong sub-address (not left/right)
|
||||
b"\x41\x02\x10\x60\x01\x00\x00\x00", # non-zero trailing bytes
|
||||
]
|
||||
for msg in invalid_msgs:
|
||||
pkt = libsafety_py.make_CANPacket(0x750, 0, msg)
|
||||
self.assertFalse(self._tx(pkt), msg.hex())
|
||||
|
||||
def test_block_aeb(self, stock_longitudinal: bool = False):
|
||||
for controls_allowed in (True, False):
|
||||
for bad in (True, False):
|
||||
|
||||
@@ -14,7 +14,7 @@ from opendbc.car import structs
|
||||
from opendbc.car.can_definitions import CanRecvCallable, CanSendCallable
|
||||
from opendbc.car.hyundai.values import HyundaiFlags
|
||||
from opendbc.car.subaru.values import SubaruFlags
|
||||
from opendbc.car.toyota.values import RADAR_ACC_CAR, SECOC_CAR, TSS2_CAR, ToyotaSafetyFlags
|
||||
from opendbc.car.toyota.values import ToyotaSafetyFlags
|
||||
from opendbc.sunnypilot.car.hyundai.enable_radar_tracks import enable_radar_tracks as hyundai_enable_radar_tracks
|
||||
from opendbc.sunnypilot.car.hyundai.longitudinal.helpers import LongitudinalTuningType
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
@@ -157,9 +157,6 @@ def _initialize_toyota(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params
|
||||
if CP.brand == 'toyota':
|
||||
toyota_stock_long = int(params_dict.get("ToyotaEnforceStockLongitudinal", 0)) == 1
|
||||
toyota_stop_and_go_hack = int(params_dict.get("ToyotaStopAndGoHack", 0)) == 1
|
||||
toyota_tss2_long_tuning = int(params_dict.get("ToyotaTSS2Long", 0)) == 1
|
||||
toyota_enhanced_bsm = int(params_dict.get("ToyotaEnhancedBsm", 0)) == 1
|
||||
toyota_auto_brake_hold = int(params_dict.get("ToyotaAutoHold", 0)) == 1
|
||||
|
||||
if toyota_stock_long:
|
||||
CP_SP.flags |= ToyotaFlagsSP.STOCK_LONGITUDINAL.value
|
||||
@@ -169,12 +166,3 @@ def _initialize_toyota(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params
|
||||
|
||||
if toyota_stop_and_go_hack and CP.openpilotLongitudinalControl:
|
||||
CP_SP.flags |= ToyotaFlagsSP.STOP_AND_GO_HACK.value
|
||||
|
||||
if toyota_tss2_long_tuning:
|
||||
CP_SP.flags |= ToyotaFlagsSP.TSS2_LONG_TUNING.value
|
||||
|
||||
if toyota_enhanced_bsm and CP.carFingerprint in (TSS2_CAR - SECOC_CAR):
|
||||
CP_SP.flags |= ToyotaFlagsSP.SP_ENHANCED_BSM.value
|
||||
|
||||
if toyota_auto_brake_hold and CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR - SECOC_CAR):
|
||||
CP_SP.flags |= ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD.value
|
||||
|
||||
@@ -1,80 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.toyota import toyotacan
|
||||
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
|
||||
|
||||
GearShifter = structs.CarState.GearShifter
|
||||
|
||||
# frames of confirmed hold-eligible standstill required before engaging
|
||||
BRAKE_HOLD_ALLOWED_TIMER = 100
|
||||
|
||||
DISALLOWED_GEARS = (GearShifter.park, GearShifter.reverse)
|
||||
|
||||
# PRE_COLLISION_2 fields that go high when the camera's own PCS/AEB is genuinely intervening this
|
||||
# frame (PCSALM mirrors PRECOLLISION_ACTIVE; IBTRGR/PBATRGR/PREFILL/AVSTRGR/PBRTRGR/PPTRGR are its
|
||||
# actuation triggers - see create_pcs_commands for the same field set on the stock-DSU PCS path).
|
||||
# Deliberately over-inclusive: a false positive here just means we pass a quiescent frame through
|
||||
# instead of holding it, never the other way around, so err toward checking more fields, not fewer.
|
||||
PCS_TRIGGER_FIELDS = ("PCSALM", "IBTRGR", "PBATRGR", "PREFILL", "AVSTRGR", "PBRTRGR", "PPTRGR")
|
||||
|
||||
|
||||
def pcs_is_active(pre_collision_2: dict) -> bool:
|
||||
return any(pre_collision_2.get(field, 0) for field in PCS_TRIGGER_FIELDS) or pre_collision_2.get("DSS1GDRV", 0) != 0
|
||||
|
||||
|
||||
class AutoBrakeHold:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
@property
|
||||
def enabled(self):
|
||||
return bool(self.CP_SP.flags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD)
|
||||
|
||||
|
||||
# Auto Brake Hold (@AlexandreSato, @rav4kumar): holds the car at a stop with cruise off by
|
||||
# overriding PRE_COLLISION_2 - the only channel on this platform that can command the brake
|
||||
# independent of ACC engagement, since PCS/AEB is an always-on active safety system by design.
|
||||
# Yields to any genuine PCS activation this frame - the real message is only ever overridden while
|
||||
# it's quiescent - and releases for the rest of the current standstill episode on a brake press,
|
||||
# rather than for a single frame.
|
||||
class AutoBrakeHoldCarController(AutoBrakeHold):
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
super().__init__(CP, CP_SP)
|
||||
|
||||
self.active = False
|
||||
self._counter = 0
|
||||
self._released = False
|
||||
self._prev_brake_pressed = False
|
||||
|
||||
def update(self, CS: structs.CarState, frame: int, packer) -> list:
|
||||
relay_blocked = (CS.out.standstill and CS.out.cruiseState.available and not CS.out.cruiseState.enabled and
|
||||
not CS.out.gasPressed)
|
||||
hold_allowed = relay_blocked and CS.out.gearShifter not in DISALLOWED_GEARS
|
||||
|
||||
if hold_allowed:
|
||||
# only a fresh press releases hold - the press that caused the stop is already reflected in
|
||||
# _prev_brake_pressed by the time standstill is reached, so it doesn't count as a release
|
||||
if CS.out.brakePressed and not self._prev_brake_pressed:
|
||||
self._released = True
|
||||
self._counter += 1
|
||||
self.active = self._counter > BRAKE_HOLD_ALLOWED_TIMER and not self._released
|
||||
else:
|
||||
self._counter = 0
|
||||
self.active = False
|
||||
self._released = False
|
||||
|
||||
self._prev_brake_pressed = CS.out.brakePressed
|
||||
|
||||
can_sends = []
|
||||
if relay_blocked and frame % 2 == 0:
|
||||
override = self.active and not pcs_is_active(CS.pre_collision_2)
|
||||
can_sends.append(toyotacan.create_brake_hold_command(packer, frame, CS.pre_collision_2, override))
|
||||
|
||||
return can_sends
|
||||
@@ -1,126 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.can_definitions import CanData
|
||||
from opendbc.car.toyota import toyotacan
|
||||
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
|
||||
|
||||
LEFT_BLINDSPOT = b"\x41"
|
||||
RIGHT_BLINDSPOT = b"\x42"
|
||||
LEFT_SIDE = LEFT_BLINDSPOT[0]
|
||||
RIGHT_SIDE = RIGHT_BLINDSPOT[0]
|
||||
|
||||
# BLINDSPOTD1/D2 aren't real distances despite the DBC name: verified against 3 real routes, values
|
||||
# fall into 3 clean bands - idle (0), a small transitional-noise band (seen 1-31, rare: single-digit
|
||||
# occurrence counts, present during side/state transitions), and two real zone codes (~46-47 and
|
||||
# ~50-53, consistent across routes and cars - likely mirroring stock BSM's own ADJACENT/APPROACHING
|
||||
# split). A plain nonzero check lets the noise band through as false "occupied" hits; gate on the
|
||||
# real-zone floor instead - comfortably above every noise value seen, comfortably below neither zone code.
|
||||
BLINDSPOT_NOISE_FLOOR = 35
|
||||
|
||||
# DEBUG also carries a 1-bit BLINDSPOT flag (byte 4) that isn't read here. Checked against the same
|
||||
# route: stayed 0 across all frames, including every confirmed real detection - not a usable signal.
|
||||
|
||||
|
||||
class EnhancedBsm:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
@property
|
||||
def enabled(self):
|
||||
return bool(self.CP_SP.flags & ToyotaFlagsSP.SP_ENHANCED_BSM)
|
||||
|
||||
|
||||
class _BsmSideState:
|
||||
def __init__(self):
|
||||
self.blindspot = False
|
||||
self.counter = 0
|
||||
|
||||
def update(self, distance_1, distance_2):
|
||||
# every fresh matching-side reading directly reflects the current occupancy check - this is not
|
||||
# a latch. A drop to an idle/noise reading on this exact side clears it immediately, same as the
|
||||
# original. The counter is purely a silence timeout for when this side stops responding entirely,
|
||||
# not a "hold the last detection for a while" mechanism.
|
||||
self.blindspot = distance_1 > BLINDSPOT_NOISE_FLOOR or distance_2 > BLINDSPOT_NOISE_FLOOR
|
||||
self.counter = 100
|
||||
|
||||
def decay(self):
|
||||
self.counter = max(0, self.counter - 1)
|
||||
if self.counter == 0:
|
||||
self.blindspot = False
|
||||
|
||||
|
||||
# Enhanced BSM (@arne182, @rav4kumar)
|
||||
class EnhancedBsmCarState(EnhancedBsm):
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
super().__init__(CP, CP_SP)
|
||||
|
||||
self._sides = {LEFT_SIDE: _BsmSideState(), RIGHT_SIDE: _BsmSideState()}
|
||||
|
||||
def update(self, cp, frame: int) -> tuple[bool, bool]:
|
||||
# Let's keep all the commented out code for easy debug purposes in the future.
|
||||
distance_1 = cp.vl["DEBUG"].get('BLINDSPOTD1')
|
||||
distance_2 = cp.vl["DEBUG"].get('BLINDSPOTD2')
|
||||
side = cp.vl["DEBUG"].get('BLINDSPOTSIDE')
|
||||
|
||||
if all(val is not None for val in [distance_1, distance_2, side]) and side in self._sides:
|
||||
self._sides[side].update(distance_1, distance_2)
|
||||
|
||||
for side_state in self._sides.values():
|
||||
side_state.decay()
|
||||
|
||||
return self._sides[LEFT_SIDE].blindspot, self._sides[RIGHT_SIDE].blindspot
|
||||
|
||||
|
||||
class _BsmSideController:
|
||||
def __init__(self, addr_byte: bytes):
|
||||
self.addr_byte = addr_byte
|
||||
self.debug_enabled = False
|
||||
self.last_poll_frame = 0
|
||||
|
||||
def update(self, frame: int, poll_phase: int, e_bsm_rate: int, always_on: bool, vego_ok: bool) -> list[CanData]:
|
||||
can_sends = []
|
||||
|
||||
if not self.debug_enabled:
|
||||
if always_on or vego_ok: # eagle eye camera will stop working if bsm is switched on under 6m/s
|
||||
can_sends.append(toyotacan.create_set_bsm_debug_mode(self.addr_byte, True))
|
||||
self.debug_enabled = True
|
||||
self.last_poll_frame = frame # give the poll loop a fresh baseline so the stale-poll disable check below can't fire before the first real poll
|
||||
else:
|
||||
# no periodic re-assert: re-sending DiagnosticSessionControl(extendedSession) while already in that
|
||||
# session appears to make the ECU intermittently drop its own detection state (confirmed against a
|
||||
# real route - two reasserts landed inside an 8s window where the ECU went silent on that side).
|
||||
# send it once and leave it alone, matching the original, proven-stable behavior.
|
||||
if not always_on and frame - self.last_poll_frame > 50:
|
||||
can_sends.append(toyotacan.create_set_bsm_debug_mode(self.addr_byte, False))
|
||||
self.debug_enabled = False
|
||||
|
||||
if frame % e_bsm_rate == poll_phase:
|
||||
can_sends.append(toyotacan.create_bsm_polling_status(self.addr_byte))
|
||||
self.last_poll_frame = frame
|
||||
|
||||
return can_sends
|
||||
|
||||
|
||||
# Enhanced BSM (@arne182, @rav4kumar)
|
||||
class EnhancedBsmCarController(EnhancedBsm):
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
super().__init__(CP, CP_SP)
|
||||
|
||||
self._left = _BsmSideController(LEFT_BLINDSPOT)
|
||||
self._right = _BsmSideController(RIGHT_BLINDSPOT)
|
||||
|
||||
def update(self, CS: structs.CarState, frame: int, e_bsm_rate: int = 20, always_on: bool = True) -> list[CanData]:
|
||||
if frame <= 200:
|
||||
return []
|
||||
|
||||
vego_ok = CS.out.vEgo > 6
|
||||
can_sends = self._left.update(frame, 0, e_bsm_rate, always_on, vego_ok)
|
||||
can_sends += self._right.update(frame, e_bsm_rate // 2, e_bsm_rate, always_on, vego_ok)
|
||||
return can_sends
|
||||
@@ -1,231 +0,0 @@
|
||||
import unittest
|
||||
from unittest.mock import patch
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.sunnypilot.car.toyota.auto_brake_hold import AutoBrakeHoldCarController, BRAKE_HOLD_ALLOWED_TIMER, pcs_is_active
|
||||
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
|
||||
|
||||
GearShifter = structs.CarState.GearShifter
|
||||
|
||||
|
||||
def make_car_params_sp(enabled: bool = True) -> structs.CarParamsSP:
|
||||
cp_sp = structs.CarParamsSP()
|
||||
cp_sp.flags = ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD if enabled else 0
|
||||
return cp_sp
|
||||
|
||||
|
||||
class FakeCarState:
|
||||
def __init__(self, standstill=True, cruise_enabled=False, cruise_available=True, gas_pressed=False,
|
||||
gear=GearShifter.drive, brake_pressed=False, pre_collision_2=None):
|
||||
self.out = structs.CarState()
|
||||
self.out.standstill = standstill
|
||||
self.out.cruiseState.enabled = cruise_enabled
|
||||
self.out.cruiseState.available = cruise_available
|
||||
self.out.gasPressed = gas_pressed
|
||||
self.out.gearShifter = gear
|
||||
self.out.brakePressed = brake_pressed
|
||||
self.pre_collision_2 = pre_collision_2 if pre_collision_2 is not None else {}
|
||||
|
||||
|
||||
@patch("opendbc.sunnypilot.car.toyota.auto_brake_hold.toyotacan.create_brake_hold_command")
|
||||
class TestAutoBrakeHoldCarController(unittest.TestCase):
|
||||
def _make(self):
|
||||
return AutoBrakeHoldCarController(structs.CarParams(), make_car_params_sp())
|
||||
|
||||
def test_enabled_reflects_flag(self, mock_create):
|
||||
for value in range(256):
|
||||
with self.subTest(flags=value):
|
||||
cp_sp = structs.CarParamsSP()
|
||||
cp_sp.flags = value
|
||||
ctrl = AutoBrakeHoldCarController(structs.CarParams(), cp_sp)
|
||||
self.assertEqual(ctrl.enabled, bool(value & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD))
|
||||
|
||||
def test_does_not_engage_before_timer(self, mock_create):
|
||||
ctrl = self._make()
|
||||
cs = FakeCarState(brake_pressed=False)
|
||||
for i in range(BRAKE_HOLD_ALLOWED_TIMER):
|
||||
ctrl.update(cs, i, None)
|
||||
self.assertFalse(ctrl.active)
|
||||
|
||||
def test_engages_after_timer_once_brake_released(self, mock_create):
|
||||
ctrl = self._make()
|
||||
cs = FakeCarState(brake_pressed=False)
|
||||
for i in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
|
||||
ctrl.update(cs, i, None)
|
||||
self.assertTrue(ctrl.active)
|
||||
|
||||
def test_brake_still_down_from_the_stop_does_not_block_engagement(self, mock_create):
|
||||
# the driver's foot is normally still on the brake on the very frame standstill is reached -
|
||||
# that must not count as a "fresh press" release, or the feature could never engage
|
||||
ctrl = self._make()
|
||||
# decelerating into the stop with the brake held continuously
|
||||
cs = FakeCarState(standstill=False, brake_pressed=True)
|
||||
for i in range(30):
|
||||
ctrl.update(cs, i, None)
|
||||
# reaches standstill, foot stays down for a while, then lifts
|
||||
cs.out.standstill = True
|
||||
for i in range(30, 50):
|
||||
ctrl.update(cs, i, None)
|
||||
self.assertFalse(ctrl.active, "must not engage while the original stopping press is still held")
|
||||
cs.out.brakePressed = False
|
||||
for i in range(50, 50 + BRAKE_HOLD_ALLOWED_TIMER + 1):
|
||||
ctrl.update(cs, i, None)
|
||||
self.assertTrue(ctrl.active, "should engage once the stopping press is released and the timer elapses")
|
||||
|
||||
def test_fresh_brake_press_mid_hold_releases_and_stays_released(self, mock_create):
|
||||
ctrl = self._make()
|
||||
cs = FakeCarState(brake_pressed=False)
|
||||
frame = 0
|
||||
for _ in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
|
||||
ctrl.update(cs, frame, None)
|
||||
frame += 1
|
||||
self.assertTrue(ctrl.active)
|
||||
|
||||
cs.out.brakePressed = True
|
||||
ctrl.update(cs, frame, None)
|
||||
frame += 1
|
||||
self.assertFalse(ctrl.active, "a fresh press should release immediately")
|
||||
|
||||
for _ in range(20):
|
||||
ctrl.update(cs, frame, None)
|
||||
frame += 1
|
||||
self.assertFalse(ctrl.active, "must stay released for the rest of the episode while continuously held")
|
||||
|
||||
cs.out.brakePressed = False
|
||||
for _ in range(20):
|
||||
ctrl.update(cs, frame, None)
|
||||
frame += 1
|
||||
self.assertFalse(ctrl.active, "must stay released for the rest of the episode even after lifting off again")
|
||||
|
||||
def test_drive_off_and_restop_rearms(self, mock_create):
|
||||
ctrl = self._make()
|
||||
cs = FakeCarState(brake_pressed=False)
|
||||
frame = 0
|
||||
for _ in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
|
||||
ctrl.update(cs, frame, None)
|
||||
frame += 1
|
||||
cs.out.brakePressed = True
|
||||
ctrl.update(cs, frame, None)
|
||||
frame += 1
|
||||
self.assertFalse(ctrl.active)
|
||||
|
||||
# drives off - leaves the standstill episode entirely
|
||||
cs.out.standstill = False
|
||||
ctrl.update(cs, frame, None)
|
||||
frame += 1
|
||||
|
||||
# stops again
|
||||
cs.out.standstill = True
|
||||
cs.out.brakePressed = False
|
||||
for _ in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
|
||||
ctrl.update(cs, frame, None)
|
||||
frame += 1
|
||||
self.assertTrue(ctrl.active, "should re-arm and engage again at the next stop")
|
||||
|
||||
def test_cruise_engaged_blocks_hold(self, mock_create):
|
||||
ctrl = self._make()
|
||||
cs = FakeCarState(cruise_enabled=True, brake_pressed=False)
|
||||
for i in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
|
||||
ctrl.update(cs, i, None)
|
||||
self.assertFalse(ctrl.active, "must not hold while ACC is engaged - that's the point of the constraint")
|
||||
|
||||
def test_gas_pressed_blocks_hold(self, mock_create):
|
||||
ctrl = self._make()
|
||||
cs = FakeCarState(gas_pressed=True, brake_pressed=False)
|
||||
for i in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
|
||||
ctrl.update(cs, i, None)
|
||||
self.assertFalse(ctrl.active)
|
||||
|
||||
def test_park_and_reverse_block_hold(self, mock_create):
|
||||
for gear in (GearShifter.park, GearShifter.reverse):
|
||||
with self.subTest(gear=gear):
|
||||
ctrl = self._make()
|
||||
cs = FakeCarState(gear=gear, brake_pressed=False)
|
||||
for i in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
|
||||
ctrl.update(cs, i, None)
|
||||
self.assertFalse(ctrl.active)
|
||||
|
||||
def test_yields_to_live_pcs_without_dropping_active_state(self, mock_create):
|
||||
ctrl = self._make()
|
||||
cs = FakeCarState(brake_pressed=False)
|
||||
frame = 0
|
||||
for _ in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
|
||||
ctrl.update(cs, frame, None)
|
||||
frame += 1
|
||||
self.assertTrue(ctrl.active)
|
||||
mock_create.reset_mock()
|
||||
|
||||
# a real PCS event shows up on the live signal
|
||||
cs.pre_collision_2 = {"PCSALM": 1}
|
||||
# advance to the next even frame the message is actually built on
|
||||
while frame % 2 != 0:
|
||||
frame += 1
|
||||
ctrl.update(cs, frame, None)
|
||||
override_arg = mock_create.call_args.args[-1]
|
||||
self.assertTrue(ctrl.active, "internal hold state should not be cleared by a live PCS event")
|
||||
self.assertFalse(override_arg, "must not override PRE_COLLISION_2 while PCS is genuinely active")
|
||||
|
||||
# once PCS goes quiet again, override resumes on our own signal, not stale PCS state
|
||||
frame += 2
|
||||
cs.pre_collision_2 = {}
|
||||
ctrl.update(cs, frame, None)
|
||||
override_arg = mock_create.call_args.args[-1]
|
||||
self.assertTrue(override_arg)
|
||||
|
||||
def test_message_only_built_every_other_frame(self, mock_create):
|
||||
ctrl = self._make()
|
||||
cs = FakeCarState(brake_pressed=False)
|
||||
for i in range(10):
|
||||
ctrl.update(cs, i, None)
|
||||
self.assertEqual(mock_create.call_count, 5)
|
||||
|
||||
def test_no_message_sent_outside_relay_blocked_window(self, mock_create):
|
||||
# outside relay_blocked, toyota_fwd_hook lets the real PRE_COLLISION_2 relay through on its own -
|
||||
# our passthrough copy would just be redundant traffic panda rejects, so it must not be sent at all
|
||||
cases = [
|
||||
dict(cruise_enabled=True),
|
||||
dict(gas_pressed=True),
|
||||
dict(cruise_available=False),
|
||||
]
|
||||
for kwargs in cases:
|
||||
with self.subTest(kwargs=kwargs):
|
||||
ctrl = self._make()
|
||||
cs = FakeCarState(brake_pressed=False, **kwargs)
|
||||
for i in range(10):
|
||||
ctrl.update(cs, i, None)
|
||||
mock_create.assert_not_called()
|
||||
|
||||
def test_message_still_sent_in_park_or_reverse(self, mock_create):
|
||||
# relay_blocked has no gear check, matching toyota_fwd_hook - park/reverse must not open a gap
|
||||
# in the PRE_COLLISION_2 relay even though hold can never engage there (this was a real
|
||||
# regression: gating the send on hold_allowed's gear check left bus 0 with nothing at this
|
||||
# address while parked, which is what tripped a genuine PCS dash fault on-road)
|
||||
for gear in (GearShifter.park, GearShifter.reverse):
|
||||
with self.subTest(gear=gear):
|
||||
ctrl = self._make()
|
||||
cs = FakeCarState(gear=gear, brake_pressed=False)
|
||||
for i in range(BRAKE_HOLD_ALLOWED_TIMER + 1):
|
||||
ctrl.update(cs, i, None)
|
||||
self.assertFalse(ctrl.active, "must never actually hold in park/reverse")
|
||||
self.assertGreater(mock_create.call_count, 0, "must still relay PRE_COLLISION_2 in park/reverse")
|
||||
override_arg = mock_create.call_args.args[-1]
|
||||
self.assertFalse(override_arg, "never override while gear disallows an actual hold")
|
||||
|
||||
|
||||
class TestPcsIsActive(unittest.TestCase):
|
||||
def test_all_zero_is_not_active(self):
|
||||
self.assertFalse(pcs_is_active({}))
|
||||
self.assertFalse(pcs_is_active({"PCSALM": 0, "DSS1GDRV": 0}))
|
||||
|
||||
def test_any_trigger_field_is_active(self):
|
||||
for field in ("PCSALM", "IBTRGR", "PBATRGR", "PREFILL", "AVSTRGR", "PBRTRGR", "PPTRGR"):
|
||||
with self.subTest(field=field):
|
||||
self.assertTrue(pcs_is_active({field: 1}))
|
||||
|
||||
def test_nonzero_force_signal_is_active(self):
|
||||
self.assertTrue(pcs_is_active({"DSS1GDRV": -5}))
|
||||
self.assertFalse(pcs_is_active({"DSS1GDRV": 0}))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -1,15 +0,0 @@
|
||||
import unittest
|
||||
|
||||
from opendbc.car.toyota.carstate import AccelPersonality, get_accel_personality
|
||||
|
||||
|
||||
class TestDriveMode(unittest.TestCase):
|
||||
def test_acceleration_profile_mapping(self):
|
||||
self.assertEqual(get_accel_personality(0, 0), AccelPersonality.normal)
|
||||
self.assertEqual(get_accel_personality(0, 1), AccelPersonality.eco)
|
||||
self.assertEqual(get_accel_personality(1, 0), AccelPersonality.sport)
|
||||
self.assertEqual(get_accel_personality(1, 1), AccelPersonality.sport)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -1,57 +0,0 @@
|
||||
import unittest
|
||||
from types import SimpleNamespace
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.toyota.carcontroller import get_long_tune
|
||||
from opendbc.car.toyota.values import CAR, ToyotaFlags
|
||||
from opendbc.sunnypilot.car.interfaces import _initialize_toyota
|
||||
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
|
||||
|
||||
|
||||
def build_params(*, tss2=True):
|
||||
flags = ToyotaFlags.TSS2.value if tss2 else 0
|
||||
CP = structs.CarParams(
|
||||
brand="toyota",
|
||||
carFingerprint=str(CAR.TOYOTA_COROLLA_TSS2),
|
||||
flags=flags,
|
||||
)
|
||||
return CP, structs.CarParamsSP()
|
||||
|
||||
|
||||
class TestTss2LongTuning(unittest.TestCase):
|
||||
def test_param_handoff(self):
|
||||
for enabled in (False, True):
|
||||
with self.subTest(enabled=enabled):
|
||||
CP, CP_SP = build_params()
|
||||
_initialize_toyota(CP, CP_SP, {"ToyotaTSS2Long": int(enabled)})
|
||||
self.assertEqual(bool(CP_SP.flags & ToyotaFlagsSP.TSS2_LONG_TUNING), enabled)
|
||||
|
||||
def test_tss2_tune_selection(self):
|
||||
controller_params = SimpleNamespace(ACCEL_MAX=2.0, ACCEL_MIN=-3.5)
|
||||
CP, CP_SP = build_params()
|
||||
|
||||
stock_tune = get_long_tune(CP, CP_SP, controller_params)
|
||||
stock_tune.speed = 2.0
|
||||
self.assertEqual(stock_tune.k_i, 0.5)
|
||||
stock_tune.speed = 5.0
|
||||
self.assertEqual(stock_tune.k_i, 0.25)
|
||||
|
||||
CP_SP.flags |= ToyotaFlagsSP.TSS2_LONG_TUNING.value
|
||||
custom_tune = get_long_tune(CP, CP_SP, controller_params)
|
||||
custom_tune.speed = 0.0
|
||||
self.assertEqual(custom_tune.k_i, 0.30)
|
||||
custom_tune.speed = 5.0
|
||||
self.assertEqual(custom_tune.k_i, 0.28)
|
||||
|
||||
def test_non_tss2_ignores_custom_tune_flag(self):
|
||||
controller_params = SimpleNamespace(ACCEL_MAX=2.0, ACCEL_MIN=-3.5)
|
||||
CP, CP_SP = build_params(tss2=False)
|
||||
CP_SP.flags |= ToyotaFlagsSP.TSS2_LONG_TUNING.value
|
||||
|
||||
standard_tune = get_long_tune(CP, CP_SP, controller_params)
|
||||
standard_tune.speed = 0.0
|
||||
self.assertEqual(standard_tune.k_i, 3.6)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -14,10 +14,6 @@ class ToyotaFlagsSP(IntFlag):
|
||||
ZSS = 4
|
||||
STOCK_LONGITUDINAL = 8
|
||||
STOP_AND_GO_HACK = 16
|
||||
SP_ENHANCED_BSM = 32
|
||||
SP_NEED_DEBUG_BSM = 64
|
||||
SP_AUTO_BRAKE_HOLD = 128
|
||||
TSS2_LONG_TUNING = 512
|
||||
|
||||
|
||||
class ToyotaSafetyFlagsSP:
|
||||
|
||||
@@ -203,7 +203,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
aTarget @5 :Float32;
|
||||
events @6 :List(OnroadEventSP.Event);
|
||||
e2eAlerts @7 :E2eAlerts;
|
||||
accelController @8 :AccelController;
|
||||
|
||||
struct DynamicExperimentalControl {
|
||||
state @0 :DynamicExperimentalControlState;
|
||||
@@ -306,18 +305,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
greenLightAlert @0 :Bool;
|
||||
leadDepartAlert @1 :Bool;
|
||||
}
|
||||
|
||||
struct AccelController {
|
||||
enabled @0 :Bool;
|
||||
active @1 :Bool;
|
||||
profile @2 :Profile;
|
||||
reserved3 @3 :Void;
|
||||
enum Profile {
|
||||
eco @0;
|
||||
normal @1;
|
||||
sport @2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct OnroadEventSP @0xda96579883444c35 {
|
||||
|
||||
@@ -1892,36 +1892,34 @@ const ::capnp::_::RawSchema s_e60821c0505ad473 = {
|
||||
4, 12, i_e60821c0505ad473, nullptr, nullptr, { &s_e60821c0505ad473, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
};
|
||||
#endif // !CAPNP_LITE
|
||||
static const ::capnp::_::AlignedData<192> b_f35cc4560bbf6ec2 = {
|
||||
static const ::capnp::_::AlignedData<172> b_f35cc4560bbf6ec2 = {
|
||||
{ 0, 0, 0, 0, 5, 0, 6, 0,
|
||||
194, 110, 191, 11, 86, 196, 92, 243,
|
||||
13, 0, 0, 0, 1, 0, 2, 0,
|
||||
89, 10, 85, 29, 102, 186, 38, 181,
|
||||
6, 0, 7, 0, 0, 0, 0, 0,
|
||||
5, 0, 7, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
21, 0, 0, 0, 2, 1, 0, 0,
|
||||
33, 0, 0, 0, 103, 0, 0, 0,
|
||||
33, 0, 0, 0, 87, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
141, 0, 0, 0, 255, 1, 0, 0,
|
||||
125, 0, 0, 0, 199, 1, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
99, 117, 115, 116, 111, 109, 46, 99,
|
||||
97, 112, 110, 112, 58, 76, 111, 110,
|
||||
103, 105, 116, 117, 100, 105, 110, 97,
|
||||
108, 80, 108, 97, 110, 83, 80, 0,
|
||||
24, 0, 0, 0, 1, 0, 1, 0,
|
||||
20, 0, 0, 0, 1, 0, 1, 0,
|
||||
119, 191, 14, 16, 174, 91, 106, 188,
|
||||
41, 0, 0, 0, 218, 0, 0, 0,
|
||||
33, 0, 0, 0, 218, 0, 0, 0,
|
||||
163, 23, 92, 92, 71, 153, 141, 193,
|
||||
49, 0, 0, 0, 154, 0, 0, 0,
|
||||
41, 0, 0, 0, 154, 0, 0, 0,
|
||||
187, 34, 168, 255, 81, 184, 158, 154,
|
||||
53, 0, 0, 0, 90, 0, 0, 0,
|
||||
45, 0, 0, 0, 90, 0, 0, 0,
|
||||
196, 110, 217, 86, 5, 68, 71, 173,
|
||||
53, 0, 0, 0, 186, 0, 0, 0,
|
||||
45, 0, 0, 0, 186, 0, 0, 0,
|
||||
254, 40, 174, 34, 232, 188, 103, 165,
|
||||
57, 0, 0, 0, 82, 0, 0, 0,
|
||||
161, 196, 243, 6, 208, 214, 220, 136,
|
||||
57, 0, 0, 0, 130, 0, 0, 0,
|
||||
49, 0, 0, 0, 82, 0, 0, 0,
|
||||
68, 121, 110, 97, 109, 105, 99, 69,
|
||||
120, 112, 101, 114, 105, 109, 101, 110,
|
||||
116, 97, 108, 67, 111, 110, 116, 114,
|
||||
@@ -1936,72 +1934,63 @@ static const ::capnp::_::AlignedData<192> b_f35cc4560bbf6ec2 = {
|
||||
83, 111, 117, 114, 99, 101, 0, 0,
|
||||
69, 50, 101, 65, 108, 101, 114, 116,
|
||||
115, 0, 0, 0, 0, 0, 0, 0,
|
||||
65, 99, 99, 101, 108, 67, 111, 110,
|
||||
116, 114, 111, 108, 108, 101, 114, 0,
|
||||
36, 0, 0, 0, 3, 0, 4, 0,
|
||||
32, 0, 0, 0, 3, 0, 4, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 1, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
237, 0, 0, 0, 34, 0, 0, 0,
|
||||
209, 0, 0, 0, 34, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
232, 0, 0, 0, 3, 0, 1, 0,
|
||||
244, 0, 0, 0, 2, 0, 1, 0,
|
||||
204, 0, 0, 0, 3, 0, 1, 0,
|
||||
216, 0, 0, 0, 2, 0, 1, 0,
|
||||
1, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 1, 0, 1, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
241, 0, 0, 0, 186, 0, 0, 0,
|
||||
213, 0, 0, 0, 186, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
244, 0, 0, 0, 3, 0, 1, 0,
|
||||
0, 1, 0, 0, 2, 0, 1, 0,
|
||||
216, 0, 0, 0, 3, 0, 1, 0,
|
||||
228, 0, 0, 0, 2, 0, 1, 0,
|
||||
2, 0, 0, 0, 1, 0, 0, 0,
|
||||
0, 0, 1, 0, 2, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
253, 0, 0, 0, 154, 0, 0, 0,
|
||||
225, 0, 0, 0, 154, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 1, 0, 0, 3, 0, 1, 0,
|
||||
12, 1, 0, 0, 2, 0, 1, 0,
|
||||
228, 0, 0, 0, 3, 0, 1, 0,
|
||||
240, 0, 0, 0, 2, 0, 1, 0,
|
||||
3, 0, 0, 0, 2, 0, 0, 0,
|
||||
0, 0, 1, 0, 3, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
9, 1, 0, 0, 90, 0, 0, 0,
|
||||
237, 0, 0, 0, 90, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
8, 1, 0, 0, 3, 0, 1, 0,
|
||||
20, 1, 0, 0, 2, 0, 1, 0,
|
||||
236, 0, 0, 0, 3, 0, 1, 0,
|
||||
248, 0, 0, 0, 2, 0, 1, 0,
|
||||
4, 0, 0, 0, 1, 0, 0, 0,
|
||||
0, 0, 1, 0, 4, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
17, 1, 0, 0, 66, 0, 0, 0,
|
||||
245, 0, 0, 0, 66, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
12, 1, 0, 0, 3, 0, 1, 0,
|
||||
24, 1, 0, 0, 2, 0, 1, 0,
|
||||
240, 0, 0, 0, 3, 0, 1, 0,
|
||||
252, 0, 0, 0, 2, 0, 1, 0,
|
||||
5, 0, 0, 0, 2, 0, 0, 0,
|
||||
0, 0, 1, 0, 5, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
21, 1, 0, 0, 66, 0, 0, 0,
|
||||
249, 0, 0, 0, 66, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
16, 1, 0, 0, 3, 0, 1, 0,
|
||||
28, 1, 0, 0, 2, 0, 1, 0,
|
||||
244, 0, 0, 0, 3, 0, 1, 0,
|
||||
0, 1, 0, 0, 2, 0, 1, 0,
|
||||
6, 0, 0, 0, 3, 0, 0, 0,
|
||||
0, 0, 1, 0, 6, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
25, 1, 0, 0, 58, 0, 0, 0,
|
||||
253, 0, 0, 0, 58, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
20, 1, 0, 0, 3, 0, 1, 0,
|
||||
48, 1, 0, 0, 2, 0, 1, 0,
|
||||
248, 0, 0, 0, 3, 0, 1, 0,
|
||||
20, 1, 0, 0, 2, 0, 1, 0,
|
||||
7, 0, 0, 0, 4, 0, 0, 0,
|
||||
0, 0, 1, 0, 7, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
45, 1, 0, 0, 82, 0, 0, 0,
|
||||
17, 1, 0, 0, 82, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
44, 1, 0, 0, 3, 0, 1, 0,
|
||||
56, 1, 0, 0, 2, 0, 1, 0,
|
||||
8, 0, 0, 0, 5, 0, 0, 0,
|
||||
0, 0, 1, 0, 8, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
53, 1, 0, 0, 130, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
52, 1, 0, 0, 3, 0, 1, 0,
|
||||
64, 1, 0, 0, 2, 0, 1, 0,
|
||||
16, 1, 0, 0, 3, 0, 1, 0,
|
||||
28, 1, 0, 0, 2, 0, 1, 0,
|
||||
100, 101, 99, 0, 0, 0, 0, 0,
|
||||
16, 0, 0, 0, 0, 0, 0, 0,
|
||||
119, 191, 14, 16, 174, 91, 106, 188,
|
||||
@@ -2073,15 +2062,6 @@ static const ::capnp::_::AlignedData<192> b_f35cc4560bbf6ec2 = {
|
||||
254, 40, 174, 34, 232, 188, 103, 165,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
16, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
97, 99, 99, 101, 108, 67, 111, 110,
|
||||
116, 114, 111, 108, 108, 101, 114, 0,
|
||||
16, 0, 0, 0, 0, 0, 0, 0,
|
||||
161, 196, 243, 6, 208, 214, 220, 136,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
16, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, }
|
||||
@@ -2089,7 +2069,6 @@ static const ::capnp::_::AlignedData<192> b_f35cc4560bbf6ec2 = {
|
||||
::capnp::word const* const bp_f35cc4560bbf6ec2 = b_f35cc4560bbf6ec2.words;
|
||||
#if !CAPNP_LITE
|
||||
static const ::capnp::_::RawSchema* const d_f35cc4560bbf6ec2[] = {
|
||||
&s_88dcd6d006f3c4a1,
|
||||
&s_9a9eb851ffa822bb,
|
||||
&s_a567bce822ae28fe,
|
||||
&s_ad47440556d96ec4,
|
||||
@@ -2097,11 +2076,11 @@ static const ::capnp::_::RawSchema* const d_f35cc4560bbf6ec2[] = {
|
||||
&s_c18d99475c5c17a3,
|
||||
&s_f6e831752fcdf793,
|
||||
};
|
||||
static const uint16_t m_f35cc4560bbf6ec2[] = {5, 8, 0, 7, 6, 1, 2, 3, 4};
|
||||
static const uint16_t i_f35cc4560bbf6ec2[] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
|
||||
static const uint16_t m_f35cc4560bbf6ec2[] = {5, 0, 7, 6, 1, 2, 3, 4};
|
||||
static const uint16_t i_f35cc4560bbf6ec2[] = {0, 1, 2, 3, 4, 5, 6, 7};
|
||||
const ::capnp::_::RawSchema s_f35cc4560bbf6ec2 = {
|
||||
0xf35cc4560bbf6ec2, b_f35cc4560bbf6ec2.words, 192, d_f35cc4560bbf6ec2, m_f35cc4560bbf6ec2,
|
||||
7, 9, i_f35cc4560bbf6ec2, nullptr, nullptr, { &s_f35cc4560bbf6ec2, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
0xf35cc4560bbf6ec2, b_f35cc4560bbf6ec2.words, 172, d_f35cc4560bbf6ec2, m_f35cc4560bbf6ec2,
|
||||
6, 8, i_f35cc4560bbf6ec2, nullptr, nullptr, { &s_f35cc4560bbf6ec2, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
};
|
||||
#endif // !CAPNP_LITE
|
||||
static const ::capnp::_::AlignedData<73> b_bc6a5bae100ebf77 = {
|
||||
@@ -3273,148 +3252,6 @@ const ::capnp::_::RawSchema s_a567bce822ae28fe = {
|
||||
0, 2, i_a567bce822ae28fe, nullptr, nullptr, { &s_a567bce822ae28fe, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
};
|
||||
#endif // !CAPNP_LITE
|
||||
static const ::capnp::_::AlignedData<84> b_88dcd6d006f3c4a1 = {
|
||||
{ 0, 0, 0, 0, 5, 0, 6, 0,
|
||||
161, 196, 243, 6, 208, 214, 220, 136,
|
||||
32, 0, 0, 0, 1, 0, 1, 0,
|
||||
194, 110, 191, 11, 86, 196, 92, 243,
|
||||
0, 0, 7, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
21, 0, 0, 0, 130, 1, 0, 0,
|
||||
41, 0, 0, 0, 23, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
49, 0, 0, 0, 231, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
99, 117, 115, 116, 111, 109, 46, 99,
|
||||
97, 112, 110, 112, 58, 76, 111, 110,
|
||||
103, 105, 116, 117, 100, 105, 110, 97,
|
||||
108, 80, 108, 97, 110, 83, 80, 46,
|
||||
65, 99, 99, 101, 108, 67, 111, 110,
|
||||
116, 114, 111, 108, 108, 101, 114, 0,
|
||||
4, 0, 0, 0, 1, 0, 1, 0,
|
||||
166, 80, 70, 176, 68, 149, 159, 140,
|
||||
1, 0, 0, 0, 66, 0, 0, 0,
|
||||
80, 114, 111, 102, 105, 108, 101, 0,
|
||||
16, 0, 0, 0, 3, 0, 4, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 1, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
97, 0, 0, 0, 66, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
92, 0, 0, 0, 3, 0, 1, 0,
|
||||
104, 0, 0, 0, 2, 0, 1, 0,
|
||||
1, 0, 0, 0, 1, 0, 0, 0,
|
||||
0, 0, 1, 0, 1, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
101, 0, 0, 0, 58, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
96, 0, 0, 0, 3, 0, 1, 0,
|
||||
108, 0, 0, 0, 2, 0, 1, 0,
|
||||
2, 0, 0, 0, 1, 0, 0, 0,
|
||||
0, 0, 1, 0, 2, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
105, 0, 0, 0, 66, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
100, 0, 0, 0, 3, 0, 1, 0,
|
||||
112, 0, 0, 0, 2, 0, 1, 0,
|
||||
3, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 1, 0, 3, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
109, 0, 0, 0, 82, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
108, 0, 0, 0, 3, 0, 1, 0,
|
||||
120, 0, 0, 0, 2, 0, 1, 0,
|
||||
101, 110, 97, 98, 108, 101, 100, 0,
|
||||
1, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
1, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
97, 99, 116, 105, 118, 101, 0, 0,
|
||||
1, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
1, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
112, 114, 111, 102, 105, 108, 101, 0,
|
||||
15, 0, 0, 0, 0, 0, 0, 0,
|
||||
166, 80, 70, 176, 68, 149, 159, 140,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
15, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
114, 101, 115, 101, 114, 118, 101, 100,
|
||||
51, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, }
|
||||
};
|
||||
::capnp::word const* const bp_88dcd6d006f3c4a1 = b_88dcd6d006f3c4a1.words;
|
||||
#if !CAPNP_LITE
|
||||
static const ::capnp::_::RawSchema* const d_88dcd6d006f3c4a1[] = {
|
||||
&s_8c9f9544b04650a6,
|
||||
};
|
||||
static const uint16_t m_88dcd6d006f3c4a1[] = {1, 0, 2, 3};
|
||||
static const uint16_t i_88dcd6d006f3c4a1[] = {0, 1, 2, 3};
|
||||
const ::capnp::_::RawSchema s_88dcd6d006f3c4a1 = {
|
||||
0x88dcd6d006f3c4a1, b_88dcd6d006f3c4a1.words, 84, d_88dcd6d006f3c4a1, m_88dcd6d006f3c4a1,
|
||||
1, 4, i_88dcd6d006f3c4a1, nullptr, nullptr, { &s_88dcd6d006f3c4a1, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
};
|
||||
#endif // !CAPNP_LITE
|
||||
static const ::capnp::_::AlignedData<33> b_8c9f9544b04650a6 = {
|
||||
{ 0, 0, 0, 0, 5, 0, 6, 0,
|
||||
166, 80, 70, 176, 68, 149, 159, 140,
|
||||
48, 0, 0, 0, 2, 0, 0, 0,
|
||||
161, 196, 243, 6, 208, 214, 220, 136,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
21, 0, 0, 0, 194, 1, 0, 0,
|
||||
45, 0, 0, 0, 7, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
41, 0, 0, 0, 79, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
99, 117, 115, 116, 111, 109, 46, 99,
|
||||
97, 112, 110, 112, 58, 76, 111, 110,
|
||||
103, 105, 116, 117, 100, 105, 110, 97,
|
||||
108, 80, 108, 97, 110, 83, 80, 46,
|
||||
65, 99, 99, 101, 108, 67, 111, 110,
|
||||
116, 114, 111, 108, 108, 101, 114, 46,
|
||||
80, 114, 111, 102, 105, 108, 101, 0,
|
||||
0, 0, 0, 0, 1, 0, 1, 0,
|
||||
12, 0, 0, 0, 1, 0, 2, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
29, 0, 0, 0, 34, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
1, 0, 0, 0, 0, 0, 0, 0,
|
||||
21, 0, 0, 0, 58, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
2, 0, 0, 0, 0, 0, 0, 0,
|
||||
13, 0, 0, 0, 50, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
101, 99, 111, 0, 0, 0, 0, 0,
|
||||
110, 111, 114, 109, 97, 108, 0, 0,
|
||||
115, 112, 111, 114, 116, 0, 0, 0, }
|
||||
};
|
||||
::capnp::word const* const bp_8c9f9544b04650a6 = b_8c9f9544b04650a6.words;
|
||||
#if !CAPNP_LITE
|
||||
static const uint16_t m_8c9f9544b04650a6[] = {0, 1, 2};
|
||||
const ::capnp::_::RawSchema s_8c9f9544b04650a6 = {
|
||||
0x8c9f9544b04650a6, b_8c9f9544b04650a6.words, 33, nullptr, m_8c9f9544b04650a6,
|
||||
0, 3, nullptr, nullptr, nullptr, { &s_8c9f9544b04650a6, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
};
|
||||
#endif // !CAPNP_LITE
|
||||
CAPNP_DEFINE_ENUM(Profile_8c9f9544b04650a6, 8c9f9544b04650a6);
|
||||
static const ::capnp::_::AlignedData<44> b_da96579883444c35 = {
|
||||
{ 0, 0, 0, 0, 5, 0, 6, 0,
|
||||
53, 76, 68, 131, 152, 87, 150, 218,
|
||||
@@ -5788,18 +5625,6 @@ constexpr ::capnp::_::RawSchema const* LongitudinalPlanSP::E2eAlerts::_capnpPriv
|
||||
#endif // !CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL
|
||||
#endif // !CAPNP_LITE
|
||||
|
||||
// LongitudinalPlanSP::AccelController
|
||||
#if CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL
|
||||
constexpr uint16_t LongitudinalPlanSP::AccelController::_capnpPrivate::dataWordSize;
|
||||
constexpr uint16_t LongitudinalPlanSP::AccelController::_capnpPrivate::pointerCount;
|
||||
#endif // !CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL
|
||||
#if !CAPNP_LITE
|
||||
#if CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL
|
||||
constexpr ::capnp::Kind LongitudinalPlanSP::AccelController::_capnpPrivate::kind;
|
||||
constexpr ::capnp::_::RawSchema const* LongitudinalPlanSP::AccelController::_capnpPrivate::schema;
|
||||
#endif // !CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL
|
||||
#endif // !CAPNP_LITE
|
||||
|
||||
// OnroadEventSP
|
||||
#if CAPNP_NEED_REDUNDANT_CONSTEXPR_DECL
|
||||
constexpr uint16_t OnroadEventSP::_capnpPrivate::dataWordSize;
|
||||
|
||||
@@ -177,14 +177,6 @@ enum class LongitudinalPlanSource_ad47440556d96ec4: uint16_t {
|
||||
};
|
||||
CAPNP_DECLARE_ENUM(LongitudinalPlanSource, ad47440556d96ec4);
|
||||
CAPNP_DECLARE_SCHEMA(a567bce822ae28fe);
|
||||
CAPNP_DECLARE_SCHEMA(88dcd6d006f3c4a1);
|
||||
CAPNP_DECLARE_SCHEMA(8c9f9544b04650a6);
|
||||
enum class Profile_8c9f9544b04650a6: uint16_t {
|
||||
ECO,
|
||||
NORMAL,
|
||||
SPORT,
|
||||
};
|
||||
CAPNP_DECLARE_ENUM(Profile, 8c9f9544b04650a6);
|
||||
CAPNP_DECLARE_SCHEMA(da96579883444c35);
|
||||
CAPNP_DECLARE_SCHEMA(f6e831752fcdf793);
|
||||
CAPNP_DECLARE_SCHEMA(b8007ed8a646b5e6);
|
||||
@@ -483,10 +475,9 @@ struct LongitudinalPlanSP {
|
||||
typedef ::capnp::schemas::LongitudinalPlanSource_ad47440556d96ec4 LongitudinalPlanSource;
|
||||
|
||||
struct E2eAlerts;
|
||||
struct AccelController;
|
||||
|
||||
struct _capnpPrivate {
|
||||
CAPNP_DECLARE_STRUCT_HEADER(f35cc4560bbf6ec2, 2, 6)
|
||||
CAPNP_DECLARE_STRUCT_HEADER(f35cc4560bbf6ec2, 2, 5)
|
||||
#if !CAPNP_LITE
|
||||
static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; }
|
||||
#endif // !CAPNP_LITE
|
||||
@@ -627,23 +618,6 @@ struct LongitudinalPlanSP::E2eAlerts {
|
||||
};
|
||||
};
|
||||
|
||||
struct LongitudinalPlanSP::AccelController {
|
||||
AccelController() = delete;
|
||||
|
||||
class Reader;
|
||||
class Builder;
|
||||
class Pipeline;
|
||||
typedef ::capnp::schemas::Profile_8c9f9544b04650a6 Profile;
|
||||
|
||||
|
||||
struct _capnpPrivate {
|
||||
CAPNP_DECLARE_STRUCT_HEADER(88dcd6d006f3c4a1, 1, 0)
|
||||
#if !CAPNP_LITE
|
||||
static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; }
|
||||
#endif // !CAPNP_LITE
|
||||
};
|
||||
};
|
||||
|
||||
struct OnroadEventSP {
|
||||
OnroadEventSP() = delete;
|
||||
|
||||
@@ -2324,9 +2298,6 @@ public:
|
||||
inline bool hasE2eAlerts() const;
|
||||
inline ::cereal::LongitudinalPlanSP::E2eAlerts::Reader getE2eAlerts() const;
|
||||
|
||||
inline bool hasAccelController() const;
|
||||
inline ::cereal::LongitudinalPlanSP::AccelController::Reader getAccelController() const;
|
||||
|
||||
private:
|
||||
::capnp::_::StructReader _reader;
|
||||
template <typename, ::capnp::Kind>
|
||||
@@ -2399,13 +2370,6 @@ public:
|
||||
inline void adoptE2eAlerts(::capnp::Orphan< ::cereal::LongitudinalPlanSP::E2eAlerts>&& value);
|
||||
inline ::capnp::Orphan< ::cereal::LongitudinalPlanSP::E2eAlerts> disownE2eAlerts();
|
||||
|
||||
inline bool hasAccelController();
|
||||
inline ::cereal::LongitudinalPlanSP::AccelController::Builder getAccelController();
|
||||
inline void setAccelController( ::cereal::LongitudinalPlanSP::AccelController::Reader value);
|
||||
inline ::cereal::LongitudinalPlanSP::AccelController::Builder initAccelController();
|
||||
inline void adoptAccelController(::capnp::Orphan< ::cereal::LongitudinalPlanSP::AccelController>&& value);
|
||||
inline ::capnp::Orphan< ::cereal::LongitudinalPlanSP::AccelController> disownAccelController();
|
||||
|
||||
private:
|
||||
::capnp::_::StructBuilder _builder;
|
||||
template <typename, ::capnp::Kind>
|
||||
@@ -2428,7 +2392,6 @@ public:
|
||||
inline ::cereal::LongitudinalPlanSP::SmartCruiseControl::Pipeline getSmartCruiseControl();
|
||||
inline ::cereal::LongitudinalPlanSP::SpeedLimit::Pipeline getSpeedLimit();
|
||||
inline ::cereal::LongitudinalPlanSP::E2eAlerts::Pipeline getE2eAlerts();
|
||||
inline ::cereal::LongitudinalPlanSP::AccelController::Pipeline getAccelController();
|
||||
private:
|
||||
::capnp::AnyPointer::Pipeline _typeless;
|
||||
friend class ::capnp::PipelineHook;
|
||||
@@ -3204,97 +3167,6 @@ private:
|
||||
};
|
||||
#endif // !CAPNP_LITE
|
||||
|
||||
class LongitudinalPlanSP::AccelController::Reader {
|
||||
public:
|
||||
typedef AccelController Reads;
|
||||
|
||||
Reader() = default;
|
||||
inline explicit Reader(::capnp::_::StructReader base): _reader(base) {}
|
||||
|
||||
inline ::capnp::MessageSize totalSize() const {
|
||||
return _reader.totalSize().asPublic();
|
||||
}
|
||||
|
||||
#if !CAPNP_LITE
|
||||
inline ::kj::StringTree toString() const {
|
||||
return ::capnp::_::structString(_reader, *_capnpPrivate::brand());
|
||||
}
|
||||
#endif // !CAPNP_LITE
|
||||
|
||||
inline bool getEnabled() const;
|
||||
|
||||
inline bool getActive() const;
|
||||
|
||||
inline ::cereal::LongitudinalPlanSP::AccelController::Profile getProfile() const;
|
||||
|
||||
inline ::capnp::Void getReserved3() const;
|
||||
|
||||
private:
|
||||
::capnp::_::StructReader _reader;
|
||||
template <typename, ::capnp::Kind>
|
||||
friend struct ::capnp::ToDynamic_;
|
||||
template <typename, ::capnp::Kind>
|
||||
friend struct ::capnp::_::PointerHelpers;
|
||||
template <typename, ::capnp::Kind>
|
||||
friend struct ::capnp::List;
|
||||
friend class ::capnp::MessageBuilder;
|
||||
friend class ::capnp::Orphanage;
|
||||
};
|
||||
|
||||
class LongitudinalPlanSP::AccelController::Builder {
|
||||
public:
|
||||
typedef AccelController Builds;
|
||||
|
||||
Builder() = delete; // Deleted to discourage incorrect usage.
|
||||
// You can explicitly initialize to nullptr instead.
|
||||
inline Builder(decltype(nullptr)) {}
|
||||
inline explicit Builder(::capnp::_::StructBuilder base): _builder(base) {}
|
||||
inline operator Reader() const { return Reader(_builder.asReader()); }
|
||||
inline Reader asReader() const { return *this; }
|
||||
|
||||
inline ::capnp::MessageSize totalSize() const { return asReader().totalSize(); }
|
||||
#if !CAPNP_LITE
|
||||
inline ::kj::StringTree toString() const { return asReader().toString(); }
|
||||
#endif // !CAPNP_LITE
|
||||
|
||||
inline bool getEnabled();
|
||||
inline void setEnabled(bool value);
|
||||
|
||||
inline bool getActive();
|
||||
inline void setActive(bool value);
|
||||
|
||||
inline ::cereal::LongitudinalPlanSP::AccelController::Profile getProfile();
|
||||
inline void setProfile( ::cereal::LongitudinalPlanSP::AccelController::Profile value);
|
||||
|
||||
inline ::capnp::Void getReserved3();
|
||||
inline void setReserved3( ::capnp::Void value = ::capnp::VOID);
|
||||
|
||||
private:
|
||||
::capnp::_::StructBuilder _builder;
|
||||
template <typename, ::capnp::Kind>
|
||||
friend struct ::capnp::ToDynamic_;
|
||||
friend class ::capnp::Orphanage;
|
||||
template <typename, ::capnp::Kind>
|
||||
friend struct ::capnp::_::PointerHelpers;
|
||||
};
|
||||
|
||||
#if !CAPNP_LITE
|
||||
class LongitudinalPlanSP::AccelController::Pipeline {
|
||||
public:
|
||||
typedef AccelController Pipelines;
|
||||
|
||||
inline Pipeline(decltype(nullptr)): _typeless(nullptr) {}
|
||||
inline explicit Pipeline(::capnp::AnyPointer::Pipeline&& typeless)
|
||||
: _typeless(kj::mv(typeless)) {}
|
||||
|
||||
private:
|
||||
::capnp::AnyPointer::Pipeline _typeless;
|
||||
friend class ::capnp::PipelineHook;
|
||||
template <typename, ::capnp::Kind>
|
||||
friend struct ::capnp::ToDynamic_;
|
||||
};
|
||||
#endif // !CAPNP_LITE
|
||||
|
||||
class OnroadEventSP::Reader {
|
||||
public:
|
||||
typedef OnroadEventSP Reads;
|
||||
@@ -7009,45 +6881,6 @@ inline ::capnp::Orphan< ::cereal::LongitudinalPlanSP::E2eAlerts> LongitudinalPla
|
||||
::capnp::bounded<4>() * ::capnp::POINTERS));
|
||||
}
|
||||
|
||||
inline bool LongitudinalPlanSP::Reader::hasAccelController() const {
|
||||
return !_reader.getPointerField(
|
||||
::capnp::bounded<5>() * ::capnp::POINTERS).isNull();
|
||||
}
|
||||
inline bool LongitudinalPlanSP::Builder::hasAccelController() {
|
||||
return !_builder.getPointerField(
|
||||
::capnp::bounded<5>() * ::capnp::POINTERS).isNull();
|
||||
}
|
||||
inline ::cereal::LongitudinalPlanSP::AccelController::Reader LongitudinalPlanSP::Reader::getAccelController() const {
|
||||
return ::capnp::_::PointerHelpers< ::cereal::LongitudinalPlanSP::AccelController>::get(_reader.getPointerField(
|
||||
::capnp::bounded<5>() * ::capnp::POINTERS));
|
||||
}
|
||||
inline ::cereal::LongitudinalPlanSP::AccelController::Builder LongitudinalPlanSP::Builder::getAccelController() {
|
||||
return ::capnp::_::PointerHelpers< ::cereal::LongitudinalPlanSP::AccelController>::get(_builder.getPointerField(
|
||||
::capnp::bounded<5>() * ::capnp::POINTERS));
|
||||
}
|
||||
#if !CAPNP_LITE
|
||||
inline ::cereal::LongitudinalPlanSP::AccelController::Pipeline LongitudinalPlanSP::Pipeline::getAccelController() {
|
||||
return ::cereal::LongitudinalPlanSP::AccelController::Pipeline(_typeless.getPointerField(5));
|
||||
}
|
||||
#endif // !CAPNP_LITE
|
||||
inline void LongitudinalPlanSP::Builder::setAccelController( ::cereal::LongitudinalPlanSP::AccelController::Reader value) {
|
||||
::capnp::_::PointerHelpers< ::cereal::LongitudinalPlanSP::AccelController>::set(_builder.getPointerField(
|
||||
::capnp::bounded<5>() * ::capnp::POINTERS), value);
|
||||
}
|
||||
inline ::cereal::LongitudinalPlanSP::AccelController::Builder LongitudinalPlanSP::Builder::initAccelController() {
|
||||
return ::capnp::_::PointerHelpers< ::cereal::LongitudinalPlanSP::AccelController>::init(_builder.getPointerField(
|
||||
::capnp::bounded<5>() * ::capnp::POINTERS));
|
||||
}
|
||||
inline void LongitudinalPlanSP::Builder::adoptAccelController(
|
||||
::capnp::Orphan< ::cereal::LongitudinalPlanSP::AccelController>&& value) {
|
||||
::capnp::_::PointerHelpers< ::cereal::LongitudinalPlanSP::AccelController>::adopt(_builder.getPointerField(
|
||||
::capnp::bounded<5>() * ::capnp::POINTERS), kj::mv(value));
|
||||
}
|
||||
inline ::capnp::Orphan< ::cereal::LongitudinalPlanSP::AccelController> LongitudinalPlanSP::Builder::disownAccelController() {
|
||||
return ::capnp::_::PointerHelpers< ::cereal::LongitudinalPlanSP::AccelController>::disown(_builder.getPointerField(
|
||||
::capnp::bounded<5>() * ::capnp::POINTERS));
|
||||
}
|
||||
|
||||
inline ::cereal::LongitudinalPlanSP::DynamicExperimentalControl::DynamicExperimentalControlState LongitudinalPlanSP::DynamicExperimentalControl::Reader::getState() const {
|
||||
return _reader.getDataField< ::cereal::LongitudinalPlanSP::DynamicExperimentalControl::DynamicExperimentalControlState>(
|
||||
::capnp::bounded<0>() * ::capnp::ELEMENTS);
|
||||
@@ -7638,62 +7471,6 @@ inline void LongitudinalPlanSP::E2eAlerts::Builder::setLeadDepartAlert(bool valu
|
||||
::capnp::bounded<1>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline bool LongitudinalPlanSP::AccelController::Reader::getEnabled() const {
|
||||
return _reader.getDataField<bool>(
|
||||
::capnp::bounded<0>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline bool LongitudinalPlanSP::AccelController::Builder::getEnabled() {
|
||||
return _builder.getDataField<bool>(
|
||||
::capnp::bounded<0>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void LongitudinalPlanSP::AccelController::Builder::setEnabled(bool value) {
|
||||
_builder.setDataField<bool>(
|
||||
::capnp::bounded<0>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline bool LongitudinalPlanSP::AccelController::Reader::getActive() const {
|
||||
return _reader.getDataField<bool>(
|
||||
::capnp::bounded<1>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline bool LongitudinalPlanSP::AccelController::Builder::getActive() {
|
||||
return _builder.getDataField<bool>(
|
||||
::capnp::bounded<1>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void LongitudinalPlanSP::AccelController::Builder::setActive(bool value) {
|
||||
_builder.setDataField<bool>(
|
||||
::capnp::bounded<1>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline ::cereal::LongitudinalPlanSP::AccelController::Profile LongitudinalPlanSP::AccelController::Reader::getProfile() const {
|
||||
return _reader.getDataField< ::cereal::LongitudinalPlanSP::AccelController::Profile>(
|
||||
::capnp::bounded<1>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline ::cereal::LongitudinalPlanSP::AccelController::Profile LongitudinalPlanSP::AccelController::Builder::getProfile() {
|
||||
return _builder.getDataField< ::cereal::LongitudinalPlanSP::AccelController::Profile>(
|
||||
::capnp::bounded<1>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void LongitudinalPlanSP::AccelController::Builder::setProfile( ::cereal::LongitudinalPlanSP::AccelController::Profile value) {
|
||||
_builder.setDataField< ::cereal::LongitudinalPlanSP::AccelController::Profile>(
|
||||
::capnp::bounded<1>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline ::capnp::Void LongitudinalPlanSP::AccelController::Reader::getReserved3() const {
|
||||
return _reader.getDataField< ::capnp::Void>(
|
||||
::capnp::bounded<0>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline ::capnp::Void LongitudinalPlanSP::AccelController::Builder::getReserved3() {
|
||||
return _builder.getDataField< ::capnp::Void>(
|
||||
::capnp::bounded<0>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void LongitudinalPlanSP::AccelController::Builder::setReserved3( ::capnp::Void value) {
|
||||
_builder.setDataField< ::capnp::Void>(
|
||||
::capnp::bounded<0>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline bool OnroadEventSP::Reader::hasEvents() const {
|
||||
return !_reader.getPointerField(
|
||||
::capnp::bounded<0>() * ::capnp::POINTERS).isNull();
|
||||
|
||||
Binary file not shown.
@@ -187,12 +187,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
// toyota specific params
|
||||
{"ToyotaAutoHold", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ToyotaEnhancedBsm", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ToyotaTSS2Long", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"ToyotaDriveMode", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
// MADS params
|
||||
{"Mads", {PERSISTENT | BACKUP, BOOL, "1"}},
|
||||
{"MadsMainCruiseAllowed", {PERSISTENT | BACKUP, BOOL, "1"}},
|
||||
@@ -240,10 +234,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
// Accel Controller profiles (Eco / Normal / Sport)
|
||||
{"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"AccelPersonality", {PERSISTENT | BACKUP, INT, "1"}},
|
||||
|
||||
// sunnypilot model params
|
||||
{"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
|
||||
{"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},
|
||||
|
||||
@@ -117,16 +117,12 @@ class TestParams(OpenpilotTestCase):
|
||||
def test_params_default_value(self):
|
||||
self.params.remove("LanguageSetting")
|
||||
self.params.remove("LongitudinalPersonality")
|
||||
self.params.remove("AccelPersonalityEnabled")
|
||||
self.params.remove("AccelPersonality")
|
||||
self.params.remove("LiveParametersV2")
|
||||
|
||||
assert self.params.get("LanguageSetting") is None
|
||||
assert self.params.get("LanguageSetting", return_default=False) is None
|
||||
assert isinstance(self.params.get("LanguageSetting", return_default=True), str)
|
||||
assert isinstance(self.params.get("LongitudinalPersonality", return_default=True), int)
|
||||
assert self.params.get("AccelPersonalityEnabled", return_default=True) is False
|
||||
assert self.params.get("AccelPersonality", return_default=True) == 1
|
||||
assert self.params.get("LiveParametersV2") is None
|
||||
assert self.params.get("LiveParametersV2", return_default=True) is None
|
||||
|
||||
|
||||
@@ -11,13 +11,13 @@ from opendbc.car.structs import car
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper
|
||||
from openpilot.common.swaglog import cloudlog, ForwardingHandler
|
||||
|
||||
from opendbc.car import DT_CTRL, structs
|
||||
from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
|
||||
from opendbc.car.carlog import carlog
|
||||
from opendbc.car.fw_versions import ObdCallback
|
||||
from opendbc.car.car_helpers import get_car, interfaces
|
||||
from opendbc.car.interfaces import CarInterfaceBase, RadarInterfaceBase
|
||||
from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
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
|
||||
@@ -123,9 +123,6 @@ class Car:
|
||||
self.RI = RI
|
||||
|
||||
self.CP.alternativeExperience = 0
|
||||
if self.params.get_bool("ToyotaAutoHold"):
|
||||
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
|
||||
|
||||
# mads
|
||||
set_alternative_experience(self.CP, self.CP_SP, self.params)
|
||||
set_car_specific_params(self.CP, self.CP_SP, self.params)
|
||||
|
||||
@@ -19,7 +19,6 @@ IMPERIAL_INCREMENT = round(CV.MPH_TO_KPH, 1) # round here to avoid rounding err
|
||||
ButtonEvent = car.CarState.ButtonEvent
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
CRUISE_LONG_PRESS = 50
|
||||
TOYOTA_VIRTUAL_CRUISE_LONG_PRESS = 65
|
||||
CRUISE_NEAREST_FUNC = {
|
||||
ButtonType.accelCruise: math.ceil,
|
||||
ButtonType.decelCruise: math.floor,
|
||||
@@ -44,30 +43,6 @@ class VCruiseHelper(VCruiseHelperSP):
|
||||
def v_cruise_initialized(self):
|
||||
return self.v_cruise_kph != V_CRUISE_UNSET
|
||||
|
||||
@property
|
||||
def software_pcm_cruise_speed(self) -> bool:
|
||||
return self.CP.brand == "toyota" and self.CP.pcmCruise and self.CP.openpilotLongitudinalControl and not self.CP_SP.pcmCruiseSpeed
|
||||
|
||||
@property
|
||||
def cruise_long_press_frames(self) -> int:
|
||||
return TOYOTA_VIRTUAL_CRUISE_LONG_PRESS if self.software_pcm_cruise_speed else CRUISE_LONG_PRESS
|
||||
|
||||
@property
|
||||
def software_pcm_cruise_initialized(self) -> bool:
|
||||
return 0 < self.v_cruise_kph < V_CRUISE_UNSET and 0 < self.v_cruise_cluster_kph < V_CRUISE_UNSET
|
||||
|
||||
def _apply_software_pcm_cruise_delta(self, delta_kph: float, is_metric: bool) -> None:
|
||||
"""Move Toyota's planner/display targets together while respecting both targets' bounds."""
|
||||
cluster_min_kph = self.v_cruise_min if is_metric else self.v_cruise_min * CV.MPH_TO_KPH
|
||||
min_delta = max(V_CRUISE_MIN - self.v_cruise_kph, cluster_min_kph - self.v_cruise_cluster_kph)
|
||||
max_delta = min(V_CRUISE_MAX - self.v_cruise_kph, V_CRUISE_MAX - self.v_cruise_cluster_kph)
|
||||
if delta_kph > 0:
|
||||
applied_delta = min(delta_kph, max(0., max_delta))
|
||||
else:
|
||||
applied_delta = max(delta_kph, min(0., min_delta))
|
||||
self.v_cruise_kph = round(self.v_cruise_kph + applied_delta, 1)
|
||||
self.v_cruise_cluster_kph = round(self.v_cruise_cluster_kph + applied_delta, 1)
|
||||
|
||||
def update_v_cruise(self, CS, enabled, is_metric):
|
||||
self.v_cruise_kph_last = self.v_cruise_kph
|
||||
|
||||
@@ -76,21 +51,11 @@ class VCruiseHelper(VCruiseHelperSP):
|
||||
_enabled = self.update_enabled_state(CS, enabled)
|
||||
|
||||
if CS.cruiseState.available:
|
||||
software_pcm_enabled = not self.CP_SP.pcmCruiseSpeed and _enabled
|
||||
if self.software_pcm_cruise_speed:
|
||||
software_pcm_enabled = software_pcm_enabled and self.software_pcm_cruise_initialized
|
||||
|
||||
if not self.CP.pcmCruise or software_pcm_enabled:
|
||||
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_cruise_non_pcm(CS, _enabled, is_metric)
|
||||
v_cruise_kph_before_sla = self.v_cruise_kph
|
||||
self.update_speed_limit_assist_v_cruise_non_pcm()
|
||||
if self.software_pcm_cruise_speed:
|
||||
sla_delta_kph = self.v_cruise_kph - v_cruise_kph_before_sla
|
||||
self.v_cruise_kph = v_cruise_kph_before_sla
|
||||
self._apply_software_pcm_cruise_delta(sla_delta_kph, is_metric)
|
||||
else:
|
||||
self.v_cruise_cluster_kph = self.v_cruise_kph
|
||||
self.v_cruise_cluster_kph = self.v_cruise_kph
|
||||
else:
|
||||
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
|
||||
self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
|
||||
@@ -120,13 +85,13 @@ class VCruiseHelper(VCruiseHelperSP):
|
||||
|
||||
for b in CS.buttonEvents:
|
||||
if b.type.raw in self.button_timers and not b.pressed:
|
||||
if self.button_timers[b.type.raw] > self.cruise_long_press_frames:
|
||||
if self.button_timers[b.type.raw] > CRUISE_LONG_PRESS:
|
||||
return # end long press
|
||||
button_type = b.type.raw
|
||||
break
|
||||
else:
|
||||
for k, timer in self.button_timers.items():
|
||||
if timer and timer % self.cruise_long_press_frames == 0:
|
||||
if timer and timer % CRUISE_LONG_PRESS == 0:
|
||||
button_type = k
|
||||
long_press = True
|
||||
break
|
||||
@@ -150,26 +115,10 @@ class VCruiseHelper(VCruiseHelperSP):
|
||||
return
|
||||
|
||||
long_press, v_cruise_delta = VCruiseHelperSP.update_v_cruise_delta(self, long_press, v_cruise_delta)
|
||||
# Toyota's canonical PCM set speed and displayed cluster set speed can differ. In
|
||||
# software-owned PCM mode, round the value the driver sees and apply the same delta
|
||||
# to both targets so the planner/cluster calibration offset remains intact.
|
||||
v_cruise_reference = self.v_cruise_cluster_kph if self.software_pcm_cruise_speed else self.v_cruise_kph
|
||||
if long_press and v_cruise_reference % v_cruise_delta != 0: # partial interval
|
||||
v_cruise_reference_new = CRUISE_NEAREST_FUNC[button_type](v_cruise_reference / v_cruise_delta) * v_cruise_delta
|
||||
if long_press and self.v_cruise_kph % v_cruise_delta != 0: # partial interval
|
||||
self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta
|
||||
else:
|
||||
v_cruise_reference_new = v_cruise_reference + v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
|
||||
|
||||
if self.software_pcm_cruise_speed:
|
||||
delta_kph = v_cruise_reference_new - v_cruise_reference
|
||||
|
||||
# If SET is pressed while overriding, do not lower the target below the current speed.
|
||||
if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
|
||||
delta_kph = max(delta_kph, CS.vEgo * CV.MS_TO_KPH - self.v_cruise_kph)
|
||||
|
||||
self._apply_software_pcm_cruise_delta(delta_kph, is_metric)
|
||||
return
|
||||
|
||||
self.v_cruise_kph += v_cruise_reference_new - v_cruise_reference
|
||||
self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
|
||||
|
||||
# If set is pressed while overriding, clip cruise speed to minimum of vEgo
|
||||
if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
|
||||
@@ -178,12 +127,6 @@ class VCruiseHelper(VCruiseHelperSP):
|
||||
self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), self.v_cruise_min, V_CRUISE_MAX)
|
||||
|
||||
def update_button_timers(self, CS, enabled):
|
||||
if self.software_pcm_cruise_speed and (not enabled or not CS.cruiseState.available or not self.software_pcm_cruise_initialized):
|
||||
for k in self.button_timers:
|
||||
self.button_timers[k] = 0
|
||||
self.button_change_states[k] = {"standstill": False, "enabled": False}
|
||||
return
|
||||
|
||||
# increment timer for buttons still pressed
|
||||
for k in self.button_timers:
|
||||
if self.button_timers[k] > 0:
|
||||
|
||||
@@ -4,7 +4,6 @@ from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
|
||||
from openpilot.common.pid import PIDController
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import LongControlSP
|
||||
|
||||
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
|
||||
|
||||
@@ -40,9 +39,8 @@ def long_control_state_trans(CP_SP, active, long_control_state,
|
||||
|
||||
return long_control_state
|
||||
|
||||
class LongControl(LongControlSP):
|
||||
class LongControl:
|
||||
def __init__(self, CP, CP_SP):
|
||||
LongControlSP.__init__(self)
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
self.long_control_state = LongCtrlState.off
|
||||
@@ -61,17 +59,16 @@ class LongControl(LongControlSP):
|
||||
self.long_control_state = long_control_state_trans(self.CP_SP, active, self.long_control_state,
|
||||
should_stop, CS.brakePressed,
|
||||
CS.cruiseState.standstill)
|
||||
LongControlSP.update_state(self, self.long_control_state == LongCtrlState.stopping, active, CS)
|
||||
if self.long_control_state == LongCtrlState.off:
|
||||
self.reset()
|
||||
output_accel = 0.
|
||||
|
||||
elif self.long_control_state == LongCtrlState.stopping:
|
||||
output_accel = LongControlSP.stopping_accel(self, self.last_output_accel, CS)
|
||||
output_accel = self.last_output_accel
|
||||
if output_accel > self.CP.stopAccel:
|
||||
output_accel = min(output_accel, 0.0)
|
||||
# TODO: can we just go straight to stopAccel?
|
||||
output_accel -= LongControlSP.stopping_decel_rate(self, CS, a_target, output_accel) * DT_CTRL
|
||||
output_accel -= 1.0 * DT_CTRL # m/s^2/s while trying to stop
|
||||
self.reset()
|
||||
|
||||
else: # LongCtrlState.pid
|
||||
|
||||
@@ -35,13 +35,8 @@ def get_max_accel(v_ego):
|
||||
def get_coast_accel(pitch):
|
||||
return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
|
||||
|
||||
def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle,
|
||||
max_accel_override=None, min_accel_override=None):
|
||||
if max_accel_override is not None:
|
||||
max_accel = max_accel_override
|
||||
else:
|
||||
max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
|
||||
min_accel = A_CRUISE_MIN if e2e or min_accel_override is None else min_accel_override
|
||||
def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle):
|
||||
max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
|
||||
|
||||
if not e2e:
|
||||
a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
|
||||
@@ -53,18 +48,11 @@ def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt,
|
||||
coast_limit = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [max_accel, clipped_accel_coast])
|
||||
max_accel = min(max_accel, coast_limit)
|
||||
|
||||
target_accel = np.clip(v_cruise - v_ego, min_accel, max_accel)
|
||||
|
||||
# An override only counts as "active" if it's the bound that actually determined target_accel here --
|
||||
# turn/coast derating can shrink max_accel back below max_accel_override, and either bound can simply
|
||||
# not be reached if v_cruise - v_ego already sits inside [min_accel, max_accel] on its own.
|
||||
accel_controller_active = bool((max_accel_override is not None and max_accel == max_accel_override and target_accel == max_accel) or
|
||||
(min_accel_override is not None and min_accel == min_accel_override and target_accel == min_accel))
|
||||
|
||||
target_accel = np.clip(v_cruise - v_ego, A_CRUISE_MIN, max_accel)
|
||||
j_cruise = np.interp(v_ego, A_CRUISE_MAX_BP, J_CRUISE_VALS)
|
||||
target_accel = float(np.clip(target_accel, a_cruise_prev - j_cruise * dt, a_cruise_prev + j_cruise * dt))
|
||||
|
||||
return target_accel, accel_controller_active
|
||||
return target_accel
|
||||
|
||||
|
||||
class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
@@ -80,7 +68,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
self.a_cruise = init_a
|
||||
self.output_a_target = init_a
|
||||
self.output_should_stop = False
|
||||
self.accel_controller_active = False
|
||||
|
||||
self.v_desired_trajectory = np.zeros(CONTROL_N)
|
||||
self.a_desired_trajectory = np.zeros(CONTROL_N)
|
||||
@@ -97,8 +84,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
v_ego = sm['carState'].vEgo
|
||||
v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
|
||||
v_cruise = v_cruise_kph * CV.KPH_TO_MS
|
||||
force_decel = sm['controlsState'].forceDecel
|
||||
if force_decel:
|
||||
if sm['controlsState'].forceDecel:
|
||||
v_cruise = 0.0
|
||||
|
||||
long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
|
||||
@@ -149,17 +135,14 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
output_a_target_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
|
||||
action_t=action_t)
|
||||
output_should_stop_mpc = should_stop(v_ego, output_a_target_mpc)
|
||||
output_should_stop_mpc = self.update_lead_departure(sm, output_a_target_mpc, output_should_stop_mpc, reset_state)
|
||||
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
|
||||
output_should_stop_e2e = sm['modelV2'].action.shouldStop
|
||||
|
||||
is_e2e = self.is_e2e(sm)
|
||||
|
||||
max_accel_override = self.get_max_accel_override(v_ego)
|
||||
min_accel_override = self.get_min_accel_override(v_ego, is_e2e, force_decel)
|
||||
self.a_cruise, self.accel_controller_active = get_cruise_accel(is_e2e, v_cruise, v_ego,
|
||||
self.a_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego,
|
||||
self.a_cruise, steer_angle_without_offset, self.CP, self.dt,
|
||||
accel_coast, self.allow_throttle, max_accel_override, min_accel_override)
|
||||
accel_coast, self.allow_throttle)
|
||||
cruise_should_stop = should_stop(v_ego, self.a_cruise)
|
||||
|
||||
candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
|
||||
|
||||
Binary file not shown.
@@ -29,6 +29,12 @@ enum SpiError {
|
||||
|
||||
const unsigned int SPI_ACK_TIMEOUT = 500; // milliseconds
|
||||
const std::string SPI_DEVICE = "/dev/spidev0.0";
|
||||
// TODO: fix SPI turnaround synchronization at the protocol level.
|
||||
static uint64_t spi_last_bus_activity_ns = 0; // protected by hw_lock
|
||||
|
||||
static void wait_for_spi_turnaround(uint64_t start_ns) {
|
||||
while ((nanos_since_boot() - start_ns) < 400000) {}
|
||||
}
|
||||
|
||||
class LockEx {
|
||||
public:
|
||||
@@ -319,6 +325,8 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
|
||||
assert(tx_len < SPI_BUF_SIZE);
|
||||
assert(max_rx_len < SPI_BUF_SIZE);
|
||||
|
||||
wait_for_spi_turnaround(spi_last_bus_activity_ns);
|
||||
|
||||
xfer_count++;
|
||||
header = {
|
||||
.sync = SPI_SYNC,
|
||||
@@ -347,6 +355,7 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
wait_for_spi_turnaround(nanos_since_boot());
|
||||
|
||||
// Send data
|
||||
if (tx_data != NULL) {
|
||||
@@ -389,6 +398,7 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
|
||||
memcpy(rx_data, rx_buf + 3, rx_data_len);
|
||||
}
|
||||
|
||||
spi_last_bus_activity_ns = nanos_since_boot();
|
||||
return rx_data_len;
|
||||
|
||||
fail:
|
||||
@@ -403,6 +413,7 @@ fail:
|
||||
}
|
||||
}
|
||||
|
||||
spi_last_bus_activity_ns = nanos_since_boot();
|
||||
if (ret >= 0) ret = -1;
|
||||
return ret;
|
||||
}
|
||||
|
||||
@@ -11,15 +11,6 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
|
||||
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
|
||||
|
||||
|
||||
class PlannerSM(dict):
|
||||
def __init__(self, radar_frame: int, services: dict):
|
||||
super().__init__(services)
|
||||
self.frame = radar_frame
|
||||
self.logMonoTime = {"radarState": radar_frame}
|
||||
self.valid = {"radarState": True}
|
||||
self.alive = {"radarState": True}
|
||||
|
||||
|
||||
class Plant:
|
||||
messaging_initialized = False
|
||||
|
||||
@@ -141,7 +132,7 @@ class Plant:
|
||||
car_control.carControl.orientationNED = [0., float(pitch), 0.]
|
||||
|
||||
# ******** get controlsState messages for plotting ***
|
||||
sm = PlannerSM(self.rk.frame, {'radarState': radar.radarState,
|
||||
sm = {'radarState': radar.radarState,
|
||||
'carState': car_state.carState,
|
||||
'carControl': car_control.carControl,
|
||||
'controlsState': control.controlsState,
|
||||
@@ -150,7 +141,7 @@ class Plant:
|
||||
'modelV2': model.modelV2,
|
||||
'carStateSP': car_state_sp.carStateSP,
|
||||
'liveMapDataSP': live_map_data_sp.liveMapDataSP,
|
||||
'gpsLocation': gps_data.gpsLocation})
|
||||
'gpsLocation': gps_data.gpsLocation}
|
||||
self.planner.update(sm)
|
||||
self.acceleration = self.planner.output_a_target
|
||||
if self.planner.output_should_stop:
|
||||
|
||||
@@ -27,13 +27,6 @@ DESCRIPTIONS = {
|
||||
"In relaxed mode sunnypilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with " +
|
||||
"your steering wheel distance button."
|
||||
),
|
||||
"AccelPersonalityEnabled": tr_noop(
|
||||
"Sets your preferred acceleration and cruise-deceleration limits by profile. Lead following, braking, and stopping behavior remain " +
|
||||
"independent of this setting."
|
||||
),
|
||||
"AccelPersonality": tr_noop(
|
||||
"Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across profiles."
|
||||
),
|
||||
"IsLdwEnabled": tr_noop(
|
||||
"Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " +
|
||||
"without a turn signal activated while driving over 31 mph (50 km/h)."
|
||||
@@ -113,24 +106,6 @@ class TogglesLayout(Widget):
|
||||
icon="speed_limit.png"
|
||||
)
|
||||
|
||||
self._accel_controller_enabled = toggle_item(
|
||||
lambda: tr("Enable Accel Controller"),
|
||||
lambda: tr(DESCRIPTIONS["AccelPersonalityEnabled"]),
|
||||
self._params.get_bool("AccelPersonalityEnabled"),
|
||||
callback=self._set_accel_controller_enabled,
|
||||
icon="speed_limit.png",
|
||||
)
|
||||
|
||||
self._accel_personality_setting = multiple_button_item(
|
||||
lambda: tr("Acceleration Profile"),
|
||||
lambda: tr(DESCRIPTIONS["AccelPersonality"]),
|
||||
buttons=[lambda: tr("Eco"), lambda: tr("Normal"), lambda: tr("Sport")],
|
||||
button_width=300,
|
||||
callback=self._set_accel_personality,
|
||||
selected_index=self._params.get("AccelPersonality", return_default=True),
|
||||
icon="speed_limit.png"
|
||||
)
|
||||
|
||||
self._toggles = {}
|
||||
self._locked_toggles = set()
|
||||
for param, (title, desc, icon, needs_restart) in self._toggle_defs.items():
|
||||
@@ -160,11 +135,9 @@ class TogglesLayout(Widget):
|
||||
|
||||
self._toggles[param] = toggle
|
||||
|
||||
# insert longitudinal personality and Accel Controller settings after NDOG toggle
|
||||
# insert longitudinal personality after NDOG toggle
|
||||
if param == "DisengageOnAccelerator":
|
||||
self._toggles["LongitudinalPersonality"] = self._long_personality_setting
|
||||
self._toggles["AccelPersonalityEnabled"] = self._accel_controller_enabled
|
||||
self._toggles["AccelPersonality"] = self._accel_personality_setting
|
||||
|
||||
self._update_experimental_mode_icon()
|
||||
self._scroller = Scroller(list(self._toggles.values()), line_separator=True, spacing=0)
|
||||
@@ -185,7 +158,6 @@ class TogglesLayout(Widget):
|
||||
|
||||
def _update_toggles(self):
|
||||
ui_state.update_params()
|
||||
accel_controller_enabled = self._params.get_bool("AccelPersonalityEnabled")
|
||||
|
||||
e2e_description = tr(
|
||||
"sunnypilot defaults to driving in chill mode. Experimental mode enables alpha-level features that aren't ready for chill mode. " +
|
||||
@@ -204,15 +176,11 @@ class TogglesLayout(Widget):
|
||||
self._toggles["ExperimentalMode"].action_item.set_enabled(True)
|
||||
self._toggles["ExperimentalMode"].set_description(e2e_description)
|
||||
self._long_personality_setting.action_item.set_enabled(True)
|
||||
self._accel_controller_enabled.action_item.set_enabled(True)
|
||||
self._accel_personality_setting.action_item.set_enabled(True)
|
||||
else:
|
||||
# no long for now
|
||||
self._toggles["ExperimentalMode"].action_item.set_enabled(False)
|
||||
self._toggles["ExperimentalMode"].action_item.set_state(False)
|
||||
self._long_personality_setting.action_item.set_enabled(False)
|
||||
self._accel_controller_enabled.action_item.set_enabled(False)
|
||||
self._accel_personality_setting.action_item.set_enabled(False)
|
||||
self._params.remove("ExperimentalMode")
|
||||
|
||||
unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.")
|
||||
@@ -235,8 +203,6 @@ class TogglesLayout(Widget):
|
||||
# refresh toggles from params to mirror external changes
|
||||
for param in self._toggle_defs:
|
||||
self._toggles[param].action_item.set_state(self._params.get_bool(param))
|
||||
self._accel_controller_enabled.action_item.set_state(accel_controller_enabled)
|
||||
self._accel_personality_setting.action_item.set_selected_button(self._params.get("AccelPersonality", return_default=True))
|
||||
|
||||
# these toggles need restart, block while engaged
|
||||
for toggle_def in self._toggle_defs:
|
||||
@@ -281,9 +247,3 @@ class TogglesLayout(Widget):
|
||||
|
||||
def _set_longitudinal_personality(self, button_index: int):
|
||||
self._params.put("LongitudinalPersonality", button_index, block=True)
|
||||
|
||||
def _set_accel_personality(self, button_index: int):
|
||||
self._params.put("AccelPersonality", button_index, block=True)
|
||||
|
||||
def _set_accel_controller_enabled(self, state: bool):
|
||||
self._params.put_bool("AccelPersonalityEnabled", state, block=True)
|
||||
|
||||
@@ -14,7 +14,6 @@ from openpilot.system.ui.lib.application import gui_app
|
||||
if gui_app.sunnypilot_ui():
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.layouts.settings import SettingsLayoutSP as SettingsLayout
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.layouts.home import MiciHomeLayoutSP as MiciHomeLayout
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.layouts.onroad import OnroadViewContainerSP as AugmentedRoadView
|
||||
|
||||
ONROAD_DELAY = 2.5 # seconds
|
||||
|
||||
@@ -73,9 +72,6 @@ class MiciMainLayout(Scroller):
|
||||
# For scroll_to
|
||||
return self._body_onroad_layout if ui_state.is_body else self._car_onroad_layout
|
||||
|
||||
def _should_auto_scroll_to_onroad(self) -> bool:
|
||||
return True
|
||||
|
||||
def _setup_callbacks(self):
|
||||
self._home_layout.set_callbacks(
|
||||
on_settings=lambda: gui_app.push_widget(self._settings_layout),
|
||||
@@ -126,15 +122,13 @@ class MiciMainLayout(Scroller):
|
||||
|
||||
# FIXME: these two pops can interrupt user interacting in the settings
|
||||
if self._onroad_time_delay is not None and rl.get_time() - self._onroad_time_delay >= ONROAD_DELAY:
|
||||
if not gui_app.sunnypilot_ui() or self._should_auto_scroll_to_onroad():
|
||||
gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
|
||||
gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
|
||||
self._onroad_time_delay = None
|
||||
|
||||
# When car leaves standstill, pop nav stack and scroll to onroad
|
||||
CS = ui_state.sm["carState"]
|
||||
if not CS.standstill and self._prev_standstill:
|
||||
if not gui_app.sunnypilot_ui() or self._should_auto_scroll_to_onroad():
|
||||
gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
|
||||
gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
|
||||
self._prev_standstill = CS.standstill
|
||||
|
||||
def _on_interactive_timeout(self):
|
||||
|
||||
@@ -42,8 +42,6 @@ class TogglesLayoutMici(NavScroller):
|
||||
super().__init__()
|
||||
|
||||
self._personality_toggle = BigMultiParamToggle("driving personality", "LongitudinalPersonality", ["aggressive", "standard", "relaxed"])
|
||||
self._accel_controller_enabled = BigParamControl("enable accel controller", "AccelPersonalityEnabled")
|
||||
self._accel_personality_toggle = BigMultiParamToggle("acceleration profile", "AccelPersonality", ["eco", "normal", "sport"])
|
||||
self._experimental_btn = BigToggle("experimental mode", initial_state=ui_state.params.get_bool("ExperimentalMode"),
|
||||
toggle_callback=self._on_experimental_mode)
|
||||
is_metric_toggle = BigParamControl("use metric units", "IsMetric")
|
||||
@@ -55,8 +53,6 @@ class TogglesLayoutMici(NavScroller):
|
||||
|
||||
self._scroller.add_widgets([
|
||||
self._personality_toggle,
|
||||
self._accel_controller_enabled,
|
||||
self._accel_personality_toggle,
|
||||
self._experimental_btn,
|
||||
is_metric_toggle,
|
||||
ldw_toggle,
|
||||
@@ -69,7 +65,6 @@ class TogglesLayoutMici(NavScroller):
|
||||
# Toggle lists
|
||||
self._refresh_toggles = (
|
||||
("ExperimentalMode", self._experimental_btn),
|
||||
("AccelPersonalityEnabled", self._accel_controller_enabled),
|
||||
("IsMetric", is_metric_toggle),
|
||||
("IsLdwEnabled", ldw_toggle),
|
||||
("AlwaysOnDM", always_on_dm_toggle),
|
||||
@@ -109,23 +104,17 @@ class TogglesLayoutMici(NavScroller):
|
||||
if ui_state.has_longitudinal_control:
|
||||
self._experimental_btn.set_visible(True)
|
||||
self._personality_toggle.set_visible(True)
|
||||
self._accel_controller_enabled.set_visible(True)
|
||||
self._accel_personality_toggle.set_visible(True)
|
||||
else:
|
||||
# no long for now
|
||||
self._experimental_btn.set_visible(False)
|
||||
self._experimental_btn.set_checked(False)
|
||||
self._personality_toggle.set_visible(False)
|
||||
self._accel_controller_enabled.set_visible(False)
|
||||
self._accel_personality_toggle.set_visible(False)
|
||||
ui_state.params.remove("ExperimentalMode")
|
||||
|
||||
# Refresh toggles from params to mirror external changes
|
||||
for key, item in self._refresh_toggles:
|
||||
item.set_checked(ui_state.params.get_bool(key))
|
||||
|
||||
self._accel_personality_toggle.refresh()
|
||||
|
||||
def _on_experimental_mode(self, state: bool):
|
||||
if state and not ui_state.params.get_bool("ExperimentalModeConfirmed"):
|
||||
# Don't show enabled state until confirm
|
||||
|
||||
@@ -154,8 +154,8 @@ class ModelRenderer(Widget, ModelRendererSP):
|
||||
self._draw_lane_lines()
|
||||
self._draw_path(sm)
|
||||
|
||||
if render_lead_indicator and radar_state:
|
||||
self._draw_lead_indicator()
|
||||
# if render_lead_indicator and radar_state:
|
||||
# self._draw_lead_indicator()
|
||||
|
||||
def _update_raw_points(self, model):
|
||||
"""Update raw 3D points from model data"""
|
||||
|
||||
@@ -385,18 +385,13 @@ class BigMultiParamToggle(BigMultiToggle):
|
||||
self._load_value()
|
||||
|
||||
def _load_value(self):
|
||||
value = self._params.get(self._param, return_default=True)
|
||||
index = value if isinstance(value, int) else 0
|
||||
self.set_value(self._options[max(0, min(index, len(self._options) - 1))])
|
||||
self.set_value(self._options[self._params.get(self._param) or 0])
|
||||
|
||||
def _handle_mouse_release(self, mouse_pos: MousePos):
|
||||
super()._handle_mouse_release(mouse_pos)
|
||||
new_idx = self._options.index(self.value)
|
||||
self._params.put(self._param, new_idx)
|
||||
|
||||
def refresh(self):
|
||||
self._load_value()
|
||||
|
||||
|
||||
class BigParamControl(BigToggle):
|
||||
def __init__(self, text: str, param: str, toggle_callback: Callable | None = None):
|
||||
|
||||
@@ -192,7 +192,7 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
|
||||
|
||||
max_idx = self._get_path_length_idx(path_x_array, max_distance)
|
||||
self._path.projected_points = self._map_line_to_polygon(
|
||||
self._path.raw_points, 0.9, self._path_offset_z, max_idx, max_distance, allow_invert=False
|
||||
self._path.raw_points, self._get_path_half_width(), self._path_offset_z, max_idx, max_distance, allow_invert=False
|
||||
)
|
||||
|
||||
self._update_experimental_gradient()
|
||||
@@ -292,7 +292,7 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
|
||||
allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control
|
||||
self._blend_filter.update(int(allow_throttle))
|
||||
|
||||
if ui_state.rainbow_path:
|
||||
if ui_state.rainbow_path and self._lateral_active:
|
||||
self.rainbow_path.draw_rainbow_path(self._rect, self._path)
|
||||
return
|
||||
|
||||
|
||||
@@ -143,8 +143,7 @@ class CruiseLayout(Widget):
|
||||
self.icbm_toggle.show_description(True)
|
||||
|
||||
if has_long or has_icbm:
|
||||
software_cruise_speed = has_long and (not ui_state.CP.pcmCruise or not ui_state.CP_SP.pcmCruiseSpeed)
|
||||
self.custom_acc_toggle.action_item.set_enabled((software_cruise_speed or has_icbm) and ui_state.is_offroad())
|
||||
self.custom_acc_toggle.action_item.set_enabled(((has_long and not ui_state.CP.pcmCruise) or has_icbm) and ui_state.is_offroad())
|
||||
self.dec_toggle.action_item.set_enabled(has_long)
|
||||
self.scc_v_toggle.action_item.set_enabled(True)
|
||||
self.scc_m_toggle.action_item.set_enabled(True)
|
||||
@@ -170,7 +169,7 @@ class CruiseLayout(Widget):
|
||||
show_custom_acc_desc = True
|
||||
else:
|
||||
if has_long or has_icbm:
|
||||
if has_long and ui_state.CP.pcmCruise and ui_state.CP_SP.pcmCruiseSpeed:
|
||||
if has_long and ui_state.CP.pcmCruise:
|
||||
new_custom_acc_desc = tr(ACC_PCMCRUISE_DISABLED_DESCRIPTION)
|
||||
show_custom_acc_desc = True
|
||||
else:
|
||||
|
||||
@@ -10,7 +10,7 @@ import time
|
||||
import pyray as rl
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.sunnypilot.models.default_model import DEFAULT_MODEL
|
||||
from openpilot.sunnypilot.models.default_model import get_default_model
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.ui.ui_state import device, ui_state
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
@@ -211,7 +211,8 @@ class ModelsLayout(Widget):
|
||||
for bundle in bundles:
|
||||
folders.setdefault(next((ov_ride.value for ov_ride in bundle.overrides if ov_ride.key == "folder"), ""), []).append(bundle)
|
||||
|
||||
folders_list = [TreeFolder("", [TreeNode("Default", {'display_name': f"{DEFAULT_MODEL} (Default)", 'short_name': "Default"})])]
|
||||
folders_list = [TreeFolder("", [TreeNode("Default", {'display_name': f"{get_default_model()} (Default)",
|
||||
'short_name': "Default"})])]
|
||||
for folder, folder_bundles in sorted(folders.items(), key=lambda x: max((bundle.index for bundle in x[1]), default=-1), reverse=True):
|
||||
folder_bundles.sort(key=lambda bundle: bundle.index, reverse=True)
|
||||
name = folder + (f" - (Updated: {m.group(1)})" if folder_bundles and (m := re.search(r'\(([^)]*)\)[^(]*$', folder_bundles[0].displayName)) else "")
|
||||
@@ -249,7 +250,8 @@ class ModelsLayout(Widget):
|
||||
self._update_lagd_description(live_delay)
|
||||
self.model_manager = ui_state.sm["modelManagerSP"]
|
||||
self._handle_bundle_download_progress()
|
||||
active_name = self.model_manager.activeBundle.displayName if self.model_manager and self.model_manager.activeBundle.ref else f"{DEFAULT_MODEL} (Default)"
|
||||
default_label = f"{get_default_model()} (Default)"
|
||||
active_name = self.model_manager.activeBundle.displayName if self.model_manager and self.model_manager.activeBundle.ref else default_label
|
||||
self.current_model_item.action_item.set_value(active_name)
|
||||
|
||||
if not ui_state.is_offroad():
|
||||
|
||||
@@ -23,7 +23,7 @@ DESCRIPTIONS = {
|
||||
'stop_and_go_hack': tr_noop(
|
||||
'sunnypilot will allow some Toyota/Lexus cars to auto resume during stop and go traffic. ' +
|
||||
'This feature is only applicable to certain models that are able to use longitudinal control. This is an alpha feature. Use at your own risk.'
|
||||
),
|
||||
)
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -1,19 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroll_panel_sp import GuiScrollPanel2SP
|
||||
|
||||
|
||||
class MiciMainLayoutSP(MiciMainLayout):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
scroller = self._scroller
|
||||
scroller.scroll_panel = GuiScrollPanel2SP(scroller._horizontal, handle_out_of_bounds=not scroller._snap_items)
|
||||
|
||||
def _should_auto_scroll_to_onroad(self) -> bool:
|
||||
return not self._onroad_layout.is_on_info_panel()
|
||||
@@ -7,7 +7,7 @@ See the LICENSE.md file in the root directory for more details.
|
||||
import pyray as rl
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.sunnypilot.models.default_model import DEFAULT_MODEL
|
||||
from openpilot.sunnypilot.models.default_model import get_default_model
|
||||
from openpilot.selfdrive.ui.mici.widgets.button import BigButton
|
||||
from openpilot.selfdrive.ui.sunnypilot.layouts.settings.models import ModelsLayout
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, device
|
||||
@@ -27,7 +27,7 @@ class CurrentModelInfo(Widget):
|
||||
subheader_color = rl.Color(255, 255, 255, int(255 * 0.9 * 0.65))
|
||||
max_width = int(self._rect.width - 20)
|
||||
self.current_model_header = UnifiedLabel(tr("active model"), 48, max_width=max_width, text_color=header_color, font_weight=FontWeight.DISPLAY)
|
||||
default_text = f"{DEFAULT_MODEL} (Default)".lower()
|
||||
default_text = f"{get_default_model()} (Default)".lower()
|
||||
self.current_model_text = UnifiedLabel(default_text, 32, max_width=max_width, text_color=subheader_color, font_weight=FontWeight.ROMAN, scroll=True)
|
||||
|
||||
self.info_header = UnifiedLabel("cache size", 48, max_width=max_width, text_color=header_color, font_weight=FontWeight.DISPLAY)
|
||||
@@ -95,7 +95,7 @@ class ModelsLayoutMici(NavScroller):
|
||||
|
||||
folders = self._get_grouped_bundles(favorites)
|
||||
folder_buttons = []
|
||||
default_btn = BigButton(f"{DEFAULT_MODEL} (Default)".lower())
|
||||
default_btn = BigButton(f"{get_default_model()} (Default)".lower())
|
||||
default_btn.set_click_callback(self._select_default)
|
||||
folder_buttons.append(default_btn)
|
||||
|
||||
@@ -162,7 +162,8 @@ class ModelsLayoutMici(NavScroller):
|
||||
self._was_downloading = is_downloading
|
||||
|
||||
self.current_model_info.current_model_header.set_text(tr("active model"))
|
||||
model_text = manager.activeBundle.displayName.lower() if manager.activeBundle.ref else f"{DEFAULT_MODEL} (Default)".lower()
|
||||
default_model_text = f"{get_default_model()} (Default)".lower()
|
||||
model_text = manager.activeBundle.displayName.lower() if manager.activeBundle.ref else default_model_text
|
||||
self.current_model_info.current_model_text.set_text(model_text)
|
||||
self.current_model_info.info_header.set_text(tr("cache size"))
|
||||
self.current_model_info.info_text.set_text(f"{ModelsLayout.calculate_cache_size():.2f} MB")
|
||||
@@ -191,4 +192,3 @@ class ModelsLayoutMici(NavScroller):
|
||||
self.current_model_info.info_header.set_text(tr("progress") + self._download_progress)
|
||||
self.current_model_info.info_header._shimmer = True
|
||||
self.current_model_info.info_text.set_text(f"{progress/count:.2f}%")
|
||||
|
||||
|
||||
@@ -1,64 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from collections.abc import Callable
|
||||
import pyray as rl
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroller_sp import ScrollerSP
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.onroad.augmented_road_view import AugmentedRoadViewSP
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.layouts.onroad_info_panel import OnroadInfoPanel
|
||||
|
||||
CONFIDENCE_BALL_VISIBLE_RATIO = 0.4
|
||||
HORIZONTAL_SETTLE_PX = 5
|
||||
HORIZONTAL_RESET_RATIO = 0.5
|
||||
|
||||
|
||||
class OnroadViewContainerSP(ScrollerSP):
|
||||
def __init__(self, bookmark_callback=None):
|
||||
super().__init__(horizontal=False, snap_items=True, spacing=0, pad=0, scroll_indicator=False, edge_shadows=False)
|
||||
self.road_view = AugmentedRoadViewSP(bookmark_callback=bookmark_callback)
|
||||
self.onroad_info_panel = OnroadInfoPanel(bookmark_callback=bookmark_callback)
|
||||
|
||||
self._scroller.add_widgets([
|
||||
self.road_view,
|
||||
self.onroad_info_panel,
|
||||
])
|
||||
self._scroller.set_reset_scroll_at_show(False)
|
||||
self._scroller.set_scrolling_enabled(lambda: abs(self.rect.x) < HORIZONTAL_SETTLE_PX)
|
||||
|
||||
for child in (self.road_view, self.onroad_info_panel):
|
||||
inner_touch_valid = child._touch_valid_callback
|
||||
child.set_touch_valid_callback(
|
||||
lambda inner=inner_touch_valid: self._touch_valid() and (inner() if inner else True)
|
||||
)
|
||||
|
||||
def set_rect(self, rect: rl.Rectangle):
|
||||
super().set_rect(rect)
|
||||
self.road_view.set_rect(rect)
|
||||
self.onroad_info_panel.set_rect(rect)
|
||||
return self
|
||||
|
||||
def is_swiping_left(self) -> bool:
|
||||
return self.road_view.is_swiping_left() or self.onroad_info_panel.is_swiping_left()
|
||||
|
||||
def set_click_callback(self, click_callback: Callable[[], None] | None) -> None:
|
||||
self.road_view.set_click_callback(click_callback)
|
||||
self.onroad_info_panel.set_click_callback(click_callback)
|
||||
|
||||
def is_on_info_panel(self) -> bool:
|
||||
"""True when scrolled past halfway toward onroad_info_panel (used by main layout
|
||||
to skip auto-pop-back-to-camera while user is reading the info panel)."""
|
||||
return abs(self._scroller.scroll_panel.get_offset()) > self._rect.height / 2
|
||||
|
||||
def _render(self, rect: rl.Rectangle):
|
||||
if abs(self.rect.x) > gui_app.width * HORIZONTAL_RESET_RATIO:
|
||||
self._scroller.scroll_panel.set_offset(0)
|
||||
|
||||
vertical_offset = self._scroller.scroll_panel.get_offset()
|
||||
show_ball = abs(vertical_offset) < rect.height * CONFIDENCE_BALL_VISIBLE_RATIO
|
||||
self.road_view.set_show_confidence_ball(show_ball)
|
||||
|
||||
super()._render(rect)
|
||||
@@ -1,403 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import pyray as rl
|
||||
from dataclasses import dataclass, field
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
from openpilot.selfdrive.ui.mici.onroad.alert_renderer import AlertRenderer
|
||||
from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import BookmarkIcon
|
||||
|
||||
METER_TO_KM = 0.001
|
||||
METER_TO_MILE = 0.000621371
|
||||
|
||||
CONTENT_MARGIN = 16
|
||||
SPEED_LIMIT_SIGN_WIDTH = 146
|
||||
VIENNA_SIGN_SIZE = 146
|
||||
MUTCD_SIGN_HEIGHT = 178
|
||||
OFFSET_BADGE_SIZE = 50
|
||||
OFFSET_BADGE_PANEL_PADDING = 4
|
||||
MUTCD_OFFSET_SIGN_Y_SHIFT = 6
|
||||
VIENNA_BADGE_X_RATIO = 0.80
|
||||
VIENNA_BADGE_UPCOMING_X_RATIO = 0.70
|
||||
VIENNA_BADGE_Y_RATIO = -0.82
|
||||
UPCOMING_SIGN_SIZE_RATIO = 0.76
|
||||
UPCOMING_SIGN_OVERLAP_RATIO = 0.05
|
||||
UNIT_FONT_SIZE = 40
|
||||
SPEED_FONT_SIZE = 114
|
||||
ROAD_FONT_SIZE = 32
|
||||
SCC_TAG_WIDTH = 78
|
||||
SCC_TAG_HEIGHT = 30
|
||||
SCC_TAG_GAP = 5
|
||||
COLUMN_GAP = 12
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class OnroadInfoPanelColors:
|
||||
white: rl.Color = rl.WHITE
|
||||
black: rl.Color = rl.BLACK
|
||||
red: rl.Color = field(default_factory=lambda: rl.Color(255, 0, 0, 255))
|
||||
green: rl.Color = field(default_factory=lambda: rl.Color(0, 255, 0, 255))
|
||||
grey: rl.Color = field(default_factory=lambda: rl.Color(190, 195, 190, 255))
|
||||
light_grey: rl.Color = field(default_factory=lambda: rl.Color(200, 200, 200, 255))
|
||||
dark_grey: rl.Color = field(default_factory=lambda: rl.Color(100, 100, 100, 255))
|
||||
bg_dark: rl.Color = field(default_factory=lambda: rl.Color(0, 0, 0, 255))
|
||||
card_bg: rl.Color = field(default_factory=lambda: rl.Color(50, 50, 50, 200))
|
||||
badge_bg: rl.Color = field(default_factory=lambda: rl.Color(60, 60, 60, 255))
|
||||
|
||||
|
||||
COLORS = OnroadInfoPanelColors()
|
||||
|
||||
|
||||
class OnroadInfoPanel(Widget):
|
||||
def __init__(self, bookmark_callback=None):
|
||||
super().__init__()
|
||||
self.speed_limit: float = 0.0
|
||||
self.speed_limit_valid: bool = False
|
||||
self.speed_limit_offset: float = 0.0
|
||||
self.next_speed_limit: float = 0.0
|
||||
self.next_speed_limit_distance: float = 0.0
|
||||
self.road_name: str = ""
|
||||
self.current_speed: float = 0.0
|
||||
self.set_speed: float = 0.0
|
||||
self.cruise_enabled: bool = False
|
||||
|
||||
self._sign_slide: float = 0.0
|
||||
|
||||
self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
|
||||
self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD)
|
||||
self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM)
|
||||
|
||||
self._marquee_offset: float = 0.0
|
||||
self._marquee_direction: int = 1
|
||||
self._marquee_pause_timer: float = 0.0
|
||||
self._marquee_speed: float = 40.0
|
||||
self._marquee_pause_duration: float = 1.5
|
||||
|
||||
self._alert_renderer = AlertRenderer()
|
||||
self._alert_alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps)
|
||||
|
||||
self._bookmark_icon = BookmarkIcon(bookmark_callback)
|
||||
|
||||
def is_swiping_left(self) -> bool:
|
||||
return self._bookmark_icon.is_swiping_left()
|
||||
|
||||
def _handle_mouse_release(self, mouse_pos: MousePos) -> None:
|
||||
# Mirror stock AugmentedRoadView: suppress click while bookmark gesture active
|
||||
if not self._bookmark_icon.interacting():
|
||||
super()._handle_mouse_release(mouse_pos)
|
||||
|
||||
def _update_state(self) -> None:
|
||||
sm = ui_state.sm
|
||||
speed_conv = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
|
||||
|
||||
if sm.valid["longitudinalPlanSP"]:
|
||||
lp_sp = sm["longitudinalPlanSP"]
|
||||
resolver = lp_sp.speedLimit.resolver
|
||||
self.speed_limit = resolver.speedLimit * speed_conv
|
||||
self.speed_limit_valid = resolver.speedLimitValid
|
||||
self.speed_limit_offset = resolver.speedLimitOffset * speed_conv
|
||||
|
||||
if sm.valid["liveMapDataSP"]:
|
||||
lmd = sm["liveMapDataSP"]
|
||||
self.next_speed_limit = lmd.speedLimitAhead * speed_conv
|
||||
self.next_speed_limit_distance = lmd.speedLimitAheadDistance
|
||||
self.road_name = lmd.roadName
|
||||
|
||||
if sm.updated["carState"]:
|
||||
self.current_speed = sm["carState"].vEgo * speed_conv
|
||||
|
||||
if sm.valid["carState"] and sm.valid["controlsState"]:
|
||||
self.cruise_enabled = sm["carState"].cruiseState.enabled
|
||||
v_cruise_cluster = sm["carState"].vCruiseCluster
|
||||
set_speed_kph = sm["controlsState"].vCruiseDEPRECATED if v_cruise_cluster == 0.0 else v_cruise_cluster
|
||||
self.set_speed = set_speed_kph * (METER_TO_MILE / METER_TO_KM) if not ui_state.is_metric else set_speed_kph
|
||||
|
||||
def _render(self, rect: rl.Rectangle) -> None:
|
||||
self._update_state()
|
||||
|
||||
rl.draw_rectangle(int(rect.x), int(rect.y), int(rect.width), int(rect.height), COLORS.bg_dark)
|
||||
|
||||
left_x = rect.x + CONTENT_MARGIN
|
||||
|
||||
if self.cruise_enabled:
|
||||
unit = tr("MAX")
|
||||
display_speed = self.set_speed
|
||||
else:
|
||||
unit = tr("km/h") if ui_state.is_metric else tr("MPH")
|
||||
display_speed = self.current_speed
|
||||
|
||||
display_speed_text = str(round(display_speed))
|
||||
if self.speed_limit_valid and display_speed > self.speed_limit:
|
||||
speed_color = COLORS.red
|
||||
else:
|
||||
speed_color = COLORS.white
|
||||
|
||||
sign_width = min(SPEED_LIMIT_SIGN_WIDTH, rect.width * 0.30)
|
||||
sign_height = VIENNA_SIGN_SIZE if ui_state.is_metric else MUTCD_SIGN_HEIGHT
|
||||
|
||||
has_upcoming_limit = self.next_speed_limit > 0 and self.next_speed_limit != self.speed_limit
|
||||
target_sign_slide = 1.0 if has_upcoming_limit else 0.0
|
||||
slide_speed = 3.0 * rl.get_frame_time()
|
||||
if self._sign_slide < target_sign_slide:
|
||||
self._sign_slide = min(self._sign_slide + slide_speed, target_sign_slide)
|
||||
elif self._sign_slide > target_sign_slide:
|
||||
self._sign_slide = max(self._sign_slide - slide_speed, target_sign_slide)
|
||||
|
||||
upcoming_width = int(sign_width * UPCOMING_SIGN_SIZE_RATIO)
|
||||
upcoming_height = int(sign_height * UPCOMING_SIGN_SIZE_RATIO)
|
||||
upcoming_reserved_width = int(upcoming_width * 0.85) + 5
|
||||
sign_x_without_upcoming = rect.x + rect.width - sign_width - CONTENT_MARGIN
|
||||
sign_x_with_upcoming = rect.x + rect.width - sign_width - CONTENT_MARGIN - upcoming_reserved_width
|
||||
sign_x = sign_x_without_upcoming + (sign_x_with_upcoming - sign_x_without_upcoming) * self._sign_slide
|
||||
sign_y = rect.y + (rect.height - sign_height) / 2
|
||||
if not ui_state.is_metric and self.speed_limit_offset != 0 and self.speed_limit_valid:
|
||||
sign_y += MUTCD_OFFSET_SIGN_Y_SHIFT
|
||||
|
||||
readout_right = sign_x - COLUMN_GAP
|
||||
readout_width = max(1, readout_right - left_x)
|
||||
road_y = rect.y + rect.height - 44
|
||||
|
||||
unit_font_size = self._fit_font_size(self._font_semi_bold, unit, readout_width, 46, UNIT_FONT_SIZE, 28)
|
||||
speed_font_size = self._fit_font_size(self._font_bold, display_speed_text, readout_width, road_y - (rect.y + 54) - 8,
|
||||
SPEED_FONT_SIZE, 76)
|
||||
speed_size = measure_text_cached(self._font_bold, display_speed_text, speed_font_size)
|
||||
speed_y = min(rect.y + 54, road_y - speed_size.y - 8)
|
||||
unit_y = max(rect.y + 14, speed_y - unit_font_size - 6)
|
||||
|
||||
rl.draw_text_ex(self._font_semi_bold, unit, rl.Vector2(left_x, unit_y), unit_font_size, 0, COLORS.grey)
|
||||
rl.draw_text_ex(self._font_bold, display_speed_text, rl.Vector2(left_x, speed_y), speed_font_size, 0, speed_color)
|
||||
self._draw_road_name(left_x, road_y, readout_width)
|
||||
|
||||
if has_upcoming_limit and self._sign_slide > 0.01:
|
||||
upcoming_speed_text = str(round(self.next_speed_limit))
|
||||
distance_text = self._format_distance(self.next_speed_limit_distance)
|
||||
upcoming_x = sign_x + sign_width - int(upcoming_width * UPCOMING_SIGN_OVERLAP_RATIO)
|
||||
upcoming_y = sign_y + (sign_height - upcoming_height) / 2
|
||||
|
||||
upcoming_speed_color = COLORS.black
|
||||
if ui_state.is_metric:
|
||||
self._draw_vienna_sign(upcoming_x, upcoming_y, upcoming_width, upcoming_height, upcoming_speed_text, upcoming_speed_color, is_upcoming=True)
|
||||
else:
|
||||
self._draw_mutcd_sign(upcoming_x, upcoming_y, upcoming_width, upcoming_height, upcoming_speed_text, upcoming_speed_color, is_upcoming=True)
|
||||
|
||||
distance_font_size = self._fit_font_size(self._font_medium, distance_text, upcoming_width, 30, 24, 16)
|
||||
distance_size = measure_text_cached(self._font_medium, distance_text, distance_font_size)
|
||||
rl.draw_text_ex(self._font_medium, distance_text, rl.Vector2(upcoming_x + upcoming_width / 2 - distance_size.x / 2, upcoming_y + upcoming_height),
|
||||
distance_font_size, 0, COLORS.grey)
|
||||
|
||||
self._draw_speed_limit_sign(sign_x, sign_y, sign_width, sign_height)
|
||||
|
||||
if self.speed_limit_offset != 0 and self.speed_limit_valid:
|
||||
offset_text = str(abs(round(self.speed_limit_offset)))
|
||||
badge_size = OFFSET_BADGE_SIZE
|
||||
badge_rect = self._offset_badge_rect(rect, sign_x, sign_y, sign_width, sign_height, badge_size, has_upcoming_limit)
|
||||
|
||||
if ui_state.is_metric:
|
||||
badge_radius = badge_size / 2
|
||||
badge_center_x = badge_rect.x + badge_radius
|
||||
badge_center_y = badge_rect.y + badge_radius
|
||||
rl.draw_circle(int(badge_center_x), int(badge_center_y), badge_radius + 2, COLORS.dark_grey)
|
||||
rl.draw_circle(int(badge_center_x), int(badge_center_y), badge_radius, COLORS.badge_bg)
|
||||
self._draw_text_centered_fit(self._font_bold, offset_text, 32, rl.Vector2(badge_center_x, badge_center_y), COLORS.white,
|
||||
badge_size - 10, badge_size - 8, min_size=24)
|
||||
else:
|
||||
rl.draw_rectangle_rounded(badge_rect, 0.25, 10, COLORS.badge_bg)
|
||||
rl.draw_rectangle_rounded_lines_ex(badge_rect, 0.25, 10, 2, COLORS.dark_grey)
|
||||
self._draw_text_centered_fit(self._font_bold, offset_text, 32, rl.Vector2(badge_rect.x + badge_size / 2, badge_rect.y + badge_size / 2),
|
||||
COLORS.white, badge_size - 10, badge_size - 8, min_size=24)
|
||||
|
||||
scc_tag_x = min(left_x + speed_size.x + COLUMN_GAP, readout_right - SCC_TAG_WIDTH)
|
||||
scc_tag_y = speed_y + (speed_size.y - (SCC_TAG_HEIGHT * 2 + SCC_TAG_GAP)) / 2
|
||||
if scc_tag_x >= left_x + speed_size.x + 8:
|
||||
self._draw_scc_icons(scc_tag_x, scc_tag_y, readout_right)
|
||||
|
||||
self._bookmark_icon.render(rect)
|
||||
|
||||
if ui_state.started:
|
||||
alert_obj, no_alert = self._alert_renderer.will_render()
|
||||
self._alert_alpha_filter.update(0 if no_alert else 1)
|
||||
alpha = self._alert_alpha_filter.x
|
||||
if alpha > 0.01:
|
||||
rl.draw_rectangle(int(rect.x), int(rect.y), int(rect.width), int(rect.height), rl.Color(0, 0, 0, int(150 * alpha)))
|
||||
self._alert_renderer.render(rect)
|
||||
|
||||
def _draw_scc_icons(self, x: float, y: float, right_limit: float) -> None:
|
||||
sm = ui_state.sm
|
||||
if not sm.valid["longitudinalPlanSP"]:
|
||||
return
|
||||
scc = sm["longitudinalPlanSP"].smartCruiseControl
|
||||
|
||||
drawn = 0
|
||||
|
||||
for label, active in [("SCC-V", scc.vision.active), ("SCC-M", scc.map.active)]:
|
||||
if not active:
|
||||
continue
|
||||
tag_x = x
|
||||
if tag_x + SCC_TAG_WIDTH > right_limit:
|
||||
return
|
||||
tag_y = y + drawn * (SCC_TAG_HEIGHT + SCC_TAG_GAP)
|
||||
rl.draw_rectangle_rounded(rl.Rectangle(tag_x, tag_y, SCC_TAG_WIDTH, SCC_TAG_HEIGHT), 0.3, 10, COLORS.green)
|
||||
self._draw_text_centered_fit(self._font_bold, label, 18, rl.Vector2(tag_x + SCC_TAG_WIDTH / 2, tag_y + SCC_TAG_HEIGHT / 2), COLORS.black,
|
||||
SCC_TAG_WIDTH - 10, SCC_TAG_HEIGHT - 4, min_size=14)
|
||||
drawn += 1
|
||||
|
||||
def _draw_speed_limit_sign(self, x: float, y: float, sign_width: float, sign_height: float) -> None:
|
||||
speed_str = str(round(self.speed_limit)) if self.speed_limit_valid and self.speed_limit > 0 else "--"
|
||||
speed_color = COLORS.black if not self.speed_limit_valid or self.current_speed <= self.speed_limit else COLORS.red
|
||||
|
||||
if ui_state.is_metric:
|
||||
self._draw_vienna_sign(x, y, sign_width, sign_height, speed_str, speed_color, is_upcoming=False)
|
||||
else:
|
||||
self._draw_mutcd_sign(x, y, sign_width, sign_height, speed_str, speed_color, is_upcoming=False)
|
||||
|
||||
def _draw_road_name(self, x: float, y: float, width: float) -> None:
|
||||
if width <= 0:
|
||||
return
|
||||
|
||||
road_display = self.road_name if self.road_name else "--"
|
||||
font_size = self._fit_font_size(self._font_semi_bold, road_display, width, 38, ROAD_FONT_SIZE, 28)
|
||||
road_size = measure_text_cached(self._font_semi_bold, road_display, font_size)
|
||||
text_width = road_size.x
|
||||
|
||||
if text_width <= width:
|
||||
self._marquee_offset = 0.0
|
||||
self._marquee_direction = 1
|
||||
self._marquee_pause_timer = 0.0
|
||||
rl.draw_text_ex(self._font_semi_bold, road_display, rl.Vector2(x, y), font_size, 0, COLORS.white)
|
||||
else:
|
||||
overflow = text_width - width
|
||||
dt = rl.get_frame_time()
|
||||
|
||||
if self._marquee_pause_timer > 0:
|
||||
self._marquee_pause_timer -= dt
|
||||
else:
|
||||
self._marquee_offset += self._marquee_direction * self._marquee_speed * dt
|
||||
|
||||
if self._marquee_offset >= overflow:
|
||||
self._marquee_offset = overflow
|
||||
self._marquee_direction = -1
|
||||
self._marquee_pause_timer = self._marquee_pause_duration
|
||||
elif self._marquee_offset <= 0:
|
||||
self._marquee_offset = 0
|
||||
self._marquee_direction = 1
|
||||
self._marquee_pause_timer = self._marquee_pause_duration
|
||||
|
||||
rl.begin_scissor_mode(int(x), int(y), int(width), int(road_size.y + 4))
|
||||
text_pos = rl.Vector2(x - self._marquee_offset, y)
|
||||
rl.draw_text_ex(self._font_semi_bold, road_display, text_pos, font_size, 0, COLORS.white)
|
||||
rl.end_scissor_mode()
|
||||
|
||||
def _draw_vienna_sign(self, x: float, y: float, width: float, height: float, speed_str: str, speed_color: rl.Color, is_upcoming: bool = False) -> None:
|
||||
center = rl.Vector2(x + width / 2, y + height / 2)
|
||||
outer_radius = min(width, height) / 2
|
||||
|
||||
rl.draw_circle_v(center, outer_radius, COLORS.white)
|
||||
ring_width = outer_radius * 0.18
|
||||
rl.draw_ring(center, outer_radius - ring_width, outer_radius, 0, 360, 36, COLORS.red)
|
||||
|
||||
font_size = outer_radius * (0.7 if len(speed_str) >= 3 else 0.9)
|
||||
self._draw_text_centered_fit(self._font_bold, speed_str, int(font_size), center, speed_color, width * 0.72, height * 0.50, min_size=24)
|
||||
|
||||
def _draw_mutcd_sign(self, x: float, y: float, width: float, height: float, speed_str: str, speed_color: rl.Color, is_upcoming: bool = False) -> None:
|
||||
sign_rect = rl.Rectangle(x, y, width, height)
|
||||
rl.draw_rectangle_rounded(sign_rect, 0.35, 10, COLORS.white)
|
||||
|
||||
inset = max(4, width * 0.05)
|
||||
inner_rect = rl.Rectangle(x + inset, y + inset, width - inset * 2, height - inset * 2)
|
||||
outer_radius = 0.35 * width / 2.0
|
||||
inner_radius = outer_radius - inset
|
||||
inner_roundness = inner_radius / (inner_rect.width / 2.0)
|
||||
rl.draw_rectangle_rounded_lines_ex(inner_rect, inner_roundness, 10, 3, COLORS.black)
|
||||
|
||||
mid_x = x + width / 2
|
||||
label_size = max(18, int(width * 0.26))
|
||||
if is_upcoming:
|
||||
self._draw_text_centered_fit(self._font_bold, tr("AHEAD"), int(width * 0.34), rl.Vector2(mid_x, y + height * 0.28), COLORS.black,
|
||||
width * 0.94, height * 0.32, min_size=20)
|
||||
else:
|
||||
self._draw_text_centered_fit(self._font_bold, tr("SPEED"), label_size, rl.Vector2(mid_x, y + height * 0.20), COLORS.black,
|
||||
width * 0.84, height * 0.24, min_size=16)
|
||||
self._draw_text_centered_fit(self._font_bold, tr("LIMIT"), label_size, rl.Vector2(mid_x, y + height * 0.40), COLORS.black,
|
||||
width * 0.84, height * 0.24, min_size=16)
|
||||
|
||||
speed_font_size = int(width * 0.60) if len(speed_str) >= 3 else int(width * 0.72)
|
||||
self._draw_text_centered_fit(self._font_bold, speed_str, speed_font_size, rl.Vector2(mid_x, y + height * 0.72), speed_color,
|
||||
width * 0.90, height * 0.52, min_size=32)
|
||||
|
||||
def _draw_text_centered(self, font, text, size, pos_center, color):
|
||||
sz = measure_text_cached(font, text, size)
|
||||
rl.draw_text_ex(font, text, rl.Vector2(pos_center.x - sz.x / 2, pos_center.y - sz.y / 2), size, 0, color)
|
||||
|
||||
def _draw_text_centered_fit(self, font, text, size, pos_center, color, max_width: float, max_height: float, min_size: int = 10):
|
||||
size = self._fit_font_size(font, text, max_width, max_height, size, min_size)
|
||||
self._draw_text_centered(font, text, size, pos_center, color)
|
||||
|
||||
def _fit_font_size(self, font, text: str, max_width: float, max_height: float, max_size: int | float, min_size: int) -> int:
|
||||
size = int(max_size)
|
||||
while size > min_size:
|
||||
text_size = measure_text_cached(font, text, size)
|
||||
if text_size.x <= max_width and text_size.y <= max_height:
|
||||
return size
|
||||
size -= 2
|
||||
return min_size
|
||||
|
||||
def _offset_badge_rect(self, panel_rect: rl.Rectangle, sign_x: float, sign_y: float, sign_width: float, sign_height: float,
|
||||
badge_size: float, has_upcoming_limit: bool) -> rl.Rectangle:
|
||||
if ui_state.is_metric:
|
||||
radius = min(sign_width, sign_height) / 2
|
||||
center_x = sign_x + sign_width / 2
|
||||
center_y = sign_y + sign_height / 2
|
||||
badge_x_ratio = VIENNA_BADGE_UPCOMING_X_RATIO if has_upcoming_limit else VIENNA_BADGE_X_RATIO
|
||||
badge_center_x = center_x + radius * badge_x_ratio
|
||||
badge_center_y = center_y + radius * VIENNA_BADGE_Y_RATIO
|
||||
badge_x = badge_center_x - badge_size / 2
|
||||
badge_y = badge_center_y - badge_size / 2
|
||||
else:
|
||||
badge_x = sign_x + sign_width - badge_size * 0.45
|
||||
badge_y = sign_y - badge_size * 0.75
|
||||
|
||||
return rl.Rectangle(
|
||||
self._clamp(
|
||||
badge_x,
|
||||
panel_rect.x + OFFSET_BADGE_PANEL_PADDING,
|
||||
panel_rect.x + panel_rect.width - badge_size - OFFSET_BADGE_PANEL_PADDING,
|
||||
),
|
||||
self._clamp(
|
||||
badge_y,
|
||||
panel_rect.y + OFFSET_BADGE_PANEL_PADDING,
|
||||
panel_rect.y + panel_rect.height - badge_size - OFFSET_BADGE_PANEL_PADDING,
|
||||
),
|
||||
badge_size,
|
||||
badge_size,
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _clamp(value: float, min_value: float, max_value: float) -> float:
|
||||
return max(min_value, min(max_value, value))
|
||||
|
||||
def _format_distance(self, distance: float) -> str:
|
||||
if ui_state.is_metric:
|
||||
if distance < 50:
|
||||
return tr("Near")
|
||||
if distance >= 1000:
|
||||
return f"{distance * METER_TO_KM:.1f}" + tr("km")
|
||||
if distance < 200:
|
||||
rounded = max(10, int(distance / 10) * 10)
|
||||
else:
|
||||
rounded = int(distance / 100) * 100
|
||||
return str(rounded) + tr("m")
|
||||
else:
|
||||
distance_mi = distance * METER_TO_MILE
|
||||
if distance_mi < 0.1:
|
||||
return tr("Near")
|
||||
return f"{distance_mi:.1f}" + tr("mi")
|
||||
@@ -1,29 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import AugmentedRoadView
|
||||
|
||||
|
||||
class _SuppressedConfidenceBall:
|
||||
def render(self, *_):
|
||||
pass
|
||||
|
||||
|
||||
class AugmentedRoadViewSP(AugmentedRoadView):
|
||||
def __init__(self, **kwargs):
|
||||
super().__init__(**kwargs)
|
||||
self._show_confidence_ball: bool = True
|
||||
self._real_confidence_ball = self._confidence_ball
|
||||
self._confidence_ball = _SuppressedConfidenceBall()
|
||||
|
||||
def set_show_confidence_ball(self, show: bool) -> None:
|
||||
self._show_confidence_ball = show
|
||||
|
||||
def _render(self, _) -> None:
|
||||
super()._render(_)
|
||||
if self._show_confidence_ball:
|
||||
self._real_confidence_ball.render(self.rect)
|
||||
@@ -1,83 +0,0 @@
|
||||
import pyray as rl
|
||||
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.system.ui.lib.application import MouseEvent, MousePos, gui_app
|
||||
from openpilot.system.ui.lib.scroll_panel2 import ScrollState
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
from openpilot.system.ui.widgets import scroller as scroller_mod
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroll_panel_sp import GuiScrollPanel2SP
|
||||
|
||||
|
||||
class DummyScrollIndicator:
|
||||
def update(self, *_) -> None:
|
||||
pass
|
||||
|
||||
def render(self) -> None:
|
||||
pass
|
||||
|
||||
|
||||
class DummyWidget(Widget):
|
||||
def __init__(self, rect: rl.Rectangle):
|
||||
super().__init__()
|
||||
self.set_rect(rect)
|
||||
|
||||
def _render(self, _) -> None:
|
||||
pass
|
||||
|
||||
|
||||
def _mouse_event(x: float, y: float, *, pressed: bool = False, released: bool = False,
|
||||
down: bool = True, t: float = 0.0) -> MouseEvent:
|
||||
return MouseEvent(MousePos(x, y), 0, pressed, released, down, t)
|
||||
|
||||
|
||||
class TestScrollerSP(OpenpilotTestCase):
|
||||
def test_vertical_snap_items_are_supported(self, monkeypatch):
|
||||
monkeypatch.setattr(scroller_mod, "ScrollIndicator", DummyScrollIndicator)
|
||||
|
||||
scroller = scroller_mod._Scroller([], horizontal=False, snap_items=True, scroll_indicator=False)
|
||||
scroller.set_rect(rl.Rectangle(0, 0, 100, 100))
|
||||
scroller.scroll_panel.set_offset(-60)
|
||||
|
||||
captured_snap_target = None
|
||||
|
||||
def update(_, __, snap_target=None):
|
||||
nonlocal captured_snap_target
|
||||
captured_snap_target = snap_target
|
||||
return scroller.scroll_panel.get_offset()
|
||||
|
||||
monkeypatch.setattr(scroller.scroll_panel, "update", update)
|
||||
|
||||
visible_items: list[Widget] = [
|
||||
DummyWidget(rl.Rectangle(0, -60, 100, 100)),
|
||||
DummyWidget(rl.Rectangle(0, 40, 100, 100)),
|
||||
]
|
||||
scroller._get_scroll(visible_items, 200)
|
||||
|
||||
assert captured_snap_target == -100
|
||||
|
||||
def test_scroll_panel_sp_rejects_orthogonal_drags(self, monkeypatch):
|
||||
panel = GuiScrollPanel2SP(horizontal=True)
|
||||
bounds = rl.Rectangle(0, 0, 100, 100)
|
||||
|
||||
monkeypatch.setattr(gui_app, "_mouse_events", [_mouse_event(10, 10, pressed=True, t=1.0)])
|
||||
panel.update(bounds, 200)
|
||||
assert panel.state == ScrollState.PRESSED
|
||||
|
||||
monkeypatch.setattr(gui_app, "_mouse_events", [_mouse_event(23, 60, t=1.1)])
|
||||
panel.update(bounds, 200)
|
||||
|
||||
assert panel.state == ScrollState.STEADY
|
||||
assert panel.get_offset() == 0
|
||||
|
||||
def test_scroll_panel_sp_can_disable_out_of_bounds_handling(self, monkeypatch):
|
||||
panel = GuiScrollPanel2SP(horizontal=False, handle_out_of_bounds=False)
|
||||
bounds = rl.Rectangle(0, 0, 100, 100)
|
||||
monkeypatch.setattr(gui_app, "_mouse_events", [])
|
||||
|
||||
panel.set_offset(20)
|
||||
panel.update(bounds, 200)
|
||||
assert panel.get_offset() == 0
|
||||
|
||||
panel.set_offset(-150)
|
||||
panel.update(bounds, 200)
|
||||
assert panel.get_offset() == -100
|
||||
@@ -1,33 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import pyray as rl
|
||||
from openpilot.system.ui.lib.application import MouseEvent
|
||||
from openpilot.system.ui.lib.scroll_panel2 import GuiScrollPanel2, ScrollState
|
||||
|
||||
|
||||
class GuiScrollPanel2SP(GuiScrollPanel2):
|
||||
"""Scroll panel behavior for nested Mici pagers."""
|
||||
|
||||
def __init__(self, horizontal: bool = True, handle_out_of_bounds: bool = True) -> None:
|
||||
super().__init__(horizontal, handle_out_of_bounds=handle_out_of_bounds)
|
||||
|
||||
def _handle_mouse_event(self, mouse_event: MouseEvent, bounds: rl.Rectangle, bounds_size: float,
|
||||
content_size: float) -> None:
|
||||
state_before_update = self._state
|
||||
super()._handle_mouse_event(mouse_event, bounds, bounds_size, content_size)
|
||||
|
||||
if self._state == ScrollState.MANUAL_SCROLL and state_before_update == ScrollState.PRESSED and \
|
||||
self._initial_click_event is not None:
|
||||
drag_x = abs(mouse_event.pos.x - self._initial_click_event.pos.x)
|
||||
drag_y = abs(mouse_event.pos.y - self._initial_click_event.pos.y)
|
||||
primary_drag = drag_x if self._horizontal else drag_y
|
||||
cross_drag = drag_y if self._horizontal else drag_x
|
||||
if cross_drag > primary_drag:
|
||||
self._state = ScrollState.STEADY
|
||||
self._velocity = 0.0
|
||||
self._velocity_buffer.clear()
|
||||
@@ -1,16 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from openpilot.system.ui.widgets.scroller import Scroller
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroll_panel_sp import GuiScrollPanel2SP
|
||||
|
||||
|
||||
class ScrollerSP(Scroller):
|
||||
def __init__(self, **kwargs):
|
||||
super().__init__(**kwargs)
|
||||
inner = self._scroller
|
||||
inner.scroll_panel = GuiScrollPanel2SP(inner._horizontal, handle_out_of_bounds=not inner._snap_items)
|
||||
@@ -4,11 +4,23 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.chevron_metrics import ChevronMetrics
|
||||
from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
|
||||
|
||||
class ModelRendererSP:
|
||||
def __init__(self):
|
||||
self.rainbow_path = RainbowPath()
|
||||
self.chevron_metrics = ChevronMetrics()
|
||||
self._width_filter = FirstOrderFilter(0.9, 0.1, 1 / gui_app.target_fps)
|
||||
|
||||
@property
|
||||
def _lateral_active(self) -> bool:
|
||||
return ui_state.status in (UIStatus.ENGAGED, UIStatus.LAT_ONLY)
|
||||
|
||||
def _get_path_half_width(self) -> float:
|
||||
target = 0.9 if self._lateral_active else 0.40
|
||||
return self._width_filter.update(target)
|
||||
|
||||
@@ -10,9 +10,6 @@ from openpilot.selfdrive.ui.layouts.main import MainLayout
|
||||
from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
|
||||
if gui_app.sunnypilot_ui():
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.layouts.main import MiciMainLayoutSP as MiciMainLayout
|
||||
|
||||
BIG_UI = gui_app.big_ui()
|
||||
|
||||
|
||||
|
||||
@@ -1 +1 @@
|
||||
#define SUNNYPILOT_VERSION "2026.08.20-4680"
|
||||
#define SUNNYPILOT_VERSION "2026.08.22-4716"
|
||||
|
||||
@@ -8,7 +8,10 @@ from openpilot.common.params import Params
|
||||
|
||||
|
||||
def get_lat_delay(params: Params, stock_lat_delay: float) -> float:
|
||||
if params.get_bool("LagdToggle"):
|
||||
return float(params.get("LagdValueCache", return_default=True))
|
||||
# live learning on: use what lagd publishes.
|
||||
# off: use the fixed steerActuatorDelay + software delay sum that LagdToggle caches.
|
||||
|
||||
return stock_lat_delay
|
||||
if params.get_bool("LagdToggle"):
|
||||
return stock_lat_delay
|
||||
|
||||
return float(params.get("LagdValueCache", return_default=True))
|
||||
|
||||
@@ -272,18 +272,17 @@ def _parse_size(size_str: str) -> tuple[int, int]:
|
||||
return int(width), int(height)
|
||||
|
||||
|
||||
def read_file_chunked_to_shm(path):
|
||||
def read_file_chunked_to_disk(path):
|
||||
if not path:
|
||||
return None
|
||||
import atexit
|
||||
import shutil
|
||||
from openpilot.common.file_chunker import open_file_chunked
|
||||
from openpilot.common.hardware.hw import Paths
|
||||
shm_path = os.path.join(Paths.shm_path(), os.path.basename(path))
|
||||
atexit.register(lambda: os.path.exists(shm_path) and os.remove(shm_path))
|
||||
with open(shm_path, 'wb') as dst, open_file_chunked(path) as src:
|
||||
shutil.copyfileobj(src, dst)
|
||||
return shm_path
|
||||
tmp_path = f'{path}.unchunked'
|
||||
with open(tmp_path, 'wb') as f, open_file_chunked(path) as src:
|
||||
shutil.copyfileobj(src, f)
|
||||
atexit.register(lambda: os.path.exists(tmp_path) and os.remove(tmp_path))
|
||||
return tmp_path
|
||||
|
||||
|
||||
def _load_policy_runners(args: argparse.Namespace) -> tuple[list, list]:
|
||||
@@ -327,11 +326,11 @@ if __name__ == "__main__":
|
||||
model_w, model_h = args.model_size
|
||||
output_data = {}
|
||||
|
||||
args.vision_onnx = read_file_chunked_to_shm(args.vision_onnx)
|
||||
args.policy_onnx = read_file_chunked_to_shm(args.policy_onnx)
|
||||
args.off_policy_onnx = read_file_chunked_to_shm(args.off_policy_onnx)
|
||||
args.on_policy_onnx = read_file_chunked_to_shm(args.on_policy_onnx)
|
||||
args.supercombo_onnx = read_file_chunked_to_shm(args.supercombo_onnx)
|
||||
args.vision_onnx = read_file_chunked_to_disk(args.vision_onnx)
|
||||
args.policy_onnx = read_file_chunked_to_disk(args.policy_onnx)
|
||||
args.off_policy_onnx = read_file_chunked_to_disk(args.off_policy_onnx)
|
||||
args.on_policy_onnx = read_file_chunked_to_disk(args.on_policy_onnx)
|
||||
args.supercombo_onnx = read_file_chunked_to_disk(args.supercombo_onnx)
|
||||
|
||||
vision_runner = OnnxRunner(args.vision_onnx) if args.vision_onnx else None
|
||||
|
||||
|
||||
@@ -5,10 +5,15 @@ This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import os
|
||||
import tempfile
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from openpilot.common.parameterized import parameterized
|
||||
|
||||
from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, _detect_desire_key
|
||||
from openpilot.common.file_chunker import chunk_file, get_chunk_targets
|
||||
from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, _detect_desire_key, read_file_chunked_to_disk
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
|
||||
|
||||
@@ -160,3 +165,33 @@ class TestOutputSlicePreservation(OpenpilotTestCase):
|
||||
policy_slices = {'plan': slice(0, 495), 'meta': slice(495, 550)}
|
||||
assert set(vision_slices.keys()) & set(policy_slices.keys()) == set(), \
|
||||
"vision and policy slices should not overlap in keys"
|
||||
|
||||
|
||||
class TestReadFileChunkedToDisk(OpenpilotTestCase):
|
||||
def test_none_passthrough(self):
|
||||
assert read_file_chunked_to_disk(None) is None
|
||||
|
||||
def test_unchunked_source_staged_on_disk(self):
|
||||
with tempfile.TemporaryDirectory() as d:
|
||||
src = Path(d) / "driving_supercombo.onnx"
|
||||
payload = os.urandom(1024)
|
||||
src.write_bytes(payload)
|
||||
|
||||
out = Path(read_file_chunked_to_disk(str(src)))
|
||||
|
||||
assert out.parent == Path(d)
|
||||
assert out.name == "driving_supercombo.onnx.unchunked"
|
||||
assert out.read_bytes() == payload
|
||||
|
||||
def test_chunked_source_reassembled_on_disk(self):
|
||||
with tempfile.TemporaryDirectory() as d:
|
||||
src = Path(d) / "driving_supercombo.onnx"
|
||||
payload = os.urandom(4096)
|
||||
src.write_bytes(payload)
|
||||
chunk_file(str(src), get_chunk_targets(str(src), len(payload)))
|
||||
assert not src.exists()
|
||||
|
||||
out = Path(read_file_chunked_to_disk(str(src)))
|
||||
|
||||
assert out.parent == Path(d)
|
||||
assert out.read_bytes() == payload
|
||||
|
||||
@@ -3,8 +3,17 @@ import os
|
||||
import hashlib
|
||||
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.sunnypilot import get_file_hash
|
||||
from openpilot.sunnypilot.models.model_name import DEFAULT_MODEL
|
||||
from openpilot.sunnypilot.models.model_name import DEFAULT_MODEL, DEFAULT_BIG_MODEL
|
||||
|
||||
|
||||
def get_default_model() -> str:
|
||||
show_big_model = (ui_state.usbgpu and ui_state.usbgpu_compiled
|
||||
and (ui_state.usbgpu_active or ui_state.usbgpu_loading or ui_state.is_offroad()))
|
||||
|
||||
return DEFAULT_BIG_MODEL if show_big_model else DEFAULT_MODEL
|
||||
|
||||
|
||||
DEFAULT_MODEL_NAME_PATH = os.path.join(BASEDIR, "openpilot", "sunnypilot", "models", "model_name.py")
|
||||
MODEL_HASH_PATH = os.path.join(BASEDIR, "openpilot", "sunnypilot", "models", "tests", "model_hash")
|
||||
@@ -13,7 +22,6 @@ SUPERCOMBO_ONNX_PATH = os.path.join(BASEDIR, "openpilot", "selfdrive", "modeld",
|
||||
|
||||
def update_model_hash():
|
||||
supercombo_hash = get_file_hash(SUPERCOMBO_ONNX_PATH)
|
||||
|
||||
combined_hash = hashlib.sha256(supercombo_hash.encode()).hexdigest()
|
||||
|
||||
with open(MODEL_HASH_PATH, "w") as f:
|
||||
@@ -22,40 +30,28 @@ def update_model_hash():
|
||||
print(f"Generated and updated new combined model hash to {MODEL_HASH_PATH}")
|
||||
|
||||
|
||||
def get_current_default_model_name():
|
||||
print("[GET DEFAULT MODEL NAME]")
|
||||
name = DEFAULT_MODEL
|
||||
print(f'Current default model name: "{name}"')
|
||||
|
||||
return name
|
||||
|
||||
|
||||
def update_default_model_name(name: str):
|
||||
print("[CHANGE DEFAULT MODEL NAME]")
|
||||
def update_default_model_names(default_model_name: str, default_big_model_name: str):
|
||||
print("[CHANGE DEFAULT MODEL NAMES]")
|
||||
with open(DEFAULT_MODEL_NAME_PATH, "w") as f:
|
||||
f.write(f'DEFAULT_MODEL = "{name}"\n')
|
||||
print(f'New default model name: "{name}"')
|
||||
f.write(f'DEFAULT_MODEL = "{default_model_name}"\n')
|
||||
f.write(f'DEFAULT_BIG_MODEL = "{default_big_model_name}"\n')
|
||||
|
||||
print(f'New default small model name: "{default_model_name}"')
|
||||
print(f'New default big model name: "{default_big_model_name}"')
|
||||
print("[DONE]")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Update default model name and hash")
|
||||
parser.add_argument("--new_name", type=str, help="New default model name")
|
||||
parser = argparse.ArgumentParser(description="Update default model names and hash")
|
||||
parser.add_argument("--new_small_model_name", type=str, help="New default small model name")
|
||||
parser.add_argument("--new_big_model_name", type=str, help="New default big model name")
|
||||
args = parser.parse_args()
|
||||
|
||||
if not args.new_name:
|
||||
print("Warning: No new default model name provided. Use --new_name to specify")
|
||||
print("Default model name and hash will not be updated! (aborted)")
|
||||
exit(0)
|
||||
if args.new_small_model_name is None and args.new_big_model_name is None:
|
||||
new_name = input(f'Enter new default small model name (current: "{DEFAULT_MODEL}", leave empty to keep): ').strip()
|
||||
new_big_model_name = input(f'Enter new default big model name (current: "{DEFAULT_BIG_MODEL}", leave empty to keep): ').strip()
|
||||
else:
|
||||
new_name, new_big_model_name = args.new_small_model_name, args.new_big_model_name
|
||||
|
||||
current_name = get_current_default_model_name()
|
||||
new_name = args.new_name
|
||||
if current_name == new_name:
|
||||
print(f'Proposed default model name: "{new_name}"')
|
||||
confirm = input("Proposed default model name is the same as the current default model name. Confirm? (y/n): ").upper().strip()
|
||||
if confirm != "Y":
|
||||
print("Default model name and hash will not be updated! (aborted)")
|
||||
exit(0)
|
||||
|
||||
update_default_model_name(new_name)
|
||||
update_default_model_names(new_name or DEFAULT_MODEL, new_big_model_name or DEFAULT_BIG_MODEL)
|
||||
update_model_hash()
|
||||
|
||||
@@ -1 +1,2 @@
|
||||
DEFAULT_MODEL = "CD210"
|
||||
DEFAULT_BIG_MODEL = "Lebowski"
|
||||
|
||||
@@ -20,4 +20,4 @@ class TestDefaultModel(OpenpilotTestCase):
|
||||
with open(MODEL_HASH_PATH) as f:
|
||||
current_hash = f.read().strip()
|
||||
|
||||
assert combined_hash == current_hash, "Run sunnypilot/models/default_model.py to update the default model name and hash"
|
||||
assert combined_hash == current_hash, "Run openpilot/sunnypilot/models/default_model.py to update the default model name and hash"
|
||||
|
||||
+1
-1
@@ -115,7 +115,7 @@ class IntelligentCruiseButtonManagement:
|
||||
self.is_ready = ready and not button_pressed
|
||||
|
||||
def run(self, CS: car.CarState, CC: car.CarControl, LP_SP: custom.LongitudinalPlanSP, is_metric: bool) -> None:
|
||||
if self.CP_SP.pcmCruiseSpeed or not self.CP_SP.intelligentCruiseButtonManagementAvailable:
|
||||
if self.CP_SP.pcmCruiseSpeed:
|
||||
return
|
||||
|
||||
self.is_metric = is_metric
|
||||
|
||||
@@ -136,9 +136,6 @@ def initialize_params(params) -> list[dict[str, Any]]:
|
||||
keys.extend([
|
||||
"ToyotaEnforceStockLongitudinal",
|
||||
"ToyotaStopAndGoHack",
|
||||
"ToyotaTSS2Long",
|
||||
"ToyotaEnhancedBsm",
|
||||
"ToyotaAutoHold",
|
||||
])
|
||||
|
||||
return [{k: params.get(k, return_default=True)} for k in keys]
|
||||
|
||||
@@ -1,26 +1,14 @@
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.car import create_button_events
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.car.toyota.carstate import get_virtual_cruise_button, VIRTUAL_CRUISE_BUTTONS
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.parameterized import parameterized, parameterized_class
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.car.cruise import TOYOTA_VIRTUAL_CRUISE_LONG_PRESS, VCruiseHelper, V_CRUISE_INITIAL, V_CRUISE_UNSET
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_INITIAL
|
||||
from openpilot.selfdrive.car.tests.test_cruise_speed import TestVCruiseHelper
|
||||
from openpilot.sunnypilot.selfdrive.car.interfaces import initialize_params
|
||||
|
||||
ButtonEvent = car.CarState.ButtonEvent
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class TestToyotaParamsHandoff(OpenpilotTestCase):
|
||||
def test_tss2_long_tuning_param_is_forwarded_to_opendbc(self):
|
||||
keys = {next(iter(entry)) for entry in initialize_params(Params())}
|
||||
assert "ToyotaTSS2Long" in keys
|
||||
|
||||
|
||||
# TODO: test pcmCruise and pcmCruiseSpeed
|
||||
@parameterized_class(('pcm_cruise', 'pcm_cruise_speed'), [(False, True)])
|
||||
class TestCustomAccIncrements(TestVCruiseHelper):
|
||||
@@ -126,8 +114,8 @@ class TestCustomAccIncrements(TestVCruiseHelper):
|
||||
def test_rounding_behavior(self):
|
||||
"""Test rounding behavior for 5 and 10 increments"""
|
||||
test_cases = [
|
||||
(47, 5, 50), # 47 -> 50 (round up to next 5)
|
||||
(45, 5, 50), # 45 -> 50 (already at 5, increment by 5)
|
||||
(47, 5, 50), # 47 -> 50 (round up to next 5)
|
||||
(45, 5, 50), # 45 -> 50 (already at 5, increment by 5)
|
||||
(43, 10, 50), # 43 -> 50 (round up to next 10)
|
||||
(40, 10, 50), # 40 -> 50 (already at 10, increment by 10)
|
||||
]
|
||||
@@ -158,302 +146,3 @@ class TestCustomAccIncrements(TestVCruiseHelper):
|
||||
initial_speed = self.v_cruise_helper.v_cruise_kph
|
||||
self.press_button_long(ButtonType.accelCruise)
|
||||
assert self.v_cruise_helper.v_cruise_kph == initial_speed + 10 # Should fallback to 10
|
||||
|
||||
|
||||
class TestToyotaVirtualCruiseSpeed(OpenpilotTestCase):
|
||||
def setup_method(self):
|
||||
self.params = Params()
|
||||
self.params.put_bool("CustomAccIncrementsEnabled", True, block=True)
|
||||
self.params.put("CustomAccShortPressIncrement", 5, block=True)
|
||||
self.params.put("CustomAccLongPressIncrement", 5, block=True)
|
||||
|
||||
CP = car.CarParams(brand="toyota", pcmCruise=True, openpilotLongitudinalControl=True)
|
||||
CP_SP = custom.CarParamsSP(pcmCruiseSpeed=False)
|
||||
self.v_cruise_helper = VCruiseHelper(CP, CP_SP)
|
||||
self.v_cruise_helper.read_custom_set_speed_params()
|
||||
self.route_parser = CANParser("toyota_nodsu_pt_generated", [("CLUTCH", 16)], 0)
|
||||
self.route_button = 0
|
||||
|
||||
@staticmethod
|
||||
def car_state(canonical_kph, cluster_kph, *, available=True, standstill=False, gas_pressed=False, v_ego_kph=0.0, button_events=None):
|
||||
CS = car.CarState(
|
||||
gasPressed=gas_pressed,
|
||||
vEgo=v_ego_kph * CV.KPH_TO_MS,
|
||||
cruiseState={
|
||||
"available": available,
|
||||
"speed": canonical_kph * CV.KPH_TO_MS,
|
||||
"speedCluster": cluster_kph * CV.KPH_TO_MS,
|
||||
"standstill": standstill,
|
||||
},
|
||||
)
|
||||
CS.buttonEvents = button_events or []
|
||||
return CS
|
||||
|
||||
def seed_enabled(self, canonical_kph, cluster_kph, *, is_metric=True):
|
||||
CS = self.car_state(canonical_kph, cluster_kph)
|
||||
self.v_cruise_helper.update_v_cruise(CS, enabled=False, is_metric=is_metric)
|
||||
self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=is_metric)
|
||||
self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=is_metric)
|
||||
assert self.v_cruise_helper.v_cruise_kph == canonical_kph
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == cluster_kph
|
||||
|
||||
def press(self, button_type, canonical_kph, cluster_kph, hold_frames=0, *, standstill=False, gas_pressed=False, v_ego_kph=0.0, is_metric=True):
|
||||
pressed = [ButtonEvent(type=button_type, pressed=True)]
|
||||
self.v_cruise_helper.update_v_cruise(
|
||||
self.car_state(canonical_kph, cluster_kph, standstill=standstill, gas_pressed=gas_pressed, v_ego_kph=v_ego_kph, button_events=pressed),
|
||||
enabled=True,
|
||||
is_metric=is_metric,
|
||||
)
|
||||
for _ in range(hold_frames):
|
||||
self.v_cruise_helper.update_v_cruise(
|
||||
self.car_state(canonical_kph, cluster_kph, standstill=standstill, gas_pressed=gas_pressed, v_ego_kph=v_ego_kph),
|
||||
enabled=True,
|
||||
is_metric=is_metric,
|
||||
)
|
||||
released = [ButtonEvent(type=button_type, pressed=False)]
|
||||
self.v_cruise_helper.update_v_cruise(
|
||||
self.car_state(canonical_kph, cluster_kph, standstill=standstill, gas_pressed=gas_pressed, v_ego_kph=v_ego_kph, button_events=released),
|
||||
enabled=True,
|
||||
is_metric=is_metric,
|
||||
)
|
||||
|
||||
def set_increments(self, short_increment, long_increment):
|
||||
self.params.put("CustomAccShortPressIncrement", short_increment, block=True)
|
||||
self.params.put("CustomAccLongPressIncrement", long_increment, block=True)
|
||||
self.v_cruise_helper.read_custom_set_speed_params()
|
||||
|
||||
def assert_kph_almost_equal(self, actual, expected):
|
||||
self.assertAlmostEqual(actual, expected, delta=abs(expected) * 1e-6)
|
||||
|
||||
def route_button_events(self, payload):
|
||||
self.route_parser.update((1, [(0x361, bytes.fromhex(payload), 0)]))
|
||||
current = get_virtual_cruise_button(
|
||||
self.route_parser.vl["CLUTCH"]["CRUISE_RES"],
|
||||
self.route_parser.vl["CLUTCH"]["CRUISE_SET"],
|
||||
)
|
||||
events = create_button_events(current, self.route_button, VIRTUAL_CRUISE_BUTTONS)
|
||||
self.route_button = current
|
||||
return events
|
||||
|
||||
def test_short_press_rounds_display_target_and_preserves_offset(self):
|
||||
self.seed_enabled(27, 31)
|
||||
self.press(ButtonType.accelCruise, 28, 32)
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == 31
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == 35
|
||||
|
||||
def test_decel_at_display_minimum_does_not_increase_target(self):
|
||||
self.seed_enabled(26, 30)
|
||||
self.press(ButtonType.decelCruise, 25, 29)
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == 26
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == 30
|
||||
|
||||
@parameterized.expand((52, TOYOTA_VIRTUAL_CRUISE_LONG_PRESS - 1))
|
||||
def test_route_length_short_press_is_not_a_long_press(self, hold_frames):
|
||||
self.set_increments(short_increment=2, long_increment=5)
|
||||
self.seed_enabled(27, 31)
|
||||
self.press(ButtonType.accelCruise, 28, 32, hold_frames=hold_frames)
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == 29
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == 33
|
||||
|
||||
def test_toyota_long_press_uses_route_validated_cadence_and_suppresses_release(self):
|
||||
self.set_increments(short_increment=2, long_increment=5)
|
||||
self.seed_enabled(27, 31)
|
||||
|
||||
pressed = [ButtonEvent(type=ButtonType.accelCruise, pressed=True)]
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(31, 35, button_events=pressed), enabled=True, is_metric=True)
|
||||
for _ in range(TOYOTA_VIRTUAL_CRUISE_LONG_PRESS):
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(31, 35), enabled=True, is_metric=True)
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == 31
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == 35
|
||||
|
||||
released = [ButtonEvent(type=ButtonType.accelCruise, pressed=False)]
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(31, 35, button_events=released), enabled=True, is_metric=True)
|
||||
assert self.v_cruise_helper.v_cruise_kph == 31
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == 35
|
||||
|
||||
def test_route_4_32_second_hold_repeats_six_times(self):
|
||||
self.seed_enabled(26, 30)
|
||||
self.press(ButtonType.accelCruise, 30, 34, hold_frames=432)
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == 56
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == 60
|
||||
|
||||
def test_maximum_boundary_caps_pair_and_preserves_offset(self):
|
||||
self.seed_enabled(141, 145)
|
||||
self.press(ButtonType.accelCruise, 142, 146)
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == 141
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == 145
|
||||
|
||||
self.press(ButtonType.accelCruise, 143, 147)
|
||||
assert self.v_cruise_helper.v_cruise_kph == 141
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == 145
|
||||
|
||||
@parameterized.expand(
|
||||
(
|
||||
(25, 29, ButtonType.decelCruise),
|
||||
(141, 147, ButtonType.accelCruise),
|
||||
)
|
||||
)
|
||||
def test_out_of_range_raw_pair_is_not_moved_in_opposite_direction(self, canonical_kph, cluster_kph, button_type):
|
||||
self.seed_enabled(canonical_kph, cluster_kph)
|
||||
self.press(button_type, canonical_kph, cluster_kph)
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == canonical_kph
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == cluster_kph
|
||||
|
||||
def test_imperial_increment_preserves_canonical_cluster_pair(self):
|
||||
self.seed_enabled(45, 50, is_metric=False)
|
||||
self.press(ButtonType.accelCruise, 46, 51, is_metric=False)
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == 51
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == 56
|
||||
|
||||
def test_engagement_button_held_does_not_change_target(self):
|
||||
initial = self.car_state(27, 31)
|
||||
self.v_cruise_helper.update_v_cruise(initial, enabled=False, is_metric=True)
|
||||
|
||||
pressed = [ButtonEvent(type=ButtonType.decelCruise, pressed=True)]
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(27, 31, button_events=pressed), enabled=False, is_metric=True)
|
||||
for _ in range(TOYOTA_VIRTUAL_CRUISE_LONG_PRESS + 10):
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
|
||||
|
||||
released = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)]
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(28, 32, button_events=released), enabled=True, is_metric=True)
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == 28
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == 32
|
||||
|
||||
def test_delayed_pcm_target_seeds_before_software_ownership(self):
|
||||
invalid = self.car_state(0, 0)
|
||||
self.v_cruise_helper.update_v_cruise(invalid, enabled=False, is_metric=True)
|
||||
|
||||
release = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)]
|
||||
for _ in range(4):
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(0, 0, button_events=release), enabled=True, is_metric=True)
|
||||
assert self.v_cruise_helper.v_cruise_kph == V_CRUISE_UNSET
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == V_CRUISE_UNSET
|
||||
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(27, 31), enabled=True, is_metric=True)
|
||||
self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_kph, 27)
|
||||
self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_cluster_kph, 31)
|
||||
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
|
||||
self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_kph, 27)
|
||||
self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_cluster_kph, 31)
|
||||
|
||||
def test_route_payload_short_press_drives_virtual_target(self):
|
||||
self.seed_enabled(27, 31)
|
||||
|
||||
pressed = self.route_button_events("a61a0000561a1a81")
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(27, 31, button_events=pressed), enabled=True, is_metric=True)
|
||||
for _ in range(52):
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
|
||||
|
||||
released = self.route_button_events("861a0000561b1a81")
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(28, 32, button_events=released), enabled=True, is_metric=True)
|
||||
assert self.v_cruise_helper.v_cruise_kph == 31
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == 35
|
||||
|
||||
def test_prius_route_payload_short_set_drives_virtual_target(self):
|
||||
self.seed_enabled(31, 35)
|
||||
|
||||
pressed = self.route_button_events("965f000056666585")
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(31, 35, button_events=pressed), enabled=True, is_metric=True)
|
||||
for _ in range(45):
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(30, 34), enabled=True, is_metric=True)
|
||||
|
||||
released = self.route_button_events("865f000056666585")
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(30, 34, button_events=released), enabled=True, is_metric=True)
|
||||
assert self.v_cruise_helper.v_cruise_kph == 26
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == 30
|
||||
|
||||
def test_prius_route_payload_standstill_res_does_not_change_target(self):
|
||||
self.seed_enabled(27, 31)
|
||||
|
||||
pressed = self.route_button_events("a61b0000561c1c80")
|
||||
self.v_cruise_helper.update_v_cruise(
|
||||
self.car_state(27, 31, standstill=True, button_events=pressed),
|
||||
enabled=True,
|
||||
is_metric=True,
|
||||
)
|
||||
for _ in range(TOYOTA_VIRTUAL_CRUISE_LONG_PRESS):
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(27, 31, standstill=True), enabled=True, is_metric=True)
|
||||
|
||||
released = self.route_button_events("865f000056666585")
|
||||
self.v_cruise_helper.update_v_cruise(
|
||||
self.car_state(27, 31, standstill=True, button_events=released),
|
||||
enabled=True,
|
||||
is_metric=True,
|
||||
)
|
||||
assert self.v_cruise_helper.v_cruise_kph == 27
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == 31
|
||||
|
||||
def test_route_payload_disengage_mid_hold_clears_pending_action(self):
|
||||
self.seed_enabled(27, 31)
|
||||
|
||||
pressed = self.route_button_events("a61a0000561a1a81")
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(27, 31, button_events=pressed), enabled=True, is_metric=True)
|
||||
for _ in range(30):
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
|
||||
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=False, is_metric=True)
|
||||
released = self.route_button_events("861a0000561b1a81")
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(28, 32, available=False, button_events=released), enabled=False, is_metric=True)
|
||||
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=False, is_metric=True)
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
|
||||
self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_kph, 28)
|
||||
self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_cluster_kph, 32)
|
||||
|
||||
def test_standstill_resume_does_not_change_target(self):
|
||||
self.seed_enabled(27, 31)
|
||||
self.press(ButtonType.accelCruise, 27, 31, standstill=True)
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == 27
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == 31
|
||||
|
||||
def test_disengagement_discards_virtual_target_and_reseeds_raw_pair(self):
|
||||
self.seed_enabled(27, 31)
|
||||
self.press(ButtonType.accelCruise, 28, 32)
|
||||
assert self.v_cruise_helper.v_cruise_kph == 31
|
||||
|
||||
raw = self.car_state(28, 32)
|
||||
self.v_cruise_helper.update_v_cruise(raw, enabled=False, is_metric=True)
|
||||
self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_kph, 28)
|
||||
self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_cluster_kph, 32)
|
||||
|
||||
self.v_cruise_helper.update_v_cruise(raw, enabled=True, is_metric=True)
|
||||
self.v_cruise_helper.update_v_cruise(raw, enabled=True, is_metric=True)
|
||||
self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_kph, 28)
|
||||
self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_cluster_kph, 32)
|
||||
|
||||
def test_unavailable_and_mads_handback_discard_virtual_target(self):
|
||||
self.seed_enabled(27, 31)
|
||||
self.press(ButtonType.accelCruise, 28, 32)
|
||||
assert self.v_cruise_helper.v_cruise_kph == 31
|
||||
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=False, is_metric=True)
|
||||
self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_kph, 28)
|
||||
self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_cluster_kph, 32)
|
||||
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(0, 0, available=False), enabled=False, is_metric=True)
|
||||
assert self.v_cruise_helper.v_cruise_kph == V_CRUISE_UNSET
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == V_CRUISE_UNSET
|
||||
|
||||
self.v_cruise_helper.update_v_cruise(self.car_state(29, 33), enabled=False, is_metric=True)
|
||||
self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_kph, 29)
|
||||
self.assert_kph_almost_equal(self.v_cruise_helper.v_cruise_cluster_kph, 33)
|
||||
|
||||
def test_set_during_gas_override_clips_target_to_ego_speed(self):
|
||||
self.seed_enabled(27, 31)
|
||||
self.press(ButtonType.decelCruise, 26, 30, gas_pressed=True, v_ego_kph=50)
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == 50
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == 54
|
||||
|
||||
@@ -1,81 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.sunnypilot import get_sanitize_int_param
|
||||
|
||||
AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
|
||||
|
||||
MAX_ACCEL_PROFILES = {
|
||||
AccelProfile.eco: [1.45, 1.40, 1.20, 0.85, 0.62, 0.36, 0.22, 0.085, 0.055, 0.045],
|
||||
AccelProfile.normal: [2.00, 1.95, 1.80, 1.06, 0.81, 0.69, 0.42, 0.160, 0.10, 0.08],
|
||||
AccelProfile.sport: [2.00, 1.99, 1.95, 1.45, 1.10, 0.82, 0.53, 0.240, 0.13, 0.09],
|
||||
}
|
||||
MAX_ACCEL_BREAKPOINTS = [0., 3., 5., 8., 12., 18., 24., 32., 42., 55.]
|
||||
|
||||
MIN_ACCEL_PROFILES = {
|
||||
AccelProfile.eco: [-0.90, -0.95, -1.00, -1.10, -1.2],
|
||||
AccelProfile.normal: [-1.00, -1.05, -1.10, -1.20, -1.3],
|
||||
AccelProfile.sport: [-1.10, -1.15, -1.20, -1.30, -1.4],
|
||||
}
|
||||
MIN_ACCEL_BREAKPOINTS = [3., 4.5, 7., 9., 25.]
|
||||
|
||||
ACCEL_SMOOTH_ALPHA = 0.90
|
||||
DECEL_SMOOTH_ALPHA = 0.40
|
||||
|
||||
class AccelController:
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
self.frame = 0
|
||||
self.last_max_accel = 2.0
|
||||
self.last_min_accel = -0.01
|
||||
self.first_run = True
|
||||
self.min_accel_first_run = True
|
||||
self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
|
||||
self._enabled = self.params.get_bool("AccelPersonalityEnabled")
|
||||
|
||||
def update(self, sm=None) -> None:
|
||||
self.frame += 1
|
||||
if self.frame % int(1.0 / DT_MDL) == 0:
|
||||
self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
|
||||
self._enabled = self.params.get_bool("AccelPersonalityEnabled")
|
||||
|
||||
@property
|
||||
def profile(self) -> int:
|
||||
return self._profile
|
||||
|
||||
def is_enabled(self) -> bool:
|
||||
return self._enabled
|
||||
|
||||
def get_max_accel(self, v_ego: float) -> float:
|
||||
v_ego = max(0.0, v_ego)
|
||||
target_max = np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self._profile])
|
||||
|
||||
if self.first_run:
|
||||
self.last_max_accel = target_max
|
||||
self.first_run = False
|
||||
return float(target_max)
|
||||
|
||||
self.last_max_accel = ACCEL_SMOOTH_ALPHA * target_max + (1 - ACCEL_SMOOTH_ALPHA) * self.last_max_accel
|
||||
return float(self.last_max_accel)
|
||||
|
||||
def get_min_accel(self, v_ego: float) -> float:
|
||||
v_ego = max(0.0, v_ego)
|
||||
target_min = np.interp(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self._profile])
|
||||
|
||||
if self.min_accel_first_run:
|
||||
self.last_min_accel = target_min
|
||||
self.min_accel_first_run = False
|
||||
else:
|
||||
self.last_min_accel = DECEL_SMOOTH_ALPHA * target_min + (1 - DECEL_SMOOTH_ALPHA) * self.last_min_accel
|
||||
|
||||
self.last_min_accel = min(self.last_min_accel, self.last_max_accel - 0.1)
|
||||
return float(self.last_min_accel)
|
||||
-227
@@ -1,227 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
|
||||
Scope is deliberately narrow: a v_ego-keyed acceleration ceiling and cruise-deceleration
|
||||
floor per profile. The controller does not modify lead following distance or the MPC lead
|
||||
candidate. The floor only ever softens the no-lead cruise candidate (slowing for a lower
|
||||
cruise speed, a curve, or a speed limit); it is excluded during forceDecel and e2e, and
|
||||
min() against the untouched MPC candidate means a real lead can always still force full
|
||||
ACCEL_MIN braking.
|
||||
|
||||
Ceiling vs floor apply on different policies: ACC (non-e2e) uses the controller's ceiling
|
||||
and floor; blended (e2e) uses the controller's ceiling but always the stock floor
|
||||
(A_CRUISE_MIN).
|
||||
"""
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
|
||||
AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES,
|
||||
)
|
||||
|
||||
|
||||
class TestAccelControllerCeiling(OpenpilotTestCase):
|
||||
def setUp(self):
|
||||
self.params = Params()
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
self.params.put("AccelPersonality", AccelProfile.normal, block=True)
|
||||
self.controller = AccelController()
|
||||
|
||||
def test_first_call_snaps_to_table_with_no_smoothing_lag(self):
|
||||
max_a = self.controller.get_max_accel(20.0)
|
||||
expected_max = np.interp(20.0, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.normal])
|
||||
self.assertAlmostEqual(max_a, expected_max, places=6)
|
||||
|
||||
def test_min_accel_first_call_snaps_to_table_not_the_neg0p01_seed(self):
|
||||
# Regression guard: get_min_accel used to have no first-run snap (unlike get_max_accel),
|
||||
# so its very first call blended the table target against a hardcoded -0.01 seed and
|
||||
# commanded a much-weaker-than-any-profile floor for the first ~10-15 frames of every drive.
|
||||
min_a = self.controller.get_min_accel(20.0)
|
||||
expected_min = np.interp(20.0, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[AccelProfile.normal])
|
||||
self.assertAlmostEqual(min_a, expected_min, places=6)
|
||||
|
||||
def test_table_lookup_matches_breakpoints_per_profile(self):
|
||||
for profile, table in MAX_ACCEL_PROFILES.items():
|
||||
self.params.put("AccelPersonality", profile, block=True)
|
||||
controller = AccelController()
|
||||
for v_ego, expected in zip(MAX_ACCEL_BREAKPOINTS, table, strict=True):
|
||||
controller.first_run = True
|
||||
max_a = controller.get_max_accel(v_ego)
|
||||
self.assertAlmostEqual(max_a, expected, places=3)
|
||||
|
||||
def test_smoothing_moves_gradually_not_instantly_on_profile_switch(self):
|
||||
v_ego = 8.0 # breakpoint where eco/normal/sport ceilings differ
|
||||
self.controller.get_max_accel(v_ego) # settle first_run on normal
|
||||
start = self.controller.last_max_accel
|
||||
self.params.put("AccelPersonality", AccelProfile.sport, block=True)
|
||||
self.controller.frame = int(1.0 / DT_MDL) - 1 # force the 1s refresh boundary on next update()
|
||||
self.controller.update()
|
||||
max_a = self.controller.get_max_accel(v_ego)
|
||||
target = MAX_ACCEL_PROFILES[AccelProfile.sport][MAX_ACCEL_BREAKPOINTS.index(v_ego)]
|
||||
self.assertNotEqual(start, target)
|
||||
self.assertGreater(max_a, start)
|
||||
self.assertLess(max_a, target)
|
||||
|
||||
def test_eco_is_selectable_not_treated_as_falsy(self):
|
||||
self.params.put("AccelPersonality", AccelProfile.eco, block=True)
|
||||
controller = AccelController()
|
||||
self.assertEqual(controller.profile, AccelProfile.eco)
|
||||
max_a = controller.get_max_accel(0.0)
|
||||
self.assertAlmostEqual(max_a, MAX_ACCEL_PROFILES[AccelProfile.eco][0], places=3)
|
||||
|
||||
def test_min_accel_never_stronger_than_stock_a_cruise_min(self):
|
||||
for v_ego in [0., 3., 4.5, 7., 9., 15., 25., 40.]:
|
||||
for _ in range(60):
|
||||
min_a = self.controller.get_min_accel(v_ego)
|
||||
self.assertGreaterEqual(min_a, -1.4) # softer or equal to the softest stock-adjacent floor, never harsher
|
||||
self.assertLess(min_a, 0.0)
|
||||
|
||||
def test_min_accel_ramps_to_stock_strength_by_highway_speed(self):
|
||||
for _ in range(200):
|
||||
min_a = self.controller.get_min_accel(25.0)
|
||||
self.assertAlmostEqual(min_a, MIN_ACCEL_PROFILES[AccelProfile.normal][-1], places=2)
|
||||
|
||||
def test_min_accel_profile_ordering_eco_softest_sport_strongest(self):
|
||||
settled = {}
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
|
||||
self.params.put("AccelPersonality", profile, block=True)
|
||||
controller = AccelController()
|
||||
for _ in range(60):
|
||||
settled[profile] = controller.get_min_accel(4.5)
|
||||
self.assertGreater(settled[AccelProfile.eco], settled[AccelProfile.normal])
|
||||
self.assertGreater(settled[AccelProfile.normal], settled[AccelProfile.sport])
|
||||
|
||||
def test_min_accel_never_inverts_above_max_accel(self):
|
||||
# Both feed the same np.clip call in get_cruise_accel -- independent smoothing must
|
||||
# never let the floor drift above the ceiling.
|
||||
for v_ego in [0., 3., 8., 20., 45.]:
|
||||
max_a = self.controller.get_max_accel(v_ego)
|
||||
min_a = self.controller.get_min_accel(v_ego)
|
||||
self.assertLessEqual(min_a, max_a - 0.05)
|
||||
|
||||
def test_params_refresh_only_at_one_second_boundary(self):
|
||||
self.controller.frame = 0
|
||||
self.params.put("AccelPersonality", AccelProfile.sport, block=True)
|
||||
self.controller.update() # frame=1, not a boundary
|
||||
self.assertEqual(self.controller.profile, AccelProfile.normal)
|
||||
self.controller.frame = int(1.0 / DT_MDL) - 1
|
||||
self.controller.update() # crosses the boundary
|
||||
self.assertEqual(self.controller.profile, AccelProfile.sport)
|
||||
|
||||
def test_enabled_reflects_params(self):
|
||||
self.params.put_bool("AccelPersonalityEnabled", False, block=True)
|
||||
controller = AccelController()
|
||||
self.assertFalse(controller.is_enabled())
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
controller.frame = int(1.0 / DT_MDL) - 1
|
||||
controller.update()
|
||||
self.assertTrue(controller.is_enabled())
|
||||
|
||||
def test_max_accel_never_exceeds_profile_ceiling(self):
|
||||
for v_ego in [0., 5., 10., 20., 30., 45., 60.]:
|
||||
max_a = self.controller.get_max_accel(v_ego)
|
||||
table_max = max(max(table) for table in MAX_ACCEL_PROFILES.values())
|
||||
self.assertLessEqual(max_a, table_max + 1e-6)
|
||||
|
||||
|
||||
class TestOffEqualsStock(OpenpilotTestCase):
|
||||
def setUp(self):
|
||||
self.params = Params()
|
||||
self.params.put_bool("AccelPersonalityEnabled", False, block=True)
|
||||
|
||||
def test_disabled_controller_is_enabled_returns_false(self):
|
||||
controller = AccelController()
|
||||
self.assertFalse(controller.is_enabled())
|
||||
|
||||
def test_get_cruise_accel_with_none_override_matches_no_kwarg(self):
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import get_cruise_accel
|
||||
args = (False, 10.0, 8.0, 0.5, 0.0, _fake_cp(), DT_MDL, 1.0, True)
|
||||
self.assertEqual(get_cruise_accel(*args), get_cruise_accel(*args, max_accel_override=None, min_accel_override=None))
|
||||
|
||||
def test_disabled_min_accel_override_is_none(self):
|
||||
planner = _bare_planner()
|
||||
self.assertIsNone(planner.get_min_accel_override(v_ego=5.0, e2e=False, force_decel=False))
|
||||
|
||||
def test_disabled_max_accel_override_is_none(self):
|
||||
planner = _bare_planner()
|
||||
self.assertIsNone(planner.get_max_accel_override(v_ego=5.0))
|
||||
|
||||
def test_force_decel_excludes_min_accel_override_even_when_enabled(self):
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
planner = _bare_planner()
|
||||
self.assertIsNone(planner.get_min_accel_override(v_ego=5.0, e2e=False, force_decel=True))
|
||||
|
||||
def test_e2e_excludes_min_accel_override_even_when_enabled(self):
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
planner = _bare_planner()
|
||||
self.assertIsNone(planner.get_min_accel_override(v_ego=5.0, e2e=True, force_decel=False))
|
||||
|
||||
def test_enabled_min_accel_override_returns_a_float(self):
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
planner = _bare_planner()
|
||||
override = planner.get_min_accel_override(v_ego=5.0, e2e=False, force_decel=False)
|
||||
self.assertIsNotNone(override)
|
||||
self.assertLess(override, 0.0)
|
||||
|
||||
def test_enabled_max_accel_override_applies_in_acc_and_blended(self):
|
||||
# Policy: max ceiling comes from AccelController in both ACC and blended (e2e) modes --
|
||||
# only the min floor is blended-vs-stock. get_max_accel_override no longer takes an e2e
|
||||
# arg because of this; the caller applies it unconditionally.
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
planner = _bare_planner()
|
||||
override = planner.get_max_accel_override(v_ego=5.0)
|
||||
self.assertIsNotNone(override)
|
||||
self.assertGreater(override, 0.0)
|
||||
|
||||
def test_blended_min_accel_uses_stock_not_controller(self):
|
||||
# e2e/blended braking floor is deliberately left at stock's A_CRUISE_MIN, never the
|
||||
# controller's floor -- this is the "acc policy = controller min+max, blended policy =
|
||||
# controller max + stock min" split, final per product decision.
|
||||
# jerk-limiting now applies unconditionally (even in e2e, per upstream's decel-jerk fix), so
|
||||
# dt=10.0 opens the jerk-limit window wide enough that it can't mask the floor/ceiling asserted here.
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import get_cruise_accel, A_CRUISE_MIN
|
||||
args = {"v_cruise": -100.0, "v_ego": 20.0, "a_cruise_prev": 0.0, "angle_steers": 0.0, "CP": _fake_cp(),
|
||||
"dt": 10.0, "accel_coast": 1.0, "allow_throttle": True}
|
||||
target, active = get_cruise_accel(True, **args, min_accel_override=-0.3)
|
||||
self.assertAlmostEqual(target, A_CRUISE_MIN, places=6)
|
||||
self.assertFalse(active) # controller's floor was ignored in favor of stock -- not "active"
|
||||
|
||||
def test_blended_max_accel_uses_controller_override(self):
|
||||
# jerk-limiting now applies unconditionally (even in e2e) -- dt=10.0 opens the jerk-limit
|
||||
# window wide enough that it can't mask the override ceiling asserted here.
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import get_cruise_accel
|
||||
args = {"v_cruise": 100.0, "v_ego": 20.0, "a_cruise_prev": 0.0, "angle_steers": 0.0, "CP": _fake_cp(),
|
||||
"dt": 10.0, "accel_coast": 1.0, "allow_throttle": True}
|
||||
target, active = get_cruise_accel(True, **args, max_accel_override=0.4)
|
||||
self.assertAlmostEqual(target, 0.4, places=6)
|
||||
self.assertTrue(active)
|
||||
self.assertIsInstance(active, bool)
|
||||
plan = messaging.new_message('longitudinalPlanSP')
|
||||
plan.longitudinalPlanSP.accelController.active = active
|
||||
|
||||
|
||||
|
||||
def _fake_cp():
|
||||
class _CP:
|
||||
steerRatio = 15.0
|
||||
wheelbase = 2.7
|
||||
return _CP()
|
||||
|
||||
|
||||
def _bare_planner():
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner.accel_controller = AccelController()
|
||||
return planner
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -1,115 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from collections import deque
|
||||
import math
|
||||
from typing import Any
|
||||
|
||||
from openpilot.cereal import log
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
|
||||
|
||||
LEAD_DEPARTURE_MIN_SPEED = 0.3
|
||||
LEAD_DEPARTURE_CONFIRM_FRAMES = 3
|
||||
LEAD_DEPARTURE_MIN_DISTANCE = 0.03
|
||||
LEAD_DEPARTURE_MAX_EGO_SPEED = 0.3
|
||||
|
||||
MpcPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
|
||||
|
||||
|
||||
class LeadDepartureController:
|
||||
def __init__(self, enabled: bool):
|
||||
self.enabled = enabled
|
||||
self._track_id: int | None = None
|
||||
self._distances: deque[float] = deque(maxlen=LEAD_DEPARTURE_CONFIRM_FRAMES)
|
||||
self._active = False
|
||||
|
||||
@property
|
||||
def active(self) -> bool:
|
||||
return self._active
|
||||
|
||||
def reset(self) -> None:
|
||||
self._track_id = None
|
||||
self._distances.clear()
|
||||
self._active = False
|
||||
|
||||
@staticmethod
|
||||
def _selected_lead(radar_state: Any, source: Any) -> Any | None:
|
||||
if source == MpcPlanSource.lead0:
|
||||
return radar_state.leadOne
|
||||
if source == MpcPlanSource.lead1:
|
||||
return radar_state.leadTwo
|
||||
return None
|
||||
|
||||
@staticmethod
|
||||
def _radar_has_errors(radar_state: Any) -> bool:
|
||||
errors = radar_state.radarErrors
|
||||
return errors.canError or errors.radarFault or errors.wrongConfig or errors.radarUnavailableTemporary
|
||||
|
||||
def update(self, sm: Any, source: Any, a_target: float, should_stop: bool, reset: bool, radar_valid: bool) -> bool:
|
||||
CS = sm['carState']
|
||||
CC = sm['carControl']
|
||||
controls_state = sm['controlsState']
|
||||
radar_state = sm['radarState']
|
||||
|
||||
blocked = (
|
||||
not self.enabled
|
||||
or reset
|
||||
or not CC.longActive
|
||||
or CC.cruiseControl.override
|
||||
or CS.gasPressed
|
||||
or CS.brakePressed
|
||||
or controls_state.forceDecel
|
||||
or controls_state.longControlState == LongCtrlState.off
|
||||
or not radar_valid
|
||||
or self._radar_has_errors(radar_state)
|
||||
)
|
||||
if blocked or not math.isfinite(CS.vEgo) or CS.vEgo >= LEAD_DEPARTURE_MAX_EGO_SPEED or not math.isfinite(a_target):
|
||||
self.reset()
|
||||
return should_stop
|
||||
|
||||
lead = self._selected_lead(radar_state, source)
|
||||
lead_valid = (
|
||||
lead is not None
|
||||
and lead.present
|
||||
and lead.radar
|
||||
and lead.radarTrackId >= 0
|
||||
and all(math.isfinite(value) for value in (lead.dRel, lead.vLeadK, lead.vRel))
|
||||
and lead.dRel > 0.0
|
||||
and lead.vLeadK >= LEAD_DEPARTURE_MIN_SPEED
|
||||
and lead.vRel >= LEAD_DEPARTURE_MIN_SPEED
|
||||
and a_target >= 0.0
|
||||
)
|
||||
if not lead_valid:
|
||||
self.reset()
|
||||
return should_stop
|
||||
|
||||
track_id = int(lead.radarTrackId)
|
||||
if self._active:
|
||||
if track_id != self._track_id:
|
||||
self.reset()
|
||||
return should_stop
|
||||
return False
|
||||
|
||||
if not should_stop:
|
||||
self.reset()
|
||||
return False
|
||||
|
||||
if controls_state.longControlState != LongCtrlState.stopping:
|
||||
self.reset()
|
||||
return should_stop
|
||||
|
||||
if track_id != self._track_id:
|
||||
self._track_id = track_id
|
||||
self._distances.clear()
|
||||
self._distances.append(float(lead.dRel))
|
||||
|
||||
if len(self._distances) == LEAD_DEPARTURE_CONFIRM_FRAMES and self._distances[-1] - self._distances[0] >= LEAD_DEPARTURE_MIN_DISTANCE:
|
||||
self._active = True
|
||||
return False
|
||||
|
||||
return should_stop
|
||||
@@ -1,101 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import math
|
||||
from typing import cast
|
||||
|
||||
from opendbc.car import DT_CTRL
|
||||
|
||||
STOPPING_DISTANCE = 0.75
|
||||
STOPPING_TIME = 2.5
|
||||
STOPPING_ACCEL_TOLERANCE = 0.1
|
||||
STOPPING_SPEED_TOLERANCE = 0.05
|
||||
STOPPING_SETTLE_FRAMES = 30
|
||||
STOPPING_HOLD_ACCEL = -1.2
|
||||
STOPPING_HOLD_MARGIN = 0.6
|
||||
STOPPING_HOLD_SPEED_TOLERANCE = 0.01
|
||||
|
||||
|
||||
class LongControlSP:
|
||||
def __init__(self):
|
||||
self._stopping_settle_frames: int | None = None
|
||||
self._stopping_hold_accel: float | None = None
|
||||
|
||||
def _hold_supported(self) -> bool:
|
||||
return self.CP.openpilotLongitudinalControl and not self.CP.notCar and self.CP.stopAccel < 0.0
|
||||
|
||||
def update_state(self, stopping: bool, active: bool, CS) -> None:
|
||||
if not active:
|
||||
self._stopping_settle_frames = None
|
||||
self._stopping_hold_accel = None
|
||||
return
|
||||
|
||||
invalid_speed = not all(math.isfinite(speed) for speed in (CS.vEgo, CS.vEgoRaw))
|
||||
moving = max(abs(CS.vEgo), abs(CS.vEgoRaw)) > STOPPING_SPEED_TOLERANCE
|
||||
if invalid_speed or (not stopping and moving):
|
||||
self._stopping_hold_accel = None
|
||||
elif (self._hold_supported() and math.isfinite(self.last_output_accel)
|
||||
and self.last_output_accel <= self.CP.stopAccel):
|
||||
previous_hold = self._stopping_hold_accel if self._stopping_hold_accel is not None else self.last_output_accel
|
||||
self._stopping_hold_accel = min(self.last_output_accel, previous_hold)
|
||||
if not stopping:
|
||||
self._stopping_settle_frames = None
|
||||
if self._stopping_hold_accel is not None and math.isfinite(self.last_output_accel):
|
||||
self._stopping_hold_accel = min(self.last_output_accel, self._stopping_hold_accel)
|
||||
|
||||
def stopping_accel(self, output_accel: float, CS) -> float:
|
||||
if self._stopping_hold_accel is not None and math.isfinite(CS.vEgo) and abs(CS.vEgo) <= STOPPING_SPEED_TOLERANCE:
|
||||
return min(output_accel, self._stopping_hold_accel)
|
||||
return output_accel
|
||||
|
||||
def stopping_decel_rate(self, CS, a_target: float, output_accel: float) -> float:
|
||||
if not all(math.isfinite(value) for value in (output_accel, a_target, CS.vEgo, CS.vEgoRaw, CS.aEgo)):
|
||||
return 1.0
|
||||
hold_supported = self._hold_supported()
|
||||
preserving_hold = self._stopping_hold_accel is not None
|
||||
can_hold = output_accel <= 0.0 and a_target >= output_accel
|
||||
terminal_speed = (0.0 <= CS.vEgo <= STOPPING_SPEED_TOLERANCE
|
||||
or CS.standstill and abs(CS.vEgo) <= STOPPING_SPEED_TOLERANCE)
|
||||
positive_stop_entry = self.last_output_accel > 0.0 and output_accel == 0.0
|
||||
if output_accel > 0.0 or positive_stop_entry or CS.vEgo < 0.0 and not terminal_speed:
|
||||
return 1.0
|
||||
if terminal_speed and self._stopping_settle_frames is None:
|
||||
if not preserving_hold and (not can_hold or output_accel > -STOPPING_ACCEL_TOLERANCE or CS.aEgo >= -STOPPING_ACCEL_TOLERANCE):
|
||||
return 1.0
|
||||
self._stopping_settle_frames = 0
|
||||
|
||||
time_decel = 0.0 if self._stopping_settle_frames is not None else CS.vEgo / STOPPING_TIME
|
||||
required_decel = max(time_decel, CS.vEgo ** 2 / (2.0 * STOPPING_DISTANCE), 1e-3)
|
||||
adequacy = min(max(-CS.aEgo / required_decel, 0.0), 1.0)
|
||||
planner_need = min(max((output_accel - a_target) / max(required_decel, STOPPING_ACCEL_TOLERANCE), 0.0), 1.0)
|
||||
if not terminal_speed and self._stopping_settle_frames is None and can_hold and adequacy >= 1.0:
|
||||
self._stopping_settle_frames = 0
|
||||
if hold_supported:
|
||||
self._stopping_hold_accel = output_accel
|
||||
|
||||
motion_need = 1.0 - adequacy ** 2
|
||||
terminal_need = 0.0
|
||||
if terminal_speed or self._stopping_settle_frames not in (None, 0):
|
||||
settle_frames = cast(int, self._stopping_settle_frames)
|
||||
self._stopping_settle_frames = min(settle_frames + 1, STOPPING_SETTLE_FRAMES)
|
||||
terminal_need = (self._stopping_settle_frames / STOPPING_SETTLE_FRAMES) ** 2
|
||||
|
||||
if preserving_hold and self._stopping_hold_accel is not None:
|
||||
self._stopping_hold_accel = min(output_accel, self._stopping_hold_accel)
|
||||
if terminal_speed:
|
||||
minimum_hold = min(STOPPING_HOLD_ACCEL, self.CP.stopAccel + STOPPING_HOLD_MARGIN)
|
||||
hold_target = max(self.CP.stopAccel, min(minimum_hold, self._stopping_hold_accel))
|
||||
if CS.aEgo > STOPPING_ACCEL_TOLERANCE or abs(CS.vEgoRaw) > STOPPING_HOLD_SPEED_TOLERANCE:
|
||||
return 1.0
|
||||
hold_rate = max(planner_need, terminal_need)
|
||||
if CS.vEgoRaw == 0.0 and abs(CS.vEgo) <= STOPPING_HOLD_SPEED_TOLERANCE:
|
||||
if output_accel <= hold_target:
|
||||
return planner_need
|
||||
hold_rate = max(planner_need, min(hold_rate, (output_accel - hold_target) / DT_CTRL))
|
||||
return hold_rate
|
||||
|
||||
return max(motion_need, planner_need, terminal_need)
|
||||
@@ -9,10 +9,8 @@ from openpilot.cereal import messaging, custom
|
||||
from opendbc.car import structs
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.lead_departure_controller import LeadDepartureController
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver
|
||||
@@ -25,9 +23,8 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
|
||||
|
||||
class LongitudinalPlannerSP:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
|
||||
self.accel_controller = AccelController()
|
||||
self.lead_departure_controller = LeadDepartureController(CP.openpilotLongitudinalControl and CP.autoResumeSng and not CP.notCar)
|
||||
self.events_sp = EventsSP()
|
||||
self.resolver = SpeedLimitResolver()
|
||||
self.dec = DynamicExperimentalController(CP, mpc)
|
||||
self.scc = SmartCruiseControl()
|
||||
self.resolver = SpeedLimitResolver()
|
||||
@@ -46,20 +43,6 @@ class LongitudinalPlannerSP:
|
||||
|
||||
return experimental_mode and self.dec.mode() == "blended"
|
||||
|
||||
def get_max_accel_override(self, v_ego: float) -> float | None:
|
||||
if not self.accel_controller.is_enabled():
|
||||
return None
|
||||
return self.accel_controller.get_max_accel(v_ego)
|
||||
|
||||
def get_min_accel_override(self, v_ego: float, e2e: bool, force_decel: bool) -> float | None:
|
||||
if e2e or force_decel or not self.accel_controller.is_enabled():
|
||||
return None
|
||||
return self.accel_controller.get_min_accel(v_ego)
|
||||
|
||||
def update_lead_departure(self, sm: messaging.SubMaster, a_target: float, should_stop: bool, reset: bool) -> bool:
|
||||
radar_valid = sm.valid.get('radarState', False) and getattr(sm, 'alive', {}).get('radarState', False)
|
||||
return self.lead_departure_controller.update(sm, self.mpc.source, a_target, should_stop, reset, radar_valid)
|
||||
|
||||
def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
|
||||
CS = sm['carState']
|
||||
v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
|
||||
@@ -91,7 +74,6 @@ class LongitudinalPlannerSP:
|
||||
return self.output_v_target, self.output_a_target
|
||||
|
||||
def update(self, sm: messaging.SubMaster) -> None:
|
||||
self.accel_controller.update(sm)
|
||||
self.events_sp.clear()
|
||||
self.dec.update(sm)
|
||||
self.e2e_alerts_helper.update(sm, self.events_sp)
|
||||
@@ -113,11 +95,6 @@ class LongitudinalPlannerSP:
|
||||
dec.enabled = self.dec.enabled()
|
||||
dec.active = self.dec.active()
|
||||
|
||||
accel_controller = longitudinalPlanSP.accelController
|
||||
accel_controller.enabled = self.accel_controller.is_enabled()
|
||||
accel_controller.active = self.accel_controller_active
|
||||
accel_controller.profile = self.accel_controller.profile
|
||||
|
||||
# Smart Cruise Control
|
||||
smartCruiseControl = longitudinalPlanSP.smartCruiseControl
|
||||
# Vision Control
|
||||
|
||||
+11
-368
@@ -4,8 +4,6 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from types import SimpleNamespace
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
@@ -17,23 +15,8 @@ from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import (
|
||||
_A_LAT_REG_MAX,
|
||||
_BELOW_EGO_TARGET_RELEASE_RATE,
|
||||
_ENTERING_PRED_LAT_ACC_TH,
|
||||
_MIN_ACTIVATION_SPEED,
|
||||
_RELIEF_CONFIRMATION_FRAMES,
|
||||
_TARGET_RELEASE_CONFIRMATION_FRAMES,
|
||||
_TARGET_RELEASE_RATE,
|
||||
_TARGET_TIGHTEN_CONFIRMATION_FRAMES,
|
||||
_TARGET_TIGHTEN_RATE,
|
||||
_TURNING_LAT_ACC_TH,
|
||||
_URGENT_PRED_LAT_ACC_TH,
|
||||
SmartCruiseControlVision,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision, _ENTERING_PRED_LAT_ACC_TH
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
|
||||
VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
|
||||
@@ -124,6 +107,7 @@ def generate_controlsState():
|
||||
|
||||
|
||||
class TestSmartCruiseControlVision(OpenpilotTestCase):
|
||||
|
||||
def setup_method(self):
|
||||
self.params = Params()
|
||||
self.reset_params()
|
||||
@@ -137,377 +121,36 @@ class TestSmartCruiseControlVision(OpenpilotTestCase):
|
||||
def reset_params(self):
|
||||
self.params.put_bool("SmartCruiseControlVision", True, block=True)
|
||||
|
||||
def assert_approx(self, actual, expected):
|
||||
self.assertAlmostEqual(actual, expected, delta=max(1e-12, abs(expected) * 1e-6))
|
||||
|
||||
def set_lat_accels(self, current: float, predicted: float, v_ego: float = 20.0, model_speed: float = 20.0) -> None:
|
||||
self.sm['controlsState'].curvature = current / v_ego**2
|
||||
self.sm['modelV2'].velocity.x = [model_speed] * len(ModelConstants.T_IDXS)
|
||||
self.sm['modelV2'].orientationRate.z = [predicted / model_speed] * len(ModelConstants.T_IDXS)
|
||||
|
||||
def update_lat_accels(
|
||||
self, current: float, predicted: float, cruise: float = 30.0, a_ego: float = 0.0, v_ego: float = 20.0, model_speed: float = 20.0
|
||||
) -> None:
|
||||
self.set_lat_accels(current, predicted, v_ego, model_speed)
|
||||
self.scc_v.update(self.sm, True, False, v_ego, a_ego, cruise)
|
||||
|
||||
def enter_curve(self, predicted: float = 2.2) -> None:
|
||||
self.update_lat_accels(0.5, predicted)
|
||||
self.update_lat_accels(0.5, predicted)
|
||||
assert self.scc_v.state == VisionState.entering
|
||||
|
||||
def test_initial_state(self):
|
||||
assert self.scc_v.state == VisionState.disabled
|
||||
assert not self.scc_v.is_active
|
||||
assert self.scc_v.output_v_target == V_CRUISE_UNSET
|
||||
assert self.scc_v.output_a_target == 0.0
|
||||
assert self.scc_v.output_a_target == 0.
|
||||
|
||||
def test_system_disabled(self):
|
||||
self.params.put_bool("SmartCruiseControlVision", False, block=True)
|
||||
self.scc_v.enabled = self.params.get_bool("SmartCruiseControlVision")
|
||||
|
||||
for _ in range(int(10.0 / DT_MDL)):
|
||||
self.scc_v.update(self.sm, True, False, 0.0, 0.0, 0.0)
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.scc_v.update(self.sm, True, False, 0., 0., 0.)
|
||||
assert self.scc_v.state == VisionState.disabled
|
||||
assert not self.scc_v.is_active
|
||||
|
||||
def test_disabled(self):
|
||||
for _ in range(int(10.0 / DT_MDL)):
|
||||
self.scc_v.update(self.sm, False, False, 0.0, 0.0, 0.0)
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.scc_v.update(self.sm, False, False, 0., 0., 0.)
|
||||
assert self.scc_v.state == VisionState.disabled
|
||||
|
||||
def test_transition_disabled_to_enabled(self):
|
||||
for _ in range(int(10.0 / DT_MDL)):
|
||||
self.scc_v.update(self.sm, True, False, 0.0, 0.0, 0.0)
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.scc_v.update(self.sm, True, False, 0., 0., 0.)
|
||||
assert self.scc_v.state == VisionState.enabled
|
||||
|
||||
def test_unconfirmed_release_holds_but_urgent_reentry_tightens(self):
|
||||
self.enter_curve()
|
||||
targets = [self.scc_v.output_v_target]
|
||||
|
||||
self.update_lat_accels(2.0, 2.2, a_ego=-0.8)
|
||||
assert self.scc_v.state == VisionState.turning
|
||||
assert self.scc_v.output_a_target == -0.8
|
||||
turning_demand = self.scc_v._v_demand()
|
||||
targets.append(self.scc_v.output_v_target)
|
||||
|
||||
self.update_lat_accels(1.2, 1.2, a_ego=0.3)
|
||||
assert self.scc_v.state == VisionState.leaving
|
||||
assert self.scc_v.output_a_target == 0.3
|
||||
targets.append(self.scc_v.output_v_target)
|
||||
|
||||
self.update_lat_accels(1.0, 3.0, a_ego=-1.2)
|
||||
assert self.scc_v.state == VisionState.entering
|
||||
assert self.scc_v.output_a_target == -1.2
|
||||
reentry_demand = self.scc_v._v_demand()
|
||||
targets.append(self.scc_v.output_v_target)
|
||||
|
||||
entering, turning, leaving, reentering = targets
|
||||
assert turning < entering
|
||||
self.assert_approx(turning, turning_demand)
|
||||
self.assert_approx(leaving, turning)
|
||||
assert reentering < leaving
|
||||
self.assert_approx(reentering, reentry_demand)
|
||||
|
||||
def test_new_curve_interrupts_confirmed_release_immediately(self):
|
||||
self.enter_curve()
|
||||
for _ in range(_RELIEF_CONFIRMATION_FRAMES + 1):
|
||||
self.update_lat_accels(0.8, 0.8)
|
||||
releasing_v_target = self.scc_v.output_v_target
|
||||
assert self.scc_v.state == VisionState.leaving
|
||||
|
||||
self.update_lat_accels(0.8, 3.0, a_ego=-0.7)
|
||||
assert self.scc_v.state == VisionState.entering
|
||||
assert self.scc_v.output_v_target < releasing_v_target
|
||||
assert self.scc_v.output_a_target == -0.7
|
||||
|
||||
@parameterized.expand([(-2.0,), (-0.5,), (0.0,), (0.8,)])
|
||||
def test_planner_acceleration_passes_through_exactly(self, planner_accel):
|
||||
self.enter_curve()
|
||||
self.update_lat_accels(0.5, 2.2, a_ego=planner_accel)
|
||||
assert self.scc_v.output_a_target == planner_accel
|
||||
|
||||
def test_planner_acceleration_passes_through_all_states(self):
|
||||
cases = (
|
||||
(False, False, 0.5, 2.2, -0.2, VisionState.disabled),
|
||||
(True, False, 0.5, 0.8, 0.1, VisionState.enabled),
|
||||
(True, False, 0.5, 2.2, -0.4, VisionState.entering),
|
||||
(True, False, 2.0, 2.2, -0.8, VisionState.turning),
|
||||
(True, False, 1.2, 1.2, 0.3, VisionState.leaving),
|
||||
(True, True, 1.2, 1.2, 0.6, VisionState.overriding),
|
||||
)
|
||||
for long_enabled, override, current, predicted, planner_accel, state in cases:
|
||||
self.set_lat_accels(current, predicted)
|
||||
self.scc_v.update(self.sm, long_enabled, override, 20.0, planner_accel, 30.0)
|
||||
assert self.scc_v.state == state
|
||||
assert self.scc_v.output_a_target == planner_accel
|
||||
|
||||
def test_jitter_requires_confirmed_relief_then_releases_smoothly(self):
|
||||
self.enter_curve()
|
||||
previous_v_target = self.scc_v.output_v_target
|
||||
|
||||
for frame in range(_RELIEF_CONFIRMATION_FRAMES * 2):
|
||||
self.update_lat_accels(1.0, 1.05 if frame % 2 == 0 else 1.15)
|
||||
assert self.scc_v.state == VisionState.entering
|
||||
assert self.scc_v.output_v_target >= previous_v_target
|
||||
assert self.scc_v.output_v_target - previous_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
|
||||
previous_v_target = self.scc_v.output_v_target
|
||||
|
||||
for _ in range(_RELIEF_CONFIRMATION_FRAMES):
|
||||
self.update_lat_accels(1.15, 0.8)
|
||||
assert self.scc_v.state == VisionState.entering
|
||||
assert 0.0 <= self.scc_v.output_v_target - previous_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
|
||||
previous_v_target = self.scc_v.output_v_target
|
||||
|
||||
release_cruise = 30.0
|
||||
for _ in range(_RELIEF_CONFIRMATION_FRAMES - 1):
|
||||
self.update_lat_accels(0.8, 0.8, release_cruise)
|
||||
assert self.scc_v.state == VisionState.entering
|
||||
assert 0.0 <= self.scc_v.output_v_target - previous_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
|
||||
previous_v_target = self.scc_v.output_v_target
|
||||
|
||||
active_v_targets = [previous_v_target]
|
||||
for _ in range(int((release_cruise - previous_v_target) / (_TARGET_RELEASE_RATE * DT_MDL)) + 10):
|
||||
self.update_lat_accels(0.8, 0.8, release_cruise)
|
||||
if not self.scc_v.is_active:
|
||||
break
|
||||
assert self.scc_v.state == VisionState.leaving
|
||||
assert self.scc_v.output_v_target != V_CRUISE_UNSET
|
||||
active_v_targets.append(self.scc_v.output_v_target)
|
||||
|
||||
assert self.scc_v.state == VisionState.enabled
|
||||
assert self.scc_v.output_v_target == V_CRUISE_UNSET
|
||||
self.assert_approx(active_v_targets[-1], release_cruise)
|
||||
assert np.all((np.diff(active_v_targets) >= 0.0) & (np.diff(active_v_targets) <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9))
|
||||
|
||||
def test_target_release_waits_for_relief_above_ego_speed(self):
|
||||
self.enter_curve()
|
||||
held_v_target = self.scc_v.output_v_target
|
||||
self.assert_approx(held_v_target, self.scc_v.v_ego)
|
||||
|
||||
for _ in range(_RELIEF_CONFIRMATION_FRAMES + _TARGET_RELEASE_CONFIRMATION_FRAMES - 2):
|
||||
self.update_lat_accels(0.8, 0.8)
|
||||
self.assert_approx(self.scc_v.output_v_target, held_v_target)
|
||||
|
||||
self.update_lat_accels(0.8, 0.8)
|
||||
rise = self.scc_v.output_v_target - held_v_target
|
||||
assert 0.0 < rise <= _TARGET_RELEASE_RATE * DT_MDL + 1e-9
|
||||
|
||||
def test_curve_target_is_independent_of_ego_speed(self):
|
||||
model_speed = 24.0
|
||||
predicted_yaw_rate = 0.12
|
||||
predicted_lat_accel = model_speed * predicted_yaw_rate
|
||||
expected_v_target = (_A_LAT_REG_MAX / (predicted_yaw_rate / model_speed)) ** 0.5
|
||||
targets = []
|
||||
|
||||
for v_ego in (18.0, 28.0):
|
||||
controller = SmartCruiseControlVision()
|
||||
self.set_lat_accels(0.5, predicted_lat_accel, v_ego, model_speed)
|
||||
controller.update(self.sm, True, False, v_ego, 0.0, 30.0)
|
||||
controller.update(self.sm, True, False, v_ego, 0.0, 30.0)
|
||||
assert controller.state == VisionState.entering
|
||||
targets.append(controller.v_target)
|
||||
|
||||
self.assert_approx(targets[0], expected_v_target)
|
||||
self.assert_approx(targets[1], expected_v_target)
|
||||
|
||||
def test_curve_target_respects_minimum_speed_floor(self):
|
||||
model_speed = 10.0
|
||||
predicted_yaw_rate = 2.0
|
||||
self.set_lat_accels(0.5, model_speed * predicted_yaw_rate, model_speed=model_speed)
|
||||
self.scc_v.update(self.sm, True, False, 20.0, 0.0, 30.0)
|
||||
self.scc_v.update(self.sm, True, False, 20.0, 0.0, 30.0)
|
||||
|
||||
assert self.scc_v.state == VisionState.entering
|
||||
assert self.scc_v.v_target < MIN_V
|
||||
self.assert_approx(self.scc_v.output_v_target, MIN_V)
|
||||
|
||||
@parameterized.expand(
|
||||
[([], []), ([np.nan] * len(ModelConstants.T_IDXS), [np.nan] * len(ModelConstants.T_IDXS)), ([20.0] * 5, [0.1] * 3)],
|
||||
names=["velocities", "yaw_rates"],
|
||||
)
|
||||
def test_model_vector_edges_remain_finite(self, velocities, yaw_rates):
|
||||
self.sm['modelV2'].velocity.x = velocities
|
||||
self.sm['modelV2'].orientationRate.z = yaw_rates
|
||||
self.scc_v.update(self.sm, True, False, 20.0, 0.0, 30.0)
|
||||
self.scc_v.update(self.sm, True, False, 20.0, 0.0, 30.0)
|
||||
|
||||
assert all(
|
||||
np.isfinite(value)
|
||||
for value in (
|
||||
self.scc_v.current_lat_acc,
|
||||
self.scc_v.max_pred_lat_acc,
|
||||
self.scc_v.v_target,
|
||||
self.scc_v.output_v_target,
|
||||
self.scc_v.output_a_target,
|
||||
)
|
||||
)
|
||||
|
||||
@parameterized.expand([(5.75,), (9.9,), (_MIN_ACTIVATION_SPEED,)])
|
||||
def test_vision_control_does_not_steal_launch(self, launch_speed):
|
||||
self.set_lat_accels(0.5, 3.0, launch_speed)
|
||||
self.scc_v.update(self.sm, True, False, launch_speed, 0.0, 30.0)
|
||||
self.scc_v.update(self.sm, True, False, launch_speed, 0.0, 30.0)
|
||||
|
||||
assert launch_speed <= _MIN_ACTIVATION_SPEED
|
||||
assert self.scc_v.state == VisionState.enabled
|
||||
assert not self.scc_v.is_active
|
||||
assert self.scc_v.output_v_target == V_CRUISE_UNSET
|
||||
|
||||
def test_vision_control_can_activate_above_launch_range(self):
|
||||
speed = _MIN_ACTIVATION_SPEED + 0.01
|
||||
self.set_lat_accels(0.5, 3.0, speed)
|
||||
self.scc_v.update(self.sm, True, False, speed, 0.0, 30.0)
|
||||
self.scc_v.update(self.sm, True, False, speed, 0.0, 30.0)
|
||||
|
||||
assert self.scc_v.state == VisionState.entering
|
||||
assert self.scc_v.is_active
|
||||
|
||||
def test_nonurgent_activation_has_no_target_cliff(self):
|
||||
v_ego = _MIN_ACTIVATION_SPEED + 0.01
|
||||
model_speed = 8.0
|
||||
self.update_lat_accels(0.5, 2.0, v_ego=v_ego, model_speed=model_speed)
|
||||
self.update_lat_accels(0.5, 2.0, v_ego=v_ego, model_speed=model_speed)
|
||||
|
||||
self.assert_approx(self.scc_v.v_target, 8.0)
|
||||
self.assert_approx(self.scc_v.output_v_target, v_ego)
|
||||
|
||||
def test_nonurgent_tightening_is_confirmed_and_rate_limited(self):
|
||||
self.enter_curve()
|
||||
initial_v_target = self.scc_v.output_v_target
|
||||
|
||||
for _ in range(_TARGET_TIGHTEN_CONFIRMATION_FRAMES - 1):
|
||||
self.update_lat_accels(0.5, 2.8)
|
||||
self.assert_approx(self.scc_v.output_v_target, initial_v_target)
|
||||
|
||||
self.update_lat_accels(0.5, 2.8)
|
||||
drop = initial_v_target - self.scc_v.output_v_target
|
||||
assert 0.0 < drop <= _TARGET_TIGHTEN_RATE * DT_MDL + 1e-9
|
||||
|
||||
def test_one_frame_curve_prediction_does_not_pulse_target(self):
|
||||
self.enter_curve()
|
||||
for _ in range(10):
|
||||
self.update_lat_accels(0.5, 2.2)
|
||||
stable_v_target = self.scc_v.output_v_target
|
||||
|
||||
self.update_lat_accels(0.5, 2.8)
|
||||
self.assert_approx(self.scc_v.output_v_target, stable_v_target)
|
||||
self.update_lat_accels(0.5, 2.2)
|
||||
|
||||
self.assert_approx(self.scc_v.output_v_target, stable_v_target)
|
||||
|
||||
def test_one_frame_release_does_not_reverse_target(self):
|
||||
self.enter_curve(_URGENT_PRED_LAT_ACC_TH)
|
||||
stable_v_target = self.scc_v.output_v_target
|
||||
|
||||
self.update_lat_accels(0.5, 2.2)
|
||||
self.assert_approx(self.scc_v.output_v_target, stable_v_target)
|
||||
self.update_lat_accels(0.5, _URGENT_PRED_LAT_ACC_TH)
|
||||
|
||||
self.assert_approx(self.scc_v.output_v_target, stable_v_target)
|
||||
|
||||
def test_urgent_predicted_curve_is_not_delayed(self):
|
||||
self.enter_curve()
|
||||
self.update_lat_accels(0.5, _URGENT_PRED_LAT_ACC_TH)
|
||||
|
||||
self.assert_approx(self.scc_v.output_v_target, self.scc_v._v_demand())
|
||||
|
||||
def test_current_curve_is_not_delayed(self):
|
||||
self.enter_curve()
|
||||
self.update_lat_accels(_TURNING_LAT_ACC_TH, 2.8)
|
||||
|
||||
self.assert_approx(self.scc_v.output_v_target, self.scc_v._v_demand())
|
||||
|
||||
def test_sequential_curve_confirms_release_and_tightens_urgently(self):
|
||||
self.enter_curve(3.0)
|
||||
for _ in range(20):
|
||||
self.update_lat_accels(0.5, 3.0)
|
||||
restrictive_v_target = self.scc_v.output_v_target
|
||||
|
||||
self.update_lat_accels(0.5, 1.4, a_ego=0.4)
|
||||
assert self.scc_v.state == VisionState.entering
|
||||
self.assert_approx(self.scc_v.output_v_target, restrictive_v_target)
|
||||
assert self.scc_v.output_a_target == 0.4
|
||||
|
||||
for _ in range(_TARGET_RELEASE_CONFIRMATION_FRAMES - 2):
|
||||
self.update_lat_accels(0.5, 1.4)
|
||||
self.assert_approx(self.scc_v.output_v_target, restrictive_v_target)
|
||||
|
||||
self.update_lat_accels(0.5, 1.4)
|
||||
released_v_target = self.scc_v.output_v_target
|
||||
assert 0.0 < released_v_target - restrictive_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
|
||||
|
||||
self.update_lat_accels(0.5, 3.0, a_ego=-0.6)
|
||||
assert self.scc_v.state == VisionState.entering
|
||||
self.assert_approx(self.scc_v.output_v_target, restrictive_v_target)
|
||||
assert self.scc_v.output_a_target == -0.6
|
||||
|
||||
for _ in range(4):
|
||||
self.update_lat_accels(0.5, 1.4)
|
||||
self.assert_approx(self.scc_v.output_v_target, restrictive_v_target)
|
||||
self.update_lat_accels(0.5, 3.0)
|
||||
self.assert_approx(self.scc_v.output_v_target, restrictive_v_target)
|
||||
|
||||
def test_acceleration_is_continuous_through_planner_arbitration(self):
|
||||
car_control = messaging.new_message('carControl')
|
||||
car_control.carControl.enabled = True
|
||||
car_control.carControl.cruiseControl.override = False
|
||||
self.sm['carControl'] = car_control.carControl
|
||||
self.sm['carState'].vCruiseCluster = 108.0
|
||||
|
||||
planner: Any = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner.scc = SimpleNamespace(
|
||||
vision=self.scc_v,
|
||||
map=SimpleNamespace(output_v_target=V_CRUISE_UNSET, output_a_target=0.0),
|
||||
update=lambda sm, enabled, override, v_ego, a_ego, v_cruise: self.scc_v.update(sm, enabled, override, v_ego, a_ego, v_cruise),
|
||||
)
|
||||
planner.resolver = SimpleNamespace(
|
||||
speed_limit_valid=False,
|
||||
speed_limit_last_valid=False,
|
||||
speed_limit=0.0,
|
||||
speed_limit_final_last=0.0,
|
||||
distance=0.0,
|
||||
update=lambda _v_ego, _sm: None,
|
||||
)
|
||||
planner.sla = SimpleNamespace(
|
||||
output_v_target=V_CRUISE_UNSET,
|
||||
output_a_target=0.0,
|
||||
update=lambda *_args: None,
|
||||
)
|
||||
planner.events_sp = SimpleNamespace()
|
||||
|
||||
self.set_lat_accels(0.5, 2.2)
|
||||
planner.update_targets(self.sm, 20.0, -0.8, 30.0)
|
||||
planner.update_targets(self.sm, 20.0, -0.8, 30.0)
|
||||
assert planner.source == LongitudinalPlanSource.sccVision
|
||||
assert planner.output_a_target == -0.8
|
||||
|
||||
for planner_accel in (-2.0, 0.5, -0.2):
|
||||
planner.update_targets(self.sm, 20.0, planner_accel, 30.0)
|
||||
assert planner.source == LongitudinalPlanSource.sccVision
|
||||
assert planner.output_a_target == planner_accel
|
||||
|
||||
self.set_lat_accels(0.8, 0.8)
|
||||
for _ in range(int(30.0 / (_TARGET_RELEASE_RATE * DT_MDL)) + 10):
|
||||
planner.update_targets(self.sm, 20.0, 0.4, 30.0)
|
||||
assert planner.output_a_target == 0.4
|
||||
if planner.source == LongitudinalPlanSource.cruise:
|
||||
break
|
||||
else:
|
||||
self.fail("SCC Vision did not release to cruise")
|
||||
|
||||
planner.update_targets(self.sm, 20.0, 0.4, 30.0)
|
||||
assert self.scc_v.state == VisionState.enabled
|
||||
assert planner.source == LongitudinalPlanSource.cruise
|
||||
|
||||
@parameterized.expand(
|
||||
[
|
||||
@parameterized.expand([
|
||||
("p97_just_above_threshold", True),
|
||||
("single_spike_filtered", False),
|
||||
("persistent_high_values", True),
|
||||
],
|
||||
names=["case", "should_enter"],
|
||||
)
|
||||
], names=["case", "should_enter"])
|
||||
def test_max_pred_lat_acc_uses_p97_and_threshold(self, case, should_enter):
|
||||
n = len(ModelConstants.T_IDXS)
|
||||
th = float(_ENTERING_PRED_LAT_ACC_TH)
|
||||
|
||||
-110
@@ -1,110 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import gc
|
||||
from contextlib import ExitStack
|
||||
from unittest import mock
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanSource
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import _A_LAT_REG_MAX
|
||||
|
||||
|
||||
def _run_constant_curve(*, scc_enabled: bool, cruise: float, duration: float = 70.0) -> dict[str, np.ndarray]:
|
||||
gc.collect()
|
||||
curvature = 0.005
|
||||
plant = Plant(lead_relevancy=False, speed=30.0)
|
||||
planner = plant.planner
|
||||
planner.dec._enabled = False
|
||||
planner.scc.map.enabled = False
|
||||
planner.scc.vision.enabled = scc_enabled
|
||||
solver_failures = 0
|
||||
|
||||
with ExitStack() as patches:
|
||||
patches.enter_context(mock.patch.object(planner.dec, "_read_params", return_value=None))
|
||||
patches.enter_context(mock.patch.object(planner.scc.map, "update_params", return_value=None))
|
||||
patches.enter_context(mock.patch.object(planner.scc.vision, "_update_params", return_value=None))
|
||||
|
||||
original_mpc_reset = planner.mpc.reset
|
||||
|
||||
def record_mpc_reset(*args, **kwargs):
|
||||
nonlocal solver_failures
|
||||
solver_failures += int(planner.mpc.solution_status != 0)
|
||||
return original_mpc_reset(*args, **kwargs)
|
||||
|
||||
patches.enter_context(mock.patch.object(planner.mpc, "reset", side_effect=record_mpc_reset))
|
||||
|
||||
if scc_enabled:
|
||||
original_update_calculations = planner.scc.vision._update_calculations
|
||||
|
||||
def inject_constant_curvature(sm):
|
||||
velocities = np.asarray(sm['modelV2'].velocity.x, dtype=float)
|
||||
sm['modelV2'].orientationRate.z = (curvature * velocities).tolist()
|
||||
sm['controlsState'].curvature = curvature
|
||||
original_update_calculations(sm)
|
||||
|
||||
patches.enter_context(mock.patch.object(planner.scc.vision, "_update_calculations", side_effect=inject_constant_curvature))
|
||||
|
||||
original_update = planner.update
|
||||
|
||||
def enable_longitudinal(sm):
|
||||
sm['carControl'].enabled = True
|
||||
sm['carControl'].longActive = True
|
||||
original_update(sm)
|
||||
|
||||
patches.enter_context(mock.patch.object(planner, "update", side_effect=enable_longitudinal))
|
||||
rows = []
|
||||
while plant.current_time < duration:
|
||||
output = plant.step(v_cruise=cruise)
|
||||
rows.append(
|
||||
(
|
||||
plant.current_time,
|
||||
output['speed'],
|
||||
output['should_stop'],
|
||||
planner.scc.vision.is_active,
|
||||
planner.source == LongitudinalPlanSource.sccVision,
|
||||
planner.scc.vision.output_v_target,
|
||||
)
|
||||
)
|
||||
|
||||
data = np.asarray(rows, dtype=float)
|
||||
gc.collect()
|
||||
return {
|
||||
'time': data[:, 0],
|
||||
'speed': data[:, 1],
|
||||
'should_stop': data[:, 2],
|
||||
'active': data[:, 3],
|
||||
'scc_source': data[:, 4],
|
||||
'target': data[:, 5],
|
||||
'solver_failures': np.asarray(solver_failures),
|
||||
}
|
||||
|
||||
|
||||
class TestVisionControllerClosedLoop(OpenpilotTestCase):
|
||||
def test_constant_curve_recovers_like_stock_speed_cap(self):
|
||||
target = (_A_LAT_REG_MAX / 0.005) ** 0.5
|
||||
scc = _run_constant_curve(scc_enabled=True, cruise=30.0)
|
||||
stock = _run_constant_curve(scc_enabled=False, cruise=target)
|
||||
scc_final = scc['speed'][scc['time'] >= 60.0]
|
||||
stock_final = stock['speed'][stock['time'] >= 60.0]
|
||||
|
||||
# The generated solver can report platform-specific failures for the
|
||||
# synthetic no-lead plant. The feature must not make that stock baseline
|
||||
# worse; requiring an absolute zero would hide a harness difference as a
|
||||
# controller regression.
|
||||
assert scc['solver_failures'] <= stock['solver_failures']
|
||||
assert not scc['should_stop'].any()
|
||||
assert np.all(scc['active'][scc['time'] >= 60.0])
|
||||
assert np.all(scc['scc_source'][scc['time'] >= 60.0])
|
||||
assert np.allclose(scc['target'][scc['time'] >= 60.0], target)
|
||||
assert scc_final.min() >= target - 1.0
|
||||
assert abs(scc_final.mean() - stock_final.mean()) < 0.5
|
||||
assert abs(scc_final.min() - stock_final.min()) < 1.0
|
||||
assert abs(scc_final.max() - stock_final.max()) < 1.0
|
||||
+61
-89
@@ -23,21 +23,25 @@ _ENTERING_PRED_LAT_ACC_TH = 1.3 # Predicted Lat Acc threshold to trigger enteri
|
||||
_ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1 # Predicted Lat Acc threshold to abort entering state if speed drops.
|
||||
|
||||
_TURNING_LAT_ACC_TH = 1.6 # Lat Acc threshold to trigger turning state.
|
||||
_URGENT_PRED_LAT_ACC_TH = 3. # Predicted Lat Acc threshold that requires an immediate speed reduction.
|
||||
|
||||
_LEAVING_LAT_ACC_TH = 1.3 # Lat Acc threshold to trigger leaving turn state.
|
||||
_FINISH_LAT_ACC_TH = 1.1 # Lat Acc threshold to trigger the end of the turn cycle.
|
||||
|
||||
_A_LAT_REG_MAX = 2. # Maximum lateral acceleration
|
||||
|
||||
_RELIEF_CONFIRMATION_FRAMES = max(1, int(round(0.5 / DT_MDL)))
|
||||
_TARGET_TIGHTEN_CONFIRMATION_FRAMES = max(1, int(round(0.1 / DT_MDL)))
|
||||
_TARGET_RELEASE_CONFIRMATION_FRAMES = max(1, int(round(0.15 / DT_MDL)))
|
||||
_TARGET_TIGHTEN_RATE = 5. # m/s^2
|
||||
_TARGET_RELEASE_RATE = 1. # m/s^2
|
||||
_BELOW_EGO_TARGET_RELEASE_RATE = 3. # m/s^2
|
||||
_MIN_PRED_SPEED = 1. # m/s
|
||||
_MIN_ACTIVATION_SPEED = 10. # m/s
|
||||
_NO_OVERSHOOT_TIME_HORIZON = 4. # s. Time to use for velocity desired based on a_target when not overshooting.
|
||||
|
||||
# Lookup table for the minimum smooth deceleration during the ENTERING state
|
||||
# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
|
||||
_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.] # min decel value allowed on ENTERING state
|
||||
_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.] # absolute value of lat acc ahead
|
||||
|
||||
# Lookup table for the acceleration for the TURNING state
|
||||
# depending on the current lateral acceleration of the vehicle.
|
||||
_TURNING_ACC_V = [0.5, 0., -0.4] # acc value
|
||||
_TURNING_ACC_BP = [1.5, 2.3, 3.] # absolute value of current lat acc
|
||||
|
||||
_LEAVING_ACC = 0.5 # Conformable acceleration to regain speed while leaving a turn.
|
||||
|
||||
|
||||
class SmartCruiseControlVision:
|
||||
@@ -61,62 +65,14 @@ class SmartCruiseControlVision:
|
||||
self.state = VisionState.disabled
|
||||
self.current_lat_acc = 0.
|
||||
self.max_pred_lat_acc = 0.
|
||||
self.relief_frames = 0
|
||||
self.tighten_frames = 0
|
||||
self.release_frames = 0
|
||||
|
||||
def _v_demand(self) -> float:
|
||||
return max(MIN_V, min(self.v_target, self.v_cruise_setpoint))
|
||||
|
||||
def _curve_is_urgent(self) -> bool:
|
||||
return self.current_lat_acc >= _TURNING_LAT_ACC_TH or self.max_pred_lat_acc >= _URGENT_PRED_LAT_ACC_TH
|
||||
|
||||
def _filtered_v_target(self) -> float:
|
||||
demand = self._v_demand()
|
||||
|
||||
if self.output_v_target == V_CRUISE_UNSET:
|
||||
self.tighten_frames = 0
|
||||
self.release_frames = 0
|
||||
if self._curve_is_urgent():
|
||||
return demand
|
||||
return max(demand, min(self.v_ego, self.v_cruise_setpoint))
|
||||
|
||||
if demand < self.output_v_target:
|
||||
self.release_frames = 0
|
||||
if self._curve_is_urgent():
|
||||
self.tighten_frames = 0
|
||||
return demand
|
||||
|
||||
self.tighten_frames += 1
|
||||
if self.tighten_frames < _TARGET_TIGHTEN_CONFIRMATION_FRAMES:
|
||||
return self.output_v_target
|
||||
return max(demand, self.output_v_target - _TARGET_TIGHTEN_RATE * DT_MDL)
|
||||
|
||||
self.tighten_frames = 0
|
||||
releasing_brake = self.output_v_target < min(self.v_ego, demand)
|
||||
if not releasing_brake and self.relief_frames < _RELIEF_CONFIRMATION_FRAMES:
|
||||
self.release_frames = 0
|
||||
return self.output_v_target
|
||||
|
||||
if demand > self.output_v_target:
|
||||
self.release_frames += 1
|
||||
if self.release_frames < _TARGET_RELEASE_CONFIRMATION_FRAMES:
|
||||
return self.output_v_target
|
||||
else:
|
||||
self.release_frames = 0
|
||||
|
||||
release_rate = _BELOW_EGO_TARGET_RELEASE_RATE if releasing_brake else _TARGET_RELEASE_RATE
|
||||
return min(demand, self.output_v_target + release_rate * DT_MDL)
|
||||
|
||||
def get_a_target_from_control(self) -> float:
|
||||
return self.a_ego
|
||||
return self.a_target
|
||||
|
||||
def get_v_target_from_control(self) -> float:
|
||||
if self.is_active:
|
||||
return self._filtered_v_target()
|
||||
return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
|
||||
|
||||
self.tighten_frames = 0
|
||||
self.release_frames = 0
|
||||
return V_CRUISE_UNSET
|
||||
|
||||
def _update_params(self) -> None:
|
||||
@@ -126,27 +82,25 @@ class SmartCruiseControlVision:
|
||||
def _update_calculations(self, sm: messaging.SubMaster) -> None:
|
||||
if not self.long_enabled:
|
||||
return
|
||||
else:
|
||||
rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z))
|
||||
vel_plan = np.array(sm['modelV2'].velocity.x)
|
||||
|
||||
rate_plan = np.asarray(np.abs(sm['modelV2'].orientationRate.z), dtype=float)
|
||||
vel_plan = np.asarray(sm['modelV2'].velocity.x, dtype=float)
|
||||
size = min(len(rate_plan), len(vel_plan))
|
||||
rate_plan, vel_plan = rate_plan[:size], vel_plan[:size]
|
||||
valid = np.isfinite(rate_plan) & np.isfinite(vel_plan) & (vel_plan >= _MIN_PRED_SPEED)
|
||||
self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
|
||||
|
||||
self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
|
||||
self.max_pred_lat_acc = 0.
|
||||
self.v_target = V_CRUISE_UNSET
|
||||
if np.any(valid):
|
||||
self.max_pred_lat_acc = float(np.percentile(rate_plan[valid] * vel_plan[valid], 97))
|
||||
max_pred_curvature = float(np.percentile(rate_plan[valid] / vel_plan[valid], 97))
|
||||
if max_pred_curvature > 0.:
|
||||
self.v_target = min(float((_A_LAT_REG_MAX / max_pred_curvature) ** 0.5), V_CRUISE_UNSET)
|
||||
# get the maximum lat accel from the model
|
||||
predicted_lat_accels = rate_plan * vel_plan
|
||||
self.max_pred_lat_acc = np.percentile(predicted_lat_accels, 97)
|
||||
|
||||
# get the maximum curve based on the current velocity
|
||||
v_ego = max(self.v_ego, 0.1) # ensure a value greater than 0 for calculations
|
||||
max_curve = self.max_pred_lat_acc / (v_ego**2)
|
||||
|
||||
# Get the target velocity for the maximum curve
|
||||
self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5
|
||||
|
||||
def _update_state_machine(self) -> tuple[bool, bool]:
|
||||
# ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING
|
||||
relief = self.current_lat_acc < _FINISH_LAT_ACC_TH and self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH
|
||||
self.relief_frames = self.relief_frames + 1 if self.state in ACTIVE_STATES and relief else 0
|
||||
|
||||
if self.state != VisionState.disabled:
|
||||
# longitudinal and feature disable always have priority in a non-disabled state
|
||||
if not self.long_enabled or not self.enabled:
|
||||
@@ -158,7 +112,7 @@ class SmartCruiseControlVision:
|
||||
# ENABLED
|
||||
if self.state == VisionState.enabled:
|
||||
# Do not enter a turn control cycle if the speed is low.
|
||||
if self.v_ego <= _MIN_ACTIVATION_SPEED:
|
||||
if self.v_ego <= MIN_V:
|
||||
pass
|
||||
# If significant lateral acceleration is predicted ahead, then move to Entering turn state.
|
||||
elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
|
||||
@@ -174,26 +128,23 @@ class SmartCruiseControlVision:
|
||||
# Transition to Turning if current lateral acceleration is over the threshold.
|
||||
if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
|
||||
self.state = VisionState.turning
|
||||
# Begin releasing only after both current and predicted lateral acceleration stay clear.
|
||||
elif self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES:
|
||||
self.state = VisionState.leaving
|
||||
# Abort if the predicted lateral acceleration drops
|
||||
elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
|
||||
self.state = VisionState.enabled
|
||||
|
||||
# TURNING
|
||||
elif self.state == VisionState.turning:
|
||||
# Transition out of Turning if current lateral acceleration drops below a threshold.
|
||||
# Transition to Leaving if current lateral acceleration drops below a threshold.
|
||||
if self.current_lat_acc <= _LEAVING_LAT_ACC_TH:
|
||||
self.state = VisionState.entering if self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH else VisionState.leaving
|
||||
self.state = VisionState.leaving
|
||||
|
||||
# LEAVING
|
||||
elif self.state == VisionState.leaving:
|
||||
# Transition back to Turning if current lateral acceleration goes back over the threshold.
|
||||
if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
|
||||
self.state = VisionState.turning
|
||||
# Start a new turn cycle immediately if another curve is predicted.
|
||||
elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
|
||||
self.state = VisionState.entering
|
||||
# Finish after confirmed relief and a gradual release to the cruise setpoint.
|
||||
elif self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES and self.output_v_target >= self.v_cruise_setpoint:
|
||||
# Finish if current lateral acceleration goes below a threshold.
|
||||
elif self.current_lat_acc < _FINISH_LAT_ACC_TH:
|
||||
self.state = VisionState.enabled
|
||||
|
||||
# DISABLED
|
||||
@@ -206,11 +157,32 @@ class SmartCruiseControlVision:
|
||||
|
||||
enabled = self.state in ENABLED_STATES
|
||||
active = self.state in ACTIVE_STATES
|
||||
if not active:
|
||||
self.relief_frames = 0
|
||||
|
||||
return enabled, active
|
||||
|
||||
def _update_solution(self) -> float:
|
||||
# DISABLED, ENABLED, OVERRIDING
|
||||
if self.state not in ACTIVE_STATES:
|
||||
# when not overshooting, calculate v_turn as the speed at the prediction horizon when following
|
||||
# the smooth deceleration.
|
||||
a_target = self.a_ego
|
||||
# ENTERING
|
||||
elif self.state == VisionState.entering:
|
||||
# when not overshooting, target a smooth deceleration in preparation for a sharp turn to come.
|
||||
a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V)
|
||||
# TURNING
|
||||
elif self.state == VisionState.turning:
|
||||
# When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt.
|
||||
a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V)
|
||||
# LEAVING
|
||||
elif self.state == VisionState.leaving:
|
||||
# When leaving, we provide a comfortable acceleration to regain speed.
|
||||
a_target = _LEAVING_ACC
|
||||
else:
|
||||
raise NotImplementedError(f"SCC-V state not supported: {self.state}")
|
||||
|
||||
return a_target
|
||||
|
||||
def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float,
|
||||
v_cruise_setpoint: float) -> None:
|
||||
self.long_enabled = long_enabled
|
||||
@@ -223,7 +195,7 @@ class SmartCruiseControlVision:
|
||||
self._update_calculations(sm)
|
||||
|
||||
self.is_enabled, self.is_active = self._update_state_machine()
|
||||
self.a_target = self.a_ego
|
||||
self.a_target = self._update_solution()
|
||||
|
||||
self.output_v_target = self.get_v_target_from_control()
|
||||
self.output_a_target = self.get_a_target_from_control()
|
||||
|
||||
@@ -1,266 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from types import SimpleNamespace
|
||||
from unittest import mock
|
||||
|
||||
from openpilot.cereal import log
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.lead_departure_controller import LeadDepartureController
|
||||
from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP
|
||||
|
||||
|
||||
MpcPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
|
||||
|
||||
|
||||
def make_lead(*, d_rel: float = 4.0, v_lead: float = 0.5, v_rel: float = 0.5, present: bool = True, radar: bool = True, track_id: int = 7):
|
||||
return SimpleNamespace(dRel=d_rel, vLeadK=v_lead, vRel=v_rel, present=present, radar=radar, radarTrackId=track_id)
|
||||
|
||||
|
||||
def make_sm(
|
||||
*,
|
||||
lead_one=None,
|
||||
lead_two=None,
|
||||
v_ego: float = 0.0,
|
||||
long_active: bool = True,
|
||||
long_state=LongCtrlState.stopping,
|
||||
gas: bool = False,
|
||||
brake: bool = False,
|
||||
override: bool = False,
|
||||
force_decel: bool = False,
|
||||
radar_error: str | None = None,
|
||||
):
|
||||
errors = SimpleNamespace(canError=False, radarFault=False, wrongConfig=False, radarUnavailableTemporary=False)
|
||||
if radar_error is not None:
|
||||
setattr(errors, radar_error, True)
|
||||
return {
|
||||
'carState': SimpleNamespace(vEgo=v_ego, gasPressed=gas, brakePressed=brake),
|
||||
'carControl': SimpleNamespace(longActive=long_active, cruiseControl=SimpleNamespace(override=override)),
|
||||
'controlsState': SimpleNamespace(longControlState=long_state, forceDecel=force_decel),
|
||||
'radarState': SimpleNamespace(leadOne=lead_one or make_lead(), leadTwo=lead_two or make_lead(track_id=8), radarErrors=errors),
|
||||
}
|
||||
|
||||
|
||||
def update(controller, sm, *, source=MpcPlanSource.lead0, a_target: float = 0.05, should_stop: bool = True, reset: bool = False, radar_valid: bool = True):
|
||||
return controller.update(sm, source, a_target, should_stop, reset, radar_valid)
|
||||
|
||||
|
||||
def activate(controller: LeadDepartureController):
|
||||
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.00)))
|
||||
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.01)))
|
||||
assert not update(controller, make_sm(lead_one=make_lead(d_rel=4.04)))
|
||||
assert controller.active
|
||||
|
||||
|
||||
def run_closed_loop(controller_enabled: bool, gap: float, lead_speed, duration: float, model_should_stop: bool | None = None):
|
||||
def observe_lead(_t, _name, truth):
|
||||
truth.update(radar=True, radarTrackId=7)
|
||||
return truth
|
||||
|
||||
def model_action(_t, _v_ego, _a_ego):
|
||||
return 0.0, bool(model_should_stop)
|
||||
|
||||
plant = PlantSP(
|
||||
lead_relevancy=True,
|
||||
speed=0.0,
|
||||
distance_lead=gap,
|
||||
lead_observation_fn=observe_lead,
|
||||
actuator_model=PRIUS_TSS2_ROUTE_MODEL,
|
||||
run_long_control=True,
|
||||
e2e=model_should_stop is not None,
|
||||
model_action_fn=model_action if model_should_stop is not None else None,
|
||||
)
|
||||
plant.planner.lead_departure_controller.enabled = controller_enabled
|
||||
|
||||
original_update = plant.planner.update
|
||||
|
||||
def long_active_update(sm):
|
||||
sm['carControl'].longActive = True
|
||||
original_update(sm)
|
||||
|
||||
solver_resets = 0
|
||||
original_reset = plant.planner.mpc.reset
|
||||
|
||||
def counted_reset(*args, **kwargs):
|
||||
nonlocal solver_resets
|
||||
if plant.planner.mpc.solution_status != 0:
|
||||
solver_resets += 1
|
||||
return original_reset(*args, **kwargs)
|
||||
|
||||
rows = []
|
||||
active = []
|
||||
with (
|
||||
mock.patch.object(plant.planner, 'get_max_accel_override', return_value=None),
|
||||
mock.patch.object(plant.planner, 'get_min_accel_override', return_value=None),
|
||||
mock.patch.object(plant.planner, 'update', side_effect=long_active_update),
|
||||
mock.patch.object(plant.planner.mpc, 'reset', side_effect=counted_reset),
|
||||
):
|
||||
for _ in range(round(duration / DT_MDL)):
|
||||
t = plant.current_time
|
||||
result = plant.step(v_lead=lead_speed(t), v_cruise=8.0)
|
||||
rows.append(
|
||||
(t, result['speed'], result['distance'], result['distance_lead'] - result['distance'], result['actuator_command'], result['should_stop'], result['fcw'])
|
||||
)
|
||||
active.append(plant.planner.lead_departure_controller.active)
|
||||
|
||||
return rows, active, solver_resets
|
||||
|
||||
|
||||
def first_delay(rows, cue: float, column: int, predicate):
|
||||
return next(row[0] - cue for row in rows if row[0] >= cue and predicate(row[column]))
|
||||
|
||||
|
||||
class TestLeadDepartureController(OpenpilotTestCase):
|
||||
def test_requires_three_coherent_radar_frames(self):
|
||||
controller = LeadDepartureController(True)
|
||||
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.00)))
|
||||
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.01)))
|
||||
assert not update(controller, make_sm(lead_one=make_lead(d_rel=4.04)))
|
||||
assert controller.active
|
||||
|
||||
def test_distance_confirmation_uses_a_sliding_three_frame_window(self):
|
||||
controller = LeadDepartureController(True)
|
||||
for d_rel in (4.00, 4.01, 4.02, 4.03):
|
||||
assert update(controller, make_sm(lead_one=make_lead(d_rel=d_rel)))
|
||||
assert not controller.active
|
||||
|
||||
assert not update(controller, make_sm(lead_one=make_lead(d_rel=4.06)))
|
||||
assert controller.active
|
||||
|
||||
def test_persistent_false_speed_cue_with_static_range_never_arms(self):
|
||||
controller = LeadDepartureController(True)
|
||||
for _ in range(10):
|
||||
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.0)))
|
||||
assert not controller.active
|
||||
|
||||
def test_same_track_can_move_between_lead_slots(self):
|
||||
controller = LeadDepartureController(True)
|
||||
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.00)), source=MpcPlanSource.lead0)
|
||||
assert update(controller, make_sm(lead_two=make_lead(d_rel=4.01)), source=MpcPlanSource.lead1)
|
||||
assert not update(controller, make_sm(lead_one=make_lead(d_rel=4.04)), source=MpcPlanSource.lead0)
|
||||
|
||||
def test_different_track_restarts_confirmation(self):
|
||||
controller = LeadDepartureController(True)
|
||||
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.00, track_id=7)))
|
||||
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.02, track_id=7)))
|
||||
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.20, track_id=9)))
|
||||
assert update(controller, make_sm(lead_one=make_lead(d_rel=4.22, track_id=9)))
|
||||
assert not update(controller, make_sm(lead_one=make_lead(d_rel=4.24, track_id=9)))
|
||||
|
||||
def test_active_release_latches_through_native_threshold_churn(self):
|
||||
controller = LeadDepartureController(True)
|
||||
activate(controller)
|
||||
sm = make_sm(lead_one=make_lead(d_rel=4.10), long_state=LongCtrlState.pid)
|
||||
assert not update(controller, sm, a_target=0.12, should_stop=False)
|
||||
assert not update(controller, sm, a_target=0.05, should_stop=True)
|
||||
assert controller.active
|
||||
|
||||
def test_active_release_latches_across_same_track_source_churn(self):
|
||||
controller = LeadDepartureController(True)
|
||||
activate(controller)
|
||||
sm = make_sm(lead_two=make_lead(d_rel=4.10), long_state=LongCtrlState.pid)
|
||||
assert not update(controller, sm, source=MpcPlanSource.lead1)
|
||||
assert controller.active
|
||||
|
||||
def test_active_release_cancels_on_invalid_state(self):
|
||||
cases = (
|
||||
('lead lost', make_sm(lead_one=make_lead(present=False))),
|
||||
('vision lead', make_sm(lead_one=make_lead(radar=False))),
|
||||
('track changed', make_sm(lead_one=make_lead(track_id=9))),
|
||||
('lead too slow', make_sm(lead_one=make_lead(v_lead=0.29))),
|
||||
('relative speed too low', make_sm(lead_one=make_lead(v_rel=0.29))),
|
||||
('gas', make_sm(gas=True)),
|
||||
('brake', make_sm(brake=True)),
|
||||
('override', make_sm(override=True)),
|
||||
('force decel', make_sm(force_decel=True)),
|
||||
('long inactive', make_sm(long_active=False)),
|
||||
('long control off', make_sm(long_state=LongCtrlState.off)),
|
||||
('ego rolling', make_sm(v_ego=0.3)),
|
||||
('radar CAN error', make_sm(radar_error='canError')),
|
||||
('radar fault', make_sm(radar_error='radarFault')),
|
||||
('radar config', make_sm(radar_error='wrongConfig')),
|
||||
('radar unavailable', make_sm(radar_error='radarUnavailableTemporary')),
|
||||
)
|
||||
for name, sm in cases:
|
||||
with self.subTest(name=name):
|
||||
controller = LeadDepartureController(True)
|
||||
activate(controller)
|
||||
assert update(controller, sm)
|
||||
assert not controller.active
|
||||
|
||||
def test_active_release_cancels_on_invalid_update_input(self):
|
||||
cases = (('negative target', -0.01, False, True), ('reset', 0.05, True, True), ('radar invalid', 0.05, False, False))
|
||||
for name, a_target, reset, radar_valid in cases:
|
||||
with self.subTest(name=name):
|
||||
controller = LeadDepartureController(True)
|
||||
activate(controller)
|
||||
assert update(controller, make_sm(), a_target=a_target, reset=reset, radar_valid=radar_valid)
|
||||
assert not controller.active
|
||||
|
||||
def test_inactive_controller_arms_only_from_native_stop_and_stopping_state(self):
|
||||
controller = LeadDepartureController(True)
|
||||
for d_rel in (4.00, 4.02, 4.04):
|
||||
assert not update(controller, make_sm(lead_one=make_lead(d_rel=d_rel)), should_stop=False)
|
||||
for d_rel in (4.00, 4.02, 4.04):
|
||||
assert update(controller, make_sm(lead_one=make_lead(d_rel=d_rel), long_state=LongCtrlState.pid))
|
||||
assert not controller.active
|
||||
|
||||
def test_capability_gate_disables_controller(self):
|
||||
controller = LeadDepartureController(False)
|
||||
for d_rel in (4.00, 4.02, 4.04):
|
||||
assert update(controller, make_sm(lead_one=make_lead(d_rel=d_rel)))
|
||||
assert not controller.active
|
||||
|
||||
def test_closed_loop_departure_releases_earlier_without_a_safety_regression(self):
|
||||
lead_accel = 0.31
|
||||
cue = 1.0 + 0.4 / lead_accel
|
||||
|
||||
def lead_speed(t):
|
||||
return 0.0 if t < 1.0 else min(5.0, lead_accel * (t - 1.0))
|
||||
|
||||
stock, stock_active, stock_resets = run_closed_loop(False, 3.81, lead_speed, 8.0)
|
||||
controller, controller_active, controller_resets = run_closed_loop(True, 3.81, lead_speed, 8.0)
|
||||
|
||||
stock_release = first_delay(stock, cue, 5, lambda should_stop: not should_stop)
|
||||
controller_release = first_delay(controller, cue, 5, lambda should_stop: not should_stop)
|
||||
stock_motion = first_delay(stock, cue, 1, lambda speed: speed > 0.01)
|
||||
controller_motion = first_delay(controller, cue, 1, lambda speed: speed > 0.01)
|
||||
stock_v01 = first_delay(stock, cue, 1, lambda speed: speed > 0.1)
|
||||
controller_v01 = first_delay(controller, cue, 1, lambda speed: speed > 0.1)
|
||||
|
||||
assert any(controller_active) and not any(stock_active)
|
||||
assert stock_resets == controller_resets == 0
|
||||
assert not any(row[6] for row in stock + controller)
|
||||
assert controller_release <= stock_release - 1.0
|
||||
assert controller_motion <= stock_motion - 0.1
|
||||
assert controller_v01 <= stock_v01 - 0.1
|
||||
assert min(row[3] for row in controller) >= min(row[3] for row in stock)
|
||||
assert max(abs(right[4] - left[4]) for left, right in zip(controller, controller[1:], strict=False)) <= max(
|
||||
abs(right[4] - left[4]) for left, right in zip(stock, stock[1:], strict=False)
|
||||
)
|
||||
|
||||
def test_model_stop_remains_authoritative(self):
|
||||
def lead_speed(t):
|
||||
return 0.0 if t < 1.0 else min(5.0, 0.8 * (t - 1.0))
|
||||
|
||||
rows, active, solver_resets = run_closed_loop(True, 4.0, lead_speed, 6.0, model_should_stop=True)
|
||||
|
||||
assert any(active)
|
||||
assert solver_resets == 0
|
||||
assert all(row[5] for row in rows)
|
||||
assert all(row[1] == 0.0 and row[2] == 0.0 for row in rows)
|
||||
assert not any(row[6] for row in rows)
|
||||
|
||||
def test_stationary_lead_remains_stock_identical(self):
|
||||
stock, stock_active, stock_resets = run_closed_loop(False, 8.0, lambda _t: 0.0, 12.0)
|
||||
controller, controller_active, controller_resets = run_closed_loop(True, 8.0, lambda _t: 0.0, 12.0)
|
||||
|
||||
assert stock == controller
|
||||
assert not any(stock_active) and not any(controller_active)
|
||||
assert stock_resets == controller_resets == 0
|
||||
@@ -1,642 +0,0 @@
|
||||
import numpy as np
|
||||
from unittest import mock
|
||||
|
||||
from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs
|
||||
from openpilot.common.parameterized import parameterized
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from opendbc.car.body.values import CAR as BODY
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.ford.values import CAR as FORD
|
||||
from opendbc.car.gm.values import CAR as GM
|
||||
from opendbc.car.honda.values import CAR as HONDA
|
||||
from opendbc.car.hyundai.values import CAR as HYUNDAI
|
||||
from opendbc.car.rivian.values import CAR as RIVIAN
|
||||
from opendbc.car.subaru.values import CAR as SUBARU
|
||||
from opendbc.car.tesla.values import CAR as TESLA
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import should_stop
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import (
|
||||
STOPPING_HOLD_ACCEL, STOPPING_HOLD_MARGIN, STOPPING_SETTLE_FRAMES, STOPPING_SPEED_TOLERANCE,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP
|
||||
|
||||
|
||||
PRESERVED_HOLD_VEHICLES = (
|
||||
FORD.FORD_ESCAPE_MK4,
|
||||
GM.CHEVROLET_VOLT,
|
||||
GM.CHEVROLET_BOLT_EUV,
|
||||
HONDA.HONDA_CIVIC_2022,
|
||||
HYUNDAI.HYUNDAI_SONATA,
|
||||
SUBARU.SUBARU_ASCENT,
|
||||
TESLA.TESLA_MODEL_3,
|
||||
TOYOTA.TOYOTA_RAV4_TSS2,
|
||||
VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1,
|
||||
)
|
||||
STOP_ACCEL_VEHICLES = (*PRESERVED_HOLD_VEHICLES, RIVIAN.RIVIAN_R1)
|
||||
SETTLE_VEHICLES = (TOYOTA.TOYOTA_RAV4_TSS2, HONDA.HONDA_CIVIC_2022, VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1)
|
||||
UNSUPPORTED_HOLD_VEHICLES = (
|
||||
(BODY.COMMA_BODY, True),
|
||||
(SUBARU.SUBARU_OUTBACK, True),
|
||||
(HYUNDAI.HYUNDAI_SONATA, False),
|
||||
(RIVIAN.RIVIAN_R1, True),
|
||||
)
|
||||
ROUTE_STOP_ONSETS = (
|
||||
(0.280, -0.290, -0.220, -0.220),
|
||||
(0.290, -0.497, -0.270, -0.302),
|
||||
(0.464, -0.223, -0.264, -0.292),
|
||||
(0.467, -0.582, -0.316, -0.359),
|
||||
(0.530, -0.311, -0.309, -0.333),
|
||||
(0.581, -0.467, -0.312, -0.352),
|
||||
(0.398, -0.557, -0.311, -0.348),
|
||||
(0.517, -0.290, -0.301, -0.327),
|
||||
(0.312, -0.420, -0.271, -0.304),
|
||||
(0.474, -0.509, -0.303, -0.347),
|
||||
(0.241, -0.554, -0.573, -0.617),
|
||||
(0.292, -0.154, -0.302, -0.326),
|
||||
)
|
||||
GRADE_HOLD_CASES = (
|
||||
(-0.49, -1.40),
|
||||
(0.00, -1.40),
|
||||
(0.49, -1.40),
|
||||
(0.75, -1.40),
|
||||
(0.98, -1.65),
|
||||
(1.25, -2.00),
|
||||
(1.47, -2.00),
|
||||
)
|
||||
|
||||
|
||||
def get_car_params(candidate, experimental_long=True):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
interface = interfaces[candidate]
|
||||
CP = interface.get_params(candidate, fingerprint, [], experimental_long, False, False)
|
||||
return CP, interface.get_params_sp(CP, candidate, fingerprint, [], experimental_long, False, False)
|
||||
|
||||
|
||||
def make_car_state(v_ego=0.2, a_ego=0.0, standstill=False, v_ego_raw=None) -> structs.CarState:
|
||||
raw_speed = v_ego if v_ego_raw is None else v_ego_raw
|
||||
state = structs.CarState(vEgo=float(v_ego), vEgoRaw=float(raw_speed), aEgo=float(a_ego), standstill=standstill)
|
||||
state.cruiseState.standstill = standstill
|
||||
return state
|
||||
|
||||
|
||||
def make_control(candidate, initial_accel=-0.33, experimental_long=True):
|
||||
CP, CP_SP = get_car_params(candidate, experimental_long)
|
||||
control = LongControl(CP, CP_SP)
|
||||
control.long_control_state = LongCtrlState.pid
|
||||
control.last_output_accel = initial_accel
|
||||
return CP, control
|
||||
|
||||
|
||||
def stock_stopping_output(output_accel, stop_accel):
|
||||
return min(output_accel, 0.0) - DT_CTRL if output_accel > stop_accel else output_accel
|
||||
|
||||
|
||||
def expected_hold_accel(CP, initial_accel=-0.33):
|
||||
minimum_hold = min(STOPPING_HOLD_ACCEL, CP.stopAccel + STOPPING_HOLD_MARGIN)
|
||||
return min(initial_accel, max(CP.stopAccel, minimum_hold))
|
||||
|
||||
|
||||
def settle_preserved_hold(control):
|
||||
control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
|
||||
CS = make_car_state(0.0, 0.0, standstill=True)
|
||||
for _ in range(round(4.0 / DT_CTRL)):
|
||||
control.update(True, CS, -0.1, True, (-3.5, 2.0))
|
||||
|
||||
|
||||
class TestLongControlSP(OpenpilotTestCase):
|
||||
def test_stop_threshold_matches_the_shared_helper(self):
|
||||
assert should_stop(0.29, 0.0)
|
||||
assert not should_stop(0.3, 0.0)
|
||||
assert not should_stop(0.29, 0.1)
|
||||
|
||||
def test_hold_scope_matches_every_car_interface(self):
|
||||
for candidate in interfaces:
|
||||
for experimental_long in (False, True):
|
||||
with self.subTest(candidate=candidate, experimental_long=experimental_long):
|
||||
CP, control = make_control(candidate, experimental_long=experimental_long)
|
||||
output = control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
|
||||
supported = CP.openpilotLongitudinalControl and not CP.notCar and CP.stopAccel < 0.0
|
||||
|
||||
self.assertAlmostEqual(output, -0.33)
|
||||
assert (control._stopping_hold_accel is not None) == supported
|
||||
|
||||
@parameterized.expand(UNSUPPORTED_HOLD_VEHICLES, names=("candidate", "experimental_long"))
|
||||
def test_unsupported_hold_semantics_keep_the_cache_disabled(self, candidate, experimental_long):
|
||||
_, control = make_control(candidate, experimental_long=experimental_long)
|
||||
control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
|
||||
|
||||
assert control._stopping_hold_accel is None
|
||||
|
||||
@parameterized.expand(ROUTE_STOP_ONSETS, names=("v_ego", "a_ego", "a_target", "initial_accel"))
|
||||
def test_logged_stop_onsets_hold_the_existing_brake(self, v_ego, a_ego, a_target, initial_accel):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
|
||||
output = control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0))
|
||||
assert control.long_control_state == LongCtrlState.stopping
|
||||
self.assertAlmostEqual(output, initial_accel)
|
||||
|
||||
@parameterized.expand(ROUTE_STOP_ONSETS, names=("v_ego", "a_ego", "a_target", "initial_accel"))
|
||||
def test_logged_stop_onsets_preserve_a_settled_hold(self, v_ego, a_ego, a_target, initial_accel):
|
||||
CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
|
||||
control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0))
|
||||
CS = make_car_state(0.0, 0.0, standstill=True)
|
||||
outputs = [control.update(True, CS, a_target, True, (-3.5, 2.0)) for _ in range(round(10.0 / DT_CTRL))]
|
||||
|
||||
hold_floor = expected_hold_accel(CP, initial_accel)
|
||||
self.assertAlmostEqual(outputs[-1], hold_floor)
|
||||
np.testing.assert_allclose(outputs[-100:], outputs[-1], rtol=0.0, atol=1e-12)
|
||||
assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
|
||||
|
||||
@parameterized.expand(PRESERVED_HOLD_VEHICLES, names=("candidate",))
|
||||
def test_preserved_hold_does_not_change_the_moving_approach(self, candidate):
|
||||
CP, control = make_control(candidate)
|
||||
control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
|
||||
moving = [control.update(True, make_car_state(0.25, -0.25), -0.22, True, (-3.5, 2.0)) for _ in range(20)]
|
||||
CS = make_car_state(0.0, 0.0, standstill=True)
|
||||
terminal = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(round(4.0 / DT_CTRL))]
|
||||
|
||||
np.testing.assert_allclose(moving, -0.33, rtol=0.0, atol=1e-12)
|
||||
self.assertAlmostEqual(terminal[-1], expected_hold_accel(CP))
|
||||
|
||||
def test_glide_hold_survives_a_soft_deceleration_sample(self):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
|
||||
samples = ((0.388, -0.201, -0.164), (0.330, -0.120, -0.140), (0.283, -0.0675, -0.120))
|
||||
outputs = [control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0)) for v_ego, a_ego, a_target in samples]
|
||||
|
||||
np.testing.assert_allclose(outputs, [-0.166] * len(samples), rtol=1e-6, atol=1e-12)
|
||||
|
||||
def test_glide_response_reaches_the_stock_rate_when_deceleration_stops(self):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
|
||||
control.update(True, make_car_state(0.388, -0.201), -0.164, True, (-3.5, 2.0))
|
||||
output = control.update(True, make_car_state(0.330, -0.01), -0.140, True, (-3.5, 2.0))
|
||||
|
||||
assert -0.176 < output < -0.175
|
||||
|
||||
def test_glide_response_increases_with_stopping_distance_error(self):
|
||||
_, nominal = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
|
||||
_, distance_error = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
|
||||
for control in (nominal, distance_error):
|
||||
control.update(True, make_car_state(0.388, -0.201), -0.164, True, (-3.5, 2.0))
|
||||
nominal_output = nominal.update(True, make_car_state(0.330, -0.050), -0.140, True, (-3.5, 2.0))
|
||||
distance_error_output = distance_error.update(True, make_car_state(0.400, -0.050), -0.140, True, (-3.5, 2.0))
|
||||
|
||||
assert -0.176 < distance_error_output < nominal_output
|
||||
|
||||
@parameterized.expand(((1.0, 0.0), (0.75, 0.4375), (0.5, 0.75), (0.0, 1.0)), names=("decel_fraction", "expected_rate"))
|
||||
def test_stopping_rate_scales_with_realized_deceleration(self, decel_fraction, expected_rate):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
output = control.update(True, make_car_state(0.3, -0.12 * decel_fraction), 0.0, True, (-3.5, 2.0))
|
||||
|
||||
self.assertAlmostEqual((-0.33 - output) / DT_CTRL, expected_rate, delta=1e-6)
|
||||
|
||||
def test_stopping_rate_scales_with_planner_demand(self):
|
||||
_, gentle = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
_, urgent = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
|
||||
gentle_output = gentle.update(True, make_car_state(0.3, -0.12), -0.34, True, (-3.5, 2.0))
|
||||
urgent_output = urgent.update(True, make_car_state(0.3, -0.12), -1.0, True, (-3.5, 2.0))
|
||||
|
||||
assert -0.331 < gentle_output < -0.33
|
||||
self.assertAlmostEqual(urgent_output, -0.34)
|
||||
|
||||
def test_glide_hold_yields_to_stronger_planner_braking(self):
|
||||
CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
|
||||
control.update(True, make_car_state(0.388, -0.201), -0.164, True, (-3.5, 2.0))
|
||||
output = control.update(True, make_car_state(0.330, -0.120), -1.0, True, (-3.5, 2.0))
|
||||
|
||||
self.assertAlmostEqual(output, stock_stopping_output(-0.166, CP.stopAccel))
|
||||
|
||||
@parameterized.expand(STOP_ACCEL_VEHICLES, names=("candidate",))
|
||||
def test_urgent_braking_matches_the_stock_ramp(self, candidate):
|
||||
CP, control = make_control(candidate)
|
||||
CS = make_car_state(0.8, -0.1)
|
||||
output = control.last_output_accel
|
||||
|
||||
for _ in range(round(1.0 / DT_CTRL)):
|
||||
output = control.update(True, CS, -3.0, True, (-3.5, 2.0))
|
||||
|
||||
expected = -0.33
|
||||
for _ in range(round(1.0 / DT_CTRL)):
|
||||
expected = stock_stopping_output(expected, CP.stopAccel)
|
||||
self.assertAlmostEqual(output, expected)
|
||||
|
||||
@parameterized.expand(STOP_ACCEL_VEHICLES, names=("candidate",))
|
||||
def test_stronger_planner_brake_matches_the_stock_ramp(self, candidate):
|
||||
CP, control = make_control(candidate)
|
||||
outputs = [control.update(True, make_car_state(0.3, -0.3), -1.0, True, (-3.5, 2.0)) for _ in range(10)]
|
||||
expected = []
|
||||
output = -0.33
|
||||
for _ in range(10):
|
||||
output = stock_stopping_output(output, CP.stopAccel)
|
||||
expected.append(output)
|
||||
np.testing.assert_allclose(outputs, expected, rtol=1e-6, atol=1e-12)
|
||||
|
||||
@parameterized.expand(STOP_ACCEL_VEHICLES, names=("candidate",))
|
||||
def test_insufficient_deceleration_uses_most_of_the_stock_ramp(self, candidate):
|
||||
CP, control = make_control(candidate)
|
||||
output = control.update(True, make_car_state(0.6, -0.1), -0.1, True, (-3.5, 2.0))
|
||||
if -0.33 > CP.stopAccel:
|
||||
assert -0.34 < output < -0.338
|
||||
else:
|
||||
self.assertAlmostEqual(output, -0.33)
|
||||
|
||||
def test_deceleration_noise_cannot_release_the_brake(self):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
outputs = [control.update(True, make_car_state(0.3, -0.3 if frame % 2 else 0.0), -0.1, True, (-3.5, 2.0)) for frame in range(40)]
|
||||
assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
|
||||
|
||||
def test_planner_noise_cannot_release_the_brake(self):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
outputs = [control.update(True, make_car_state(0.3, -0.3), -1.0 if frame % 2 else -0.1, True, (-3.5, 2.0)) for frame in range(40)]
|
||||
assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
|
||||
|
||||
@parameterized.expand(
|
||||
(
|
||||
(float("nan"), -0.3, -0.1),
|
||||
(0.3, float("nan"), -0.1),
|
||||
(0.3, -0.3, float("nan")),
|
||||
(float("inf"), -0.3, -0.1),
|
||||
(0.3, -float("inf"), -0.1),
|
||||
(0.3, -0.3, float("inf")),
|
||||
),
|
||||
names=("v_ego", "a_ego", "a_target"),
|
||||
)
|
||||
def test_invalid_state_uses_the_stock_ramp(self, v_ego, a_ego, a_target):
|
||||
CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
output = control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0))
|
||||
self.assertAlmostEqual(output, stock_stopping_output(-0.33, CP.stopAccel))
|
||||
|
||||
@parameterized.expand(
|
||||
(
|
||||
(0.24, 0.0, -0.49, 0.15, 0.0),
|
||||
(0.53, -0.31, -0.49, 0.35, 0.1),
|
||||
(0.24, 0.0, 0.0, 0.15, 0.0),
|
||||
(0.464, -0.223, 0.0, 0.25, 0.05),
|
||||
(0.53, -0.31, 0.0, 0.35, 0.1),
|
||||
(0.24, 0.0, 0.49, 0.15, 0.0),
|
||||
(0.53, -0.31, 0.49, 0.25, 0.05),
|
||||
(0.6, -0.3, 0.49, 0.35, 0.1),
|
||||
(0.6, -0.3, 0.49, 0.5, 0.1),
|
||||
),
|
||||
names=("speed", "initial_accel", "grade_accel", "actuator_lag", "actuator_delay"),
|
||||
)
|
||||
def test_smooth_stop_distance_is_bounded(self, speed, initial_accel, grade_accel, actuator_lag, actuator_delay):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
|
||||
applied_accel = initial_accel
|
||||
delay = [initial_accel] * round(actuator_delay / DT_CTRL)
|
||||
distance = 0.0
|
||||
outputs = []
|
||||
|
||||
for _ in range(round(4.0 / DT_CTRL)):
|
||||
command = control.update(True, make_car_state(speed, applied_accel), -0.1, True, (-3.5, 2.0))
|
||||
outputs.append(command)
|
||||
delayed_command = command
|
||||
if delay:
|
||||
delay.append(command)
|
||||
delayed_command = delay.pop(0)
|
||||
applied_accel += DT_CTRL / actuator_lag * (delayed_command + grade_accel - applied_accel)
|
||||
speed = max(0.0, speed + applied_accel * DT_CTRL)
|
||||
distance += speed * DT_CTRL
|
||||
if speed == 0.0:
|
||||
break
|
||||
|
||||
assert speed == 0.0
|
||||
assert distance < 1.0
|
||||
assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
|
||||
|
||||
@parameterized.expand(STOP_ACCEL_VEHICLES, names=("candidate",))
|
||||
def test_standstill_uses_the_stock_ramp(self, candidate):
|
||||
CP, control = make_control(candidate)
|
||||
control.long_control_state = LongCtrlState.off
|
||||
CS = make_car_state(0.0, 0.0, standstill=True)
|
||||
outputs = [control.update(True, CS, 0.0, False, (-3.5, 2.0)) for _ in range(round(2.0 / DT_CTRL))]
|
||||
expected = -0.33
|
||||
for _ in range(round(2.0 / DT_CTRL)):
|
||||
expected = stock_stopping_output(expected, CP.stopAccel)
|
||||
self.assertAlmostEqual(outputs[0], stock_stopping_output(-0.33, CP.stopAccel))
|
||||
self.assertAlmostEqual(outputs[-1], expected)
|
||||
|
||||
@parameterized.expand(PRESERVED_HOLD_VEHICLES, names=("candidate",))
|
||||
def test_preserved_hold_yields_to_stronger_planner_braking(self, candidate):
|
||||
CP, control = make_control(candidate)
|
||||
settle_preserved_hold(control)
|
||||
previous = control.last_output_accel
|
||||
output = control.update(True, make_car_state(0.0, 0.0, standstill=True), CP.stopAccel, True, (-3.5, 2.0))
|
||||
|
||||
self.assertAlmostEqual(output, stock_stopping_output(previous, CP.stopAccel))
|
||||
|
||||
def test_false_departure_restores_a_stronger_preserved_hold(self):
|
||||
CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
settle_preserved_hold(control)
|
||||
stronger_hold = control.update(True, make_car_state(0.0, 0.0, standstill=True), CP.stopAccel, True, (-3.5, 2.0))
|
||||
departure_state = make_car_state(0.0, 0.0, standstill=True)
|
||||
departure_state.cruiseState.standstill = False
|
||||
control.update(True, departure_state, 0.6, False, (-3.5, 2.0))
|
||||
restored = control.update(True, make_car_state(0.0, 0.0, standstill=True), -0.1, True, (-3.5, 2.0))
|
||||
|
||||
self.assertAlmostEqual(restored, stronger_hold)
|
||||
|
||||
@parameterized.expand(SETTLE_VEHICLES, names=("candidate",))
|
||||
def test_false_departure_restores_a_command_at_the_stop_limit(self, candidate):
|
||||
CP, control = make_control(candidate)
|
||||
strong_hold = max(CP.stopAccel - 0.2, -3.5)
|
||||
control.last_output_accel = strong_hold
|
||||
held = control.update(True, make_car_state(0.0, 0.0, standstill=True), -0.1, True, (-3.5, 2.0))
|
||||
departure_state = make_car_state(0.0, 0.0, standstill=True)
|
||||
departure_state.cruiseState.standstill = False
|
||||
control.update(True, departure_state, 0.6, False, (-3.5, 2.0))
|
||||
restored = control.update(True, make_car_state(0.0, 0.0, standstill=True), -0.1, True, (-3.5, 2.0))
|
||||
|
||||
self.assertAlmostEqual(restored, held)
|
||||
|
||||
@parameterized.expand(SETTLE_VEHICLES, names=("candidate",))
|
||||
def test_false_departure_after_reaching_the_stop_limit_restores_braking(self, candidate):
|
||||
CP, control = make_control(candidate)
|
||||
CS = make_car_state(0.0, 0.0, standstill=True)
|
||||
while control.last_output_accel > CP.stopAccel:
|
||||
reached = control.update(True, CS, -0.1, True, (-3.5, 2.0))
|
||||
|
||||
departure_state = make_car_state(0.0, 0.0, standstill=True)
|
||||
departure_state.cruiseState.standstill = False
|
||||
control.update(True, departure_state, 0.6, False, (-3.5, 2.0))
|
||||
restored = control.update(True, CS, -0.1, True, (-3.5, 2.0))
|
||||
|
||||
self.assertAlmostEqual(restored, reached)
|
||||
|
||||
@parameterized.expand(PRESERVED_HOLD_VEHICLES, names=("candidate",))
|
||||
def test_inadequate_preserved_hold_uses_the_stock_ramp(self, candidate):
|
||||
for v_ego, a_ego, standstill in ((0.0, 0.2, True), (-0.1, 0.0, False)):
|
||||
with self.subTest(v_ego=v_ego, a_ego=a_ego, standstill=standstill):
|
||||
CP, control = make_control(candidate)
|
||||
settle_preserved_hold(control)
|
||||
output = control.last_output_accel
|
||||
expected = output
|
||||
for _ in range(round(4.0 / DT_CTRL)):
|
||||
output = control.update(True, make_car_state(v_ego, a_ego, standstill), -0.1, True, (-3.5, 2.0))
|
||||
expected = max(stock_stopping_output(expected, CP.stopAccel), -3.5)
|
||||
|
||||
self.assertAlmostEqual(output, expected)
|
||||
|
||||
@parameterized.expand(PRESERVED_HOLD_VEHICLES, names=("candidate",))
|
||||
def test_false_departure_restores_the_preserved_hold(self, candidate):
|
||||
_, control = make_control(candidate)
|
||||
settle_preserved_hold(control)
|
||||
hold_accel = control.last_output_accel
|
||||
departure_state = make_car_state(0.0, 0.0, standstill=True)
|
||||
departure_state.cruiseState.standstill = False
|
||||
departure = control.update(True, departure_state, 0.6, False, (-3.5, 2.0))
|
||||
restored = control.update(True, make_car_state(0.0, 0.0, standstill=True), -0.1, True, (-3.5, 2.0))
|
||||
|
||||
assert departure > 0.0
|
||||
self.assertAlmostEqual(restored, hold_accel)
|
||||
|
||||
@parameterized.expand(
|
||||
((True, 0.0, 0.0, True), (False, 0.06, 0.06, False), (False, 0.0, 0.06, False)),
|
||||
names=("inactive", "v_ego", "v_ego_raw", "standstill"),
|
||||
)
|
||||
def test_preserved_hold_clears_after_inactive_or_real_motion(self, inactive, v_ego, v_ego_raw, standstill):
|
||||
CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
settle_preserved_hold(control)
|
||||
control.update(not inactive, make_car_state(v_ego, 0.0, standstill=standstill, v_ego_raw=v_ego_raw), 0.6, False, (-3.5, 2.0))
|
||||
output = control.update(True, make_car_state(0.0, 0.0, standstill=True), -0.1, True, (-3.5, 2.0))
|
||||
|
||||
assert control._stopping_hold_accel is None
|
||||
self.assertAlmostEqual(output, stock_stopping_output(0.0, CP.stopAccel))
|
||||
|
||||
@parameterized.expand(((float("nan"), 0.0), (0.0, float("nan"))), names=("v_ego", "v_ego_raw"))
|
||||
def test_invalid_speed_clears_the_preserved_hold(self, v_ego, v_ego_raw):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
settle_preserved_hold(control)
|
||||
previous = control.last_output_accel
|
||||
output = control.update(True, make_car_state(v_ego, 0.0, standstill=True, v_ego_raw=v_ego_raw), -0.1, True, (-3.5, 2.0))
|
||||
|
||||
assert control._stopping_hold_accel is None
|
||||
self.assertAlmostEqual(output, previous - DT_CTRL)
|
||||
|
||||
@parameterized.expand(((0.005, True), (-0.005, True), (0.02, True), (-0.02, False)), names=("v_ego_raw", "standstill"))
|
||||
def test_raw_wheel_motion_keeps_building_brake(self, v_ego_raw, standstill):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
settle_preserved_hold(control)
|
||||
previous = control.last_output_accel
|
||||
CS = make_car_state(0.0, 0.0, standstill=standstill, v_ego_raw=v_ego_raw)
|
||||
output = control.update(True, CS, -0.1, True, (-3.5, 2.0))
|
||||
|
||||
self.assertAlmostEqual(output, previous - DT_CTRL)
|
||||
|
||||
def test_preserved_hold_removes_launch_brake_backlog(self):
|
||||
CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
|
||||
CS = make_car_state(0.0, 0.0, standstill=True)
|
||||
for _ in range(round(3.0 / DT_CTRL)):
|
||||
control.update(True, CS, -0.1, True, (-3.5, 2.0))
|
||||
preserved_hold = control.last_output_accel
|
||||
|
||||
CS.cruiseState.standstill = False
|
||||
requested_accels = [control.update(True, CS, min(0.15 + frame * DT_CTRL, 1.2), False, (-3.5, 2.0)) for frame in range(round(1.0 / DT_CTRL))]
|
||||
|
||||
def release_time(initial_accel):
|
||||
applied_accel = initial_accel
|
||||
for frame, requested_accel in enumerate(requested_accels):
|
||||
accel_step = PRIUS_TSS2_ROUTE_MODEL.command_rate_limit * DT_CTRL
|
||||
applied_accel += np.clip(requested_accel - applied_accel, -accel_step, accel_step)
|
||||
if applied_accel >= 0.0:
|
||||
return (frame + 1) * DT_CTRL
|
||||
raise AssertionError("brake command did not release")
|
||||
|
||||
stock_release = release_time(CP.stopAccel)
|
||||
preserved_release = release_time(preserved_hold)
|
||||
self.assertAlmostEqual(preserved_hold, expected_hold_accel(CP))
|
||||
assert stock_release >= 0.45
|
||||
assert preserved_release <= 0.37
|
||||
assert stock_release - preserved_release >= 0.14
|
||||
|
||||
@parameterized.expand(GRADE_HOLD_CASES, names=("grade_accel", "expected_hold"))
|
||||
def test_preserved_hold_adapts_to_grade_without_creep(self, grade_accel, expected_hold):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.3)
|
||||
speed = 0.6
|
||||
actuator_accel = -0.3
|
||||
physical_accel = actuator_accel + grade_accel
|
||||
stopped_frames = 0
|
||||
max_post_stop_speed = 0.0
|
||||
|
||||
for _ in range(round(16.0 / DT_CTRL)):
|
||||
standstill = bool(speed <= 1e-6)
|
||||
measured_accel = max(physical_accel, 0.0) if standstill else physical_accel
|
||||
output = control.update(True, make_car_state(speed, measured_accel, standstill), -0.1, True, (-3.5, 2.0))
|
||||
actuator_accel += DT_CTRL / 0.25 * (output - actuator_accel)
|
||||
physical_accel = actuator_accel + grade_accel
|
||||
speed = max(0.0, speed + physical_accel * DT_CTRL) if speed > 0.0 or physical_accel > 0.0 else 0.0
|
||||
|
||||
if stopped_frames:
|
||||
max_post_stop_speed = max(max_post_stop_speed, speed)
|
||||
stopped_frames += 1
|
||||
elif speed == 0.0:
|
||||
stopped_frames = 1
|
||||
if stopped_frames >= round(8.0 / DT_CTRL):
|
||||
break
|
||||
|
||||
assert stopped_frames >= round(8.0 / DT_CTRL)
|
||||
assert max_post_stop_speed == 0.0
|
||||
self.assertAlmostEqual(output, expected_hold, delta=0.03)
|
||||
|
||||
@parameterized.expand(SETTLE_VEHICLES, names=("candidate",))
|
||||
def test_final_stop_builds_brake_smoothly_while_vehicle_settles(self, candidate):
|
||||
_, control = make_control(candidate)
|
||||
control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
|
||||
outputs = [control.update(True, make_car_state(0.0006, a_ego, standstill=True), -0.032, True, (-3.5, 2.0)) for a_ego in (-1.098, -0.950, -0.609, -0.286)]
|
||||
changes = -np.diff([-0.33, *outputs])
|
||||
assert np.all(changes > 0.0)
|
||||
assert np.all(np.diff(changes) > 0.0)
|
||||
assert changes[-1] < 0.001
|
||||
|
||||
@parameterized.expand((-0.09, 0.0, 0.1), names=("a_ego",))
|
||||
def test_settled_vehicle_uses_the_stock_hold_ramp(self, a_ego):
|
||||
CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
output = control.update(True, make_car_state(0.0, a_ego, standstill=True), -0.1, True, (-3.5, 2.0))
|
||||
self.assertAlmostEqual(output, stock_stopping_output(-0.33, CP.stopAccel))
|
||||
|
||||
@parameterized.expand(SETTLE_VEHICLES, names=("candidate",))
|
||||
def test_direct_terminal_entry_builds_brake_smoothly(self, candidate):
|
||||
_, control = make_control(candidate)
|
||||
CS = make_car_state(0.0006, -0.3, standstill=True)
|
||||
outputs = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(4)]
|
||||
|
||||
rates = -np.diff([-0.33, *outputs]) / DT_CTRL
|
||||
np.testing.assert_allclose(rates, [(frame / STOPPING_SETTLE_FRAMES) ** 2 for frame in range(1, 5)], rtol=1e-6, atol=1e-12)
|
||||
|
||||
def test_direct_terminal_entry_keeps_urgent_stock_braking(self):
|
||||
CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
output = control.update(True, make_car_state(0.0006, -0.3, standstill=True), -1.0, True, (-3.5, 2.0))
|
||||
|
||||
self.assertAlmostEqual(output, stock_stopping_output(-0.33, CP.stopAccel))
|
||||
|
||||
@parameterized.expand((0.0, -0.05), names=("initial_accel",))
|
||||
def test_direct_terminal_entry_first_builds_meaningful_brake(self, initial_accel):
|
||||
CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
|
||||
output = control.update(True, make_car_state(0.0006, -0.3, standstill=True), 0.0, True, (-3.5, 2.0))
|
||||
|
||||
self.assertAlmostEqual(output, stock_stopping_output(initial_accel, CP.stopAccel))
|
||||
|
||||
@parameterized.expand(SETTLE_VEHICLES, names=("candidate",))
|
||||
def test_final_settling_ramp_is_bounded(self, candidate):
|
||||
_, control = make_control(candidate)
|
||||
control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
|
||||
CS = make_car_state(0.0, -0.3, standstill=True)
|
||||
outputs = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(STOPPING_SETTLE_FRAMES + 1)]
|
||||
|
||||
rates = -np.diff([-0.33, *outputs]) / DT_CTRL
|
||||
expected = [(frame / STOPPING_SETTLE_FRAMES) ** 2 for frame in range(1, STOPPING_SETTLE_FRAMES + 1)] + [1.0]
|
||||
np.testing.assert_allclose(rates, expected, rtol=1e-6, atol=1e-12)
|
||||
|
||||
@parameterized.expand(((0.6, -0.1, False), (0.0, 0.0, True)), names=("v_ego", "a_ego", "standstill"))
|
||||
def test_stopping_never_releases_a_stronger_command(self, v_ego, a_ego, standstill):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -3.0)
|
||||
output = control.update(True, make_car_state(v_ego, a_ego, standstill), 0.0, True, (-3.5, 2.0))
|
||||
self.assertAlmostEqual(output, -3.0)
|
||||
|
||||
def test_reported_standstill_while_moving_can_hold_the_brake(self):
|
||||
_, control = make_control(GM.CHEVROLET_BOLT_EUV)
|
||||
control.long_control_state = LongCtrlState.off
|
||||
output = control.update(True, make_car_state(0.3, -0.3, standstill=True), -0.1, False, (-3.5, 2.0))
|
||||
self.assertAlmostEqual(output, -0.33)
|
||||
|
||||
@parameterized.expand((-0.1, 0.09), names=("a_target",))
|
||||
def test_stopping_removes_positive_acceleration_immediately(self, a_target):
|
||||
_, control = make_control(HYUNDAI.HYUNDAI_SONATA, 0.2)
|
||||
output = control.update(True, make_car_state(0.2, -0.2), a_target, True, (-3.5, 2.0))
|
||||
self.assertAlmostEqual(output, -DT_CTRL)
|
||||
|
||||
def test_rollback_uses_the_stock_ramp(self):
|
||||
CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
output = control.update(True, make_car_state(-0.1, 0.1), -0.1, True, (-3.5, 2.0))
|
||||
self.assertAlmostEqual(output, stock_stopping_output(-0.33, CP.stopAccel))
|
||||
|
||||
def test_rollback_after_settling_arms_uses_the_stock_ramp(self):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
|
||||
control.update(True, make_car_state(0.01, -0.3), -0.1, True, (-3.5, 2.0))
|
||||
previous = control.last_output_accel
|
||||
output = control.update(True, make_car_state(-0.04, -0.3), -0.1, True, (-3.5, 2.0))
|
||||
|
||||
self.assertAlmostEqual(output, previous - DT_CTRL)
|
||||
|
||||
def test_small_velocity_noise_does_not_trigger_the_stock_rate(self):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
|
||||
output = control.update(True, make_car_state(-0.04, -0.3, standstill=True, v_ego_raw=0.0), -0.1, True, (-3.5, 2.0))
|
||||
assert -0.331 < output < -0.33
|
||||
|
||||
def test_terminal_speed_chatter_cannot_extend_settling_ramp(self):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
|
||||
outputs = [
|
||||
control.update(True, make_car_state(0.049 if frame % 2 == 0 else 0.051, -0.3, v_ego_raw=0.0), -0.1, True, (-3.5, 2.0))
|
||||
for frame in range(STOPPING_SETTLE_FRAMES + 2)
|
||||
]
|
||||
|
||||
rates = -np.diff([-0.33, *outputs]) / DT_CTRL
|
||||
np.testing.assert_allclose(
|
||||
rates[:STOPPING_SETTLE_FRAMES], [(frame / STOPPING_SETTLE_FRAMES) ** 2 for frame in range(1, STOPPING_SETTLE_FRAMES + 1)], rtol=1e-6, atol=1e-12
|
||||
)
|
||||
np.testing.assert_allclose(rates[-2:], [1.0, 1.0], rtol=1e-6, atol=1e-12)
|
||||
|
||||
def test_terminal_speed_plateau_cannot_extend_settling_ramp(self):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
|
||||
CS = make_car_state(0.03, -0.3, v_ego_raw=0.0)
|
||||
outputs = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(STOPPING_SETTLE_FRAMES + 1)]
|
||||
|
||||
rates = -np.diff([-0.33, *outputs]) / DT_CTRL
|
||||
np.testing.assert_allclose(rates[-2:], [1.0, 1.0], rtol=1e-6, atol=1e-12)
|
||||
|
||||
def test_interrupted_stop_cannot_reuse_settling_hold(self):
|
||||
CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
|
||||
control.update(False, make_car_state(0.0, 0.0, standstill=True), 0.0, False, (-3.5, 2.0))
|
||||
output = control.update(True, make_car_state(0.0, -0.3, standstill=True), -0.1, True, (-3.5, 2.0))
|
||||
|
||||
self.assertAlmostEqual(output, stock_stopping_output(0.0, CP.stopAccel))
|
||||
|
||||
def test_departure_uses_the_stock_pid_path(self):
|
||||
_, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
|
||||
control.long_control_state = LongCtrlState.stopping
|
||||
output = control.update(True, make_car_state(0.0), 0.6, False, (-3.5, 2.0))
|
||||
assert control.long_control_state == LongCtrlState.pid
|
||||
assert output > 0.0
|
||||
|
||||
def test_planner_mpc_and_longcontrol_complete_a_smooth_stop(self):
|
||||
plant = PlantSP(
|
||||
lead_relevancy=True,
|
||||
speed=0.6,
|
||||
distance_lead=3.6,
|
||||
run_long_control=True,
|
||||
actuator_model=PRIUS_TSS2_ROUTE_MODEL,
|
||||
)
|
||||
plant.planner.accel_controller._enabled = True
|
||||
plant.planner.dec._enabled = False
|
||||
commands = []
|
||||
speeds = []
|
||||
states = []
|
||||
solver_statuses = []
|
||||
|
||||
with (
|
||||
mock.patch.object(plant.planner.accel_controller, "update", return_value=None),
|
||||
mock.patch.object(plant.planner.dec, "_read_params", return_value=None),
|
||||
):
|
||||
while plant.current_time < 5.0:
|
||||
result = plant.step(v_lead=0.0, v_cruise=8.0)
|
||||
commands.append(result["actuator_command"])
|
||||
speeds.append(result["speed"])
|
||||
states.append(result["long_control_state"])
|
||||
solver_statuses.append(plant.planner.mpc.solution_status)
|
||||
|
||||
stopping = states.index(LongCtrlState.stopping)
|
||||
moving_stop_commands = [
|
||||
command for command, state, speed in zip(commands, states, speeds, strict=True) if state == LongCtrlState.stopping and speed > STOPPING_SPEED_TOLERANCE
|
||||
]
|
||||
assert all(current <= previous + 1e-9 for previous, current in zip(commands[stopping:-1], commands[stopping + 1 :], strict=True))
|
||||
assert len(moving_stop_commands) > 1 and max(moving_stop_commands) - min(moving_stop_commands) < 1e-9
|
||||
assert plant.speed == 0.0 and plant.distance < 1.0
|
||||
assert plant.distance_lead - plant.distance > 3.0
|
||||
assert all(status == 0 for status in solver_statuses)
|
||||
Binary file not shown.
@@ -1,394 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from collections import deque
|
||||
from collections.abc import Callable
|
||||
from dataclasses import dataclass
|
||||
import math
|
||||
import time
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.cereal import log, messaging
|
||||
from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
|
||||
from openpilot.common.realtime import DT_CTRL, DT_MDL, Ratekeeper
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
|
||||
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
|
||||
from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant, PlannerSM
|
||||
|
||||
|
||||
LeadObservation = dict[str, Any]
|
||||
LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None]
|
||||
ModelActionFn = Callable[[float, float, float], tuple[float, bool]]
|
||||
EgoObservationFn = Callable[[float, float, float], tuple[float, float]]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ActuatorModel:
|
||||
planner_delay: float
|
||||
transport_delay: float
|
||||
actuator_lag: float
|
||||
command_rate_limit: float
|
||||
stopping_acceleration: float
|
||||
standstill_breakaway_acceleration: float
|
||||
standstill_breakaway_time: float
|
||||
|
||||
def __post_init__(self):
|
||||
nonnegative_fields = {
|
||||
"planner_delay": self.planner_delay,
|
||||
"transport_delay": self.transport_delay,
|
||||
"actuator_lag": self.actuator_lag,
|
||||
"standstill_breakaway_acceleration": self.standstill_breakaway_acceleration,
|
||||
"standstill_breakaway_time": self.standstill_breakaway_time,
|
||||
}
|
||||
if any(not math.isfinite(value) or value < 0.0 for value in nonnegative_fields.values()):
|
||||
raise ValueError(f"ActuatorModel fields must be finite and non-negative: {nonnegative_fields}")
|
||||
if not math.isfinite(self.command_rate_limit) or self.command_rate_limit <= 0.0:
|
||||
raise ValueError("command_rate_limit must be finite and positive")
|
||||
if not math.isfinite(self.stopping_acceleration) or self.stopping_acceleration > 0.0:
|
||||
raise ValueError("stopping_acceleration must be finite and non-positive")
|
||||
|
||||
|
||||
# Conservative Prius TSS2 actuator model.
|
||||
PRIUS_TSS2_ROUTE_MODEL = ActuatorModel(
|
||||
planner_delay=0.05,
|
||||
transport_delay=0.0,
|
||||
actuator_lag=0.20,
|
||||
command_rate_limit=4.0,
|
||||
stopping_acceleration=-2.0,
|
||||
standstill_breakaway_acceleration=1.0,
|
||||
standstill_breakaway_time=0.05,
|
||||
)
|
||||
|
||||
|
||||
class PlantSP(Plant):
|
||||
"""Closed-loop plant with configurable observations and actuator response."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
lead_relevancy=False,
|
||||
speed=0.0,
|
||||
distance_lead=2.0,
|
||||
enabled=True,
|
||||
only_lead2=False,
|
||||
only_radar=False,
|
||||
e2e=False,
|
||||
personality=0,
|
||||
force_decel=False,
|
||||
lead_observation_fn: LeadObservationFn | None = None,
|
||||
model_action_fn: ModelActionFn | None = None,
|
||||
ego_observation_fn: EgoObservationFn | None = None,
|
||||
actuator_delay: float | None = None,
|
||||
actuator_lag: float = 0.0,
|
||||
actuator_model: ActuatorModel | None = None,
|
||||
run_long_control: bool = False,
|
||||
):
|
||||
if actuator_delay is not None and (not math.isfinite(actuator_delay) or actuator_delay < 0.0):
|
||||
raise ValueError("actuator_delay must be finite and non-negative")
|
||||
if not math.isfinite(actuator_lag) or actuator_lag < 0.0:
|
||||
raise ValueError("actuator_lag must be finite and non-negative")
|
||||
|
||||
self.rate = 1.0 / DT_MDL
|
||||
|
||||
if not Plant.messaging_initialized:
|
||||
Plant.radar = messaging.pub_sock('radarState')
|
||||
Plant.controls_state = messaging.pub_sock('controlsState')
|
||||
Plant.selfdrive_state = messaging.pub_sock('selfdriveState')
|
||||
Plant.car_state = messaging.pub_sock('carState')
|
||||
Plant.plan = messaging.sub_sock('longitudinalPlan')
|
||||
Plant.messaging_initialized = True
|
||||
|
||||
self.v_lead_prev = 0.0
|
||||
|
||||
self.distance = 0.0
|
||||
self.speed = speed
|
||||
self.should_stop = False
|
||||
self.acceleration = 0.0
|
||||
self.a_target = 0.0
|
||||
self.actuator_command = 0.0
|
||||
self.applied_actuator_command = 0.0
|
||||
self.breakaway_confirmed = False
|
||||
self._breakaway_timer = 0.0
|
||||
|
||||
# lead car
|
||||
self.lead_relevancy = lead_relevancy
|
||||
self.distance_lead = distance_lead
|
||||
self.enabled = enabled
|
||||
self.only_lead2 = only_lead2
|
||||
self.only_radar = only_radar
|
||||
self.e2e = e2e
|
||||
self.personality = personality
|
||||
self.force_decel = force_decel
|
||||
self.lead_observation_fn = lead_observation_fn
|
||||
self.model_action_fn = model_action_fn
|
||||
self.ego_observation_fn = ego_observation_fn
|
||||
self.actuator_model = actuator_model
|
||||
self.actuator_delay = actuator_model.planner_delay if actuator_model is not None else actuator_delay
|
||||
self.transport_delay = actuator_model.transport_delay if actuator_model is not None else actuator_delay
|
||||
self.actuator_lag = actuator_model.actuator_lag if actuator_model is not None else actuator_lag
|
||||
self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, ego_observation_fn is not None,
|
||||
actuator_delay is not None, actuator_lag > 0.0, actuator_model is not None, run_long_control))
|
||||
|
||||
self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
|
||||
self.ts = 1.0 / self.rate
|
||||
time.sleep(0.1)
|
||||
self.sm = messaging.SubMaster(['longitudinalPlan'])
|
||||
|
||||
from opendbc.car.honda.values import CAR
|
||||
from opendbc.car.honda.interface import CarInterface
|
||||
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
if self.actuator_delay is not None:
|
||||
CP.longitudinalActuatorDelay = self.actuator_delay
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, CAR.HONDA_CIVIC)
|
||||
self.planner = LongitudinalPlanner(CP, CP_SP, init_v=self.speed)
|
||||
self.long_control = LongControl(CP, CP_SP) if run_long_control else None
|
||||
|
||||
if self.actuator_model is not None and self.speed >= 0.01:
|
||||
self.breakaway_confirmed = True
|
||||
self.integration_dt = DT_CTRL if run_long_control else self.ts
|
||||
delay_steps = 0 if self.transport_delay is None else round(self.transport_delay / self.integration_dt)
|
||||
self._actuator_delay_queue = deque([self.acceleration] * delay_steps)
|
||||
|
||||
@staticmethod
|
||||
def _lead_message(observation: LeadObservation):
|
||||
lead = log.RadarState.LeadData.new_message()
|
||||
for field, value in observation.items():
|
||||
setattr(lead, field, value)
|
||||
return lead
|
||||
|
||||
def _observe_lead(self, lead_name: str, truth: LeadObservation, present_by_default: bool) -> LeadObservation | None:
|
||||
if self.lead_observation_fn is None:
|
||||
return dict(truth) if present_by_default else None
|
||||
|
||||
observed = self.lead_observation_fn(self.current_time, lead_name, dict(truth))
|
||||
if observed is None:
|
||||
return None
|
||||
|
||||
complete_observation = dict(truth)
|
||||
complete_observation.update(observed)
|
||||
return complete_observation
|
||||
|
||||
def _update_actuator(self, command: float) -> tuple[float, float]:
|
||||
if self._actuator_delay_queue:
|
||||
self._actuator_delay_queue.append(command)
|
||||
delayed_command = self._actuator_delay_queue.popleft()
|
||||
else:
|
||||
delayed_command = command
|
||||
|
||||
if self.actuator_model is not None:
|
||||
max_command_delta = self.actuator_model.command_rate_limit * self.integration_dt
|
||||
self.applied_actuator_command = float(np.clip(delayed_command,
|
||||
self.applied_actuator_command - max_command_delta,
|
||||
self.applied_actuator_command + max_command_delta))
|
||||
|
||||
if self.speed < 0.01:
|
||||
if self.applied_actuator_command <= 0.0:
|
||||
self.breakaway_confirmed = False
|
||||
self._breakaway_timer = 0.0
|
||||
elif not self.breakaway_confirmed:
|
||||
breakaway_ready = self.applied_actuator_command + 1e-9 >= self.actuator_model.standstill_breakaway_acceleration
|
||||
if breakaway_ready:
|
||||
self._breakaway_timer += self.integration_dt
|
||||
else:
|
||||
self._breakaway_timer = 0.0
|
||||
|
||||
self.breakaway_confirmed = breakaway_ready and self._breakaway_timer + 1e-9 >= self.actuator_model.standstill_breakaway_time
|
||||
if not self.breakaway_confirmed:
|
||||
self.acceleration = 0.0
|
||||
return delayed_command, self.acceleration
|
||||
else:
|
||||
self.breakaway_confirmed = True
|
||||
|
||||
response_command = self.applied_actuator_command
|
||||
else:
|
||||
self.applied_actuator_command = delayed_command
|
||||
response_command = delayed_command
|
||||
|
||||
if self.actuator_lag > 0.0:
|
||||
alpha = 1.0 - math.exp(-self.integration_dt / self.actuator_lag)
|
||||
self.acceleration += alpha * (response_command - self.acceleration)
|
||||
else:
|
||||
self.acceleration = response_command
|
||||
return delayed_command, self.acceleration
|
||||
|
||||
def _integrate_ego(self, dt: float, stop_at_standstill: bool = False) -> None:
|
||||
self.speed += self.acceleration * dt
|
||||
if self.speed <= 0.0 or stop_at_standstill and self.speed < 0.01 and self.actuator_command <= 0.0:
|
||||
self.speed = self.acceleration = 0.0
|
||||
self.distance += self.speed * dt
|
||||
|
||||
def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50.0, pitch=0.0, prob_throttle=1.0):
|
||||
# ******** publish a fake model going straight and fake calibration ********
|
||||
# note that this is worst case for MPC, since model will delay long mpc by one time step
|
||||
radar = messaging.new_message('radarState')
|
||||
control = messaging.new_message('controlsState')
|
||||
ss = messaging.new_message('selfdriveState')
|
||||
car_state = messaging.new_message('carState')
|
||||
vehicle_parameters = messaging.new_message('vehicleParameters')
|
||||
car_control = messaging.new_message('carControl')
|
||||
model = messaging.new_message('modelV2')
|
||||
car_state_sp = messaging.new_message('carStateSP')
|
||||
live_map_data_sp = messaging.new_message('liveMapDataSP')
|
||||
gps_data = messaging.new_message('gpsLocation')
|
||||
a_lead = (v_lead - self.v_lead_prev) / self.ts
|
||||
self.v_lead_prev = v_lead
|
||||
|
||||
if self.lead_relevancy:
|
||||
d_rel = np.maximum(0.0, self.distance_lead - self.distance)
|
||||
v_rel = v_lead - self.speed
|
||||
if self.only_radar:
|
||||
status = True
|
||||
elif prob_lead > 0.5:
|
||||
status = True
|
||||
else:
|
||||
status = False
|
||||
else:
|
||||
d_rel = 200.0
|
||||
v_rel = 0.0
|
||||
prob_lead = 0.0
|
||||
status = False
|
||||
|
||||
truth_lead: LeadObservation = {
|
||||
"dRel": float(d_rel),
|
||||
"yRel": 0.0,
|
||||
"vRel": float(v_rel),
|
||||
"vLead": float(v_lead),
|
||||
"vLeadK": float(v_lead),
|
||||
"aLeadK": float(a_lead),
|
||||
"present": bool(status),
|
||||
# TODO use real radard logic for this
|
||||
"aLeadTau": float(_LEAD_ACCEL_TAU),
|
||||
"modelProb": float(prob_lead),
|
||||
"radar": bool(self.only_radar),
|
||||
"radarTrackId": -1,
|
||||
}
|
||||
lead_one_observation = self._observe_lead("leadOne", truth_lead, not self.only_lead2)
|
||||
lead_two_observation = self._observe_lead("leadTwo", truth_lead, True)
|
||||
if lead_one_observation is not None:
|
||||
radar.radarState.leadOne = self._lead_message(lead_one_observation)
|
||||
if lead_two_observation is not None:
|
||||
radar.radarState.leadTwo = self._lead_message(lead_two_observation)
|
||||
|
||||
# Simulate model predicting slightly faster speed
|
||||
# this is to ensure lead policy is effective when model
|
||||
# does not predict slowdown in e2e mode
|
||||
position = log.XYZTData.new_message()
|
||||
position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
|
||||
model.modelV2.position = position
|
||||
if self.model_action_fn is None:
|
||||
model_acceleration, model_should_stop = self.acceleration + 0.5, False
|
||||
else:
|
||||
model_acceleration, model_should_stop = self.model_action_fn(self.current_time, self.speed, self.acceleration)
|
||||
model.modelV2.action.desiredAcceleration = float(model_acceleration)
|
||||
model.modelV2.action.shouldStop = bool(model_should_stop)
|
||||
velocity = log.XYZTData.new_message()
|
||||
velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
|
||||
velocity.x[0] = float(self.speed) # always start at current speed
|
||||
model.modelV2.velocity = velocity
|
||||
acceleration = log.XYZTData.new_message()
|
||||
acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
|
||||
model.modelV2.acceleration = acceleration
|
||||
model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)]
|
||||
|
||||
control.controlsState.longControlState = self.long_control.long_control_state if self.long_control is not None else (
|
||||
LongCtrlState.pid if self.enabled else LongCtrlState.off)
|
||||
ss.selfdriveState.experimentalMode = self.e2e
|
||||
ss.selfdriveState.personality = self.personality
|
||||
control.controlsState.forceDecel = self.force_decel
|
||||
true_v_ego = self.speed
|
||||
true_a_ego = self.acceleration
|
||||
published_v_ego = true_v_ego
|
||||
published_a_ego = true_a_ego if self.publish_realized_a_ego else 0.0
|
||||
if self.ego_observation_fn is not None:
|
||||
published_v_ego, published_a_ego = self.ego_observation_fn(self.current_time, true_v_ego, true_a_ego)
|
||||
car_state.carState.vEgo = float(published_v_ego)
|
||||
car_state.carState.aEgo = float(published_a_ego)
|
||||
car_state.carState.standstill = bool(self.speed < 0.01)
|
||||
car_state.carState.vCruise = float(v_cruise * 3.6)
|
||||
car_control.carControl.orientationNED = [0.0, float(pitch), 0.0]
|
||||
|
||||
# ******** get controlsState messages for plotting ***
|
||||
sm = PlannerSM(self.rk.frame, {
|
||||
'radarState': radar.radarState,
|
||||
'carState': car_state.carState,
|
||||
'carControl': car_control.carControl,
|
||||
'controlsState': control.controlsState,
|
||||
'selfdriveState': ss.selfdriveState,
|
||||
'vehicleParameters': vehicle_parameters.vehicleParameters,
|
||||
'modelV2': model.modelV2,
|
||||
'carStateSP': car_state_sp.carStateSP,
|
||||
'liveMapDataSP': live_map_data_sp.liveMapDataSP,
|
||||
'gpsLocation': gps_data.gpsLocation,
|
||||
})
|
||||
self.planner.update(sm)
|
||||
self.a_target = self.planner.output_a_target
|
||||
if self.long_control is None:
|
||||
self.actuator_command = self.a_target
|
||||
if self.planner.output_should_stop:
|
||||
stopping_acceleration = -0.5 if self.actuator_model is None else self.actuator_model.stopping_acceleration
|
||||
self.actuator_command = min(stopping_acceleration, self.actuator_command)
|
||||
self._update_actuator(self.actuator_command)
|
||||
self._integrate_ego(self.ts)
|
||||
else:
|
||||
for _ in range(round(self.ts / DT_CTRL)):
|
||||
car_state.carState.vEgo = self.speed
|
||||
car_state.carState.aEgo = self.acceleration
|
||||
car_state.carState.standstill = self.speed < 0.01
|
||||
self.actuator_command = self.long_control.update(
|
||||
self.enabled, car_state.carState, self.a_target, self.planner.output_should_stop, (ACCEL_MIN, ACCEL_MAX),
|
||||
)
|
||||
self._update_actuator(self.actuator_command)
|
||||
self._integrate_ego(DT_CTRL, stop_at_standstill=True)
|
||||
self.should_stop = self.planner.output_should_stop
|
||||
fcw = self.planner.fcw
|
||||
self.distance_lead = self.distance_lead + v_lead * self.ts
|
||||
|
||||
# *** radar model ***
|
||||
if self.lead_relevancy:
|
||||
d_rel = np.maximum(0.0, self.distance_lead - self.distance)
|
||||
v_rel = v_lead - self.speed
|
||||
else:
|
||||
d_rel = 200.0
|
||||
v_rel = 0.0
|
||||
|
||||
# print at 5hz
|
||||
# if (self.rk.frame % (self.rate // 5)) == 0:
|
||||
# print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s"
|
||||
# % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel))
|
||||
|
||||
# ******** update prevs ********
|
||||
self.rk.monitor_time()
|
||||
|
||||
return {
|
||||
"distance": self.distance,
|
||||
"speed": self.speed,
|
||||
"acceleration": self.acceleration,
|
||||
"realized_acceleration": self.acceleration,
|
||||
"a_target": self.a_target,
|
||||
"actuator_command": self.actuator_command,
|
||||
"published_a_ego": published_a_ego,
|
||||
"published_v_ego": published_v_ego,
|
||||
"should_stop": self.should_stop,
|
||||
"long_control_state": (int(self.long_control.long_control_state) if self.long_control is not None
|
||||
else control.controlsState.longControlState.raw),
|
||||
"distance_lead": self.distance_lead,
|
||||
"fcw": fcw,
|
||||
"mpc_source": self.planner.mpc.source,
|
||||
"dec_mode": self.planner.dec.mode(),
|
||||
"controller_active": self.planner.accel_controller_active,
|
||||
"model_action": {
|
||||
"desiredAcceleration": float(model_acceleration),
|
||||
"shouldStop": bool(model_should_stop),
|
||||
},
|
||||
"truth_lead": dict(truth_lead),
|
||||
"lead_one_observation": None if lead_one_observation is None else dict(lead_one_observation),
|
||||
"lead_two_observation": None if lead_two_observation is None else dict(lead_two_observation),
|
||||
}
|
||||
@@ -1,164 +0,0 @@
|
||||
from collections.abc import Callable
|
||||
import math
|
||||
from typing import cast
|
||||
|
||||
from openpilot.common.parameterized import parameterized
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
|
||||
from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP
|
||||
|
||||
STOCK_STEP_KEYS = ("distance", "speed", "acceleration", "should_stop", "distance_lead", "fcw")
|
||||
|
||||
|
||||
def departing_lead(current_time: float) -> float:
|
||||
return 0.0 if current_time < 1.0 else min(2.0, 2.0 * (current_time - 1.0))
|
||||
|
||||
|
||||
def stopped_lead(_current_time: float) -> float:
|
||||
return 0.0
|
||||
|
||||
|
||||
PARITY_SCENARIOS = {
|
||||
"approach_stopped_lead": {"lead_relevancy": True, "speed": 15.0, "distance_lead": 60.0, "v_cruise": 20.0, "v_lead": stopped_lead, "steps": 80},
|
||||
"stop_then_depart": {"lead_relevancy": True, "speed": 0.0, "distance_lead": 6.0, "v_cruise": 8.0, "v_lead": departing_lead, "steps": 120},
|
||||
}
|
||||
|
||||
|
||||
def _drive(cls, *, v_cruise: float, v_lead: Callable[[float], float], steps: int, **kwargs):
|
||||
plant = cls(**kwargs)
|
||||
plant.v_lead_prev = v_lead(0.0)
|
||||
solver_failures = 0
|
||||
original_reset = plant.planner.mpc.reset
|
||||
|
||||
def counting_reset(*args, **kw):
|
||||
nonlocal solver_failures
|
||||
if plant.planner.mpc.solution_status != 0:
|
||||
solver_failures += 1
|
||||
return original_reset(*args, **kw)
|
||||
|
||||
plant.planner.mpc.reset = counting_reset
|
||||
results = []
|
||||
for _ in range(steps):
|
||||
lead_speed = v_lead(plant.current_time)
|
||||
result = plant.step(v_lead=lead_speed, v_cruise=v_cruise)
|
||||
results.append((result, plant.planner.mpc.source, plant.planner.output_a_target))
|
||||
return results, solver_failures
|
||||
|
||||
|
||||
class TestPlantSP(OpenpilotTestCase):
|
||||
@parameterized.expand(PARITY_SCENARIOS, names=("scenario",), ids=lambda scenario: scenario)
|
||||
def test_plant_sp_matches_stock_plant_on_shared_kwargs(self, scenario: str):
|
||||
kwargs = dict(PARITY_SCENARIOS[scenario])
|
||||
v_cruise = cast(float, kwargs.pop("v_cruise"))
|
||||
v_lead = cast(Callable[[float], float], kwargs.pop("v_lead"))
|
||||
steps = cast(int, kwargs.pop("steps"))
|
||||
|
||||
stock_results, stock_failures = _drive(Plant, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
|
||||
sp_results, sp_failures = _drive(PlantSP, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
|
||||
|
||||
assert stock_failures == 0, f"stock Plant solver failed {stock_failures} times in {scenario!r}"
|
||||
assert sp_failures == 0, f"PlantSP solver failed {sp_failures} times in {scenario!r}"
|
||||
|
||||
for frame, ((stock_result, stock_source, stock_a_target), (sp_result, sp_source, sp_a_target)) in enumerate(
|
||||
zip(stock_results, sp_results, strict=True),
|
||||
):
|
||||
for key in STOCK_STEP_KEYS:
|
||||
if isinstance(stock_result[key], float):
|
||||
self.assertAlmostEqual(sp_result[key], stock_result[key], msg=f"{scenario} frame {frame} key {key}")
|
||||
else:
|
||||
assert sp_result[key] == stock_result[key], f"{scenario} frame {frame} key {key}"
|
||||
assert sp_source == stock_source, f"{scenario} frame {frame} mpc.source"
|
||||
self.assertAlmostEqual(sp_a_target, stock_a_target, msg=f"{scenario} frame {frame} output_a_target")
|
||||
|
||||
if scenario == "stop_then_depart":
|
||||
departure_frame = round(1.0 / DT_MDL)
|
||||
for results in (stock_results, sp_results):
|
||||
assert all(result["speed"] < 0.01 for result, _, _ in results[:departure_frame])
|
||||
assert results[departure_frame - 1][0]["should_stop"]
|
||||
assert any(not result["should_stop"] for result, _, _ in results[departure_frame:])
|
||||
assert any(result["speed"] > 0.05 for result, _, _ in results[departure_frame:])
|
||||
stock_release = next(frame for frame, (result, _, _) in enumerate(stock_results)
|
||||
if frame >= departure_frame and not result["should_stop"])
|
||||
sp_release = next(frame for frame, (result, _, _) in enumerate(sp_results)
|
||||
if frame >= departure_frame and not result["should_stop"])
|
||||
stock_motion = next(frame for frame, (result, _, _) in enumerate(stock_results)
|
||||
if frame >= departure_frame and result["speed"] > 0.05)
|
||||
sp_motion = next(frame for frame, (result, _, _) in enumerate(sp_results)
|
||||
if frame >= departure_frame and result["speed"] > 0.05)
|
||||
assert sp_release == stock_release
|
||||
assert sp_motion == stock_motion
|
||||
|
||||
def test_full_lead_observation_is_independent_from_truth(self):
|
||||
callback_inputs = []
|
||||
|
||||
def observe_lead(current_time, lead_name, truth):
|
||||
callback_inputs.append((current_time, lead_name, truth))
|
||||
if lead_name == "leadOne":
|
||||
return {
|
||||
"dRel": 12.5,
|
||||
"vRel": -4.0,
|
||||
"vLead": 6.0,
|
||||
"vLeadK": 5.5,
|
||||
"aLeadK": -1.25,
|
||||
"aLeadTau": 0.7,
|
||||
"present": True,
|
||||
"modelProb": 0.9,
|
||||
"radarTrackId": 42,
|
||||
}
|
||||
return None
|
||||
|
||||
plant = PlantSP(lead_relevancy=True, speed=10.0, distance_lead=50.0, lead_observation_fn=observe_lead)
|
||||
result = plant.step(v_lead=8.0)
|
||||
|
||||
assert [entry[1] for entry in callback_inputs] == ["leadOne", "leadTwo"]
|
||||
self.assertAlmostEqual(callback_inputs[0][2]["dRel"], 50.0)
|
||||
self.assertAlmostEqual(result["truth_lead"]["dRel"], 50.0)
|
||||
self.assertAlmostEqual(result["lead_one_observation"]["dRel"], 12.5)
|
||||
assert result["lead_one_observation"]["radarTrackId"] == 42
|
||||
assert result["lead_two_observation"] is None
|
||||
self.assertAlmostEqual(result["distance_lead"], 50.0 + 8.0 * DT_MDL)
|
||||
|
||||
def test_model_action_realized_acceleration_and_source_logging(self):
|
||||
def model_action(current_time, v_ego, a_ego):
|
||||
return -1.25, True
|
||||
|
||||
plant = PlantSP(speed=10.0, e2e=True, force_decel=True, model_action_fn=model_action, actuator_lag=0.5)
|
||||
first = plant.step()
|
||||
second = plant.step()
|
||||
|
||||
assert first["model_action"] == {"desiredAcceleration": -1.25, "shouldStop": True}
|
||||
self.assertAlmostEqual(first["published_a_ego"], 0.0)
|
||||
self.assertAlmostEqual(second["published_a_ego"], first["realized_acceleration"])
|
||||
assert first["acceleration"] == first["realized_acceleration"]
|
||||
assert abs(first["realized_acceleration"]) < abs(first["actuator_command"])
|
||||
assert first["mpc_source"] is not None
|
||||
assert first["dec_mode"] in ("acc", "blended")
|
||||
assert "controller_active" in first
|
||||
assert first["lead_one_observation"] is not None
|
||||
assert first["truth_lead"] == first["lead_one_observation"]
|
||||
|
||||
def test_default_model_action_matches_stock_plant(self):
|
||||
result = PlantSP(speed=10.0).step()
|
||||
|
||||
self.assertAlmostEqual(result["model_action"]["desiredAcceleration"], 0.5)
|
||||
assert not result["model_action"]["shouldStop"]
|
||||
|
||||
def test_configurable_transport_delay_and_first_order_lag(self):
|
||||
plant = PlantSP(speed=10.0, actuator_delay=2 * DT_MDL, actuator_lag=0.2)
|
||||
|
||||
self.assertAlmostEqual(plant.planner.CP.longitudinalActuatorDelay, 2 * DT_MDL)
|
||||
delayed_commands = [plant._update_actuator(-1.0) for _ in range(3)]
|
||||
assert [command for command, _ in delayed_commands[:2]] == [0.0, 0.0]
|
||||
|
||||
expected_acceleration = -(1.0 - math.exp(-DT_MDL / 0.2))
|
||||
assert delayed_commands[2][0] == -1.0
|
||||
self.assertAlmostEqual(delayed_commands[2][1], expected_acceleration)
|
||||
|
||||
@parameterized.expand(
|
||||
[(-0.1, 0.0), (float("nan"), 0.0), (float("inf"), 0.0), (None, -0.1), (None, float("nan")), (None, float("inf"))],
|
||||
names=("delay", "lag"),
|
||||
)
|
||||
def test_invalid_actuator_dynamics(self, delay, lag):
|
||||
with self.assertRaises(ValueError):
|
||||
PlantSP(actuator_delay=delay, actuator_lag=lag)
|
||||
@@ -28,7 +28,7 @@ from websocket import (ABNF, WebSocket, WebSocketException, WebSocketTimeoutExce
|
||||
create_connection, WebSocketConnectionClosedException)
|
||||
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.sunnypilot.models.default_model import DEFAULT_MODEL
|
||||
from openpilot.sunnypilot.models.default_model import get_default_model
|
||||
from openpilot.sunnypilot.selfdrive.car.sync_sunnylink_params import update_car_list_param
|
||||
from openpilot.sunnypilot.sunnylink.api import SunnylinkApi
|
||||
from openpilot.sunnypilot.sunnylink.utils import sunnylink_need_register, sunnylink_ready, get_param_as_byte, save_param_from_base64_encoded_string
|
||||
@@ -181,7 +181,7 @@ def getParamsMetadata() -> str:
|
||||
schema = generate_schema()
|
||||
schema["capabilities"] = generate_capabilities()
|
||||
schema["capability_labels"] = CAPABILITY_LABELS
|
||||
schema["default_model"] = DEFAULT_MODEL
|
||||
schema["default_model"] = get_default_model()
|
||||
raw = json.dumps(schema, separators=(",", ":")).encode("utf-8")
|
||||
return base64.b64encode(gzip.compress(raw)).decode("utf-8")
|
||||
except Exception:
|
||||
|
||||
@@ -652,53 +652,6 @@
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "AccelPersonalityEnabled",
|
||||
"widget": "toggle",
|
||||
"title": "Enable Accel Controller",
|
||||
"description": "Sets your preferred acceleration and cruise-deceleration limits by profile. Lead following, braking, and stopping behavior remain independent of this setting.",
|
||||
"visibility": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
}
|
||||
],
|
||||
"enablement": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "AccelPersonality",
|
||||
"widget": "multiple_button",
|
||||
"title": "Acceleration Profile",
|
||||
"description": "Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across profiles.",
|
||||
"options": [
|
||||
{
|
||||
"value": 0,
|
||||
"label": "Eco"
|
||||
},
|
||||
{
|
||||
"value": 1,
|
||||
"label": "Normal"
|
||||
},
|
||||
{
|
||||
"value": 2,
|
||||
"label": "Sport"
|
||||
}
|
||||
],
|
||||
"enablement": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "IntelligentCruiseButtonManagement",
|
||||
"widget": "toggle",
|
||||
@@ -2349,50 +2302,6 @@
|
||||
"title": "Toyota / Lexus Settings",
|
||||
"description": "",
|
||||
"items": [
|
||||
{
|
||||
"key": "ToyotaAutoHold",
|
||||
"widget": "toggle",
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "not_engaged"
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "ToyotaEnhancedBsm",
|
||||
"widget": "toggle",
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "Toyota: Prius TSS2 BSM and some tssp",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "not_engaged"
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "ToyotaTSS2Long",
|
||||
"widget": "toggle",
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "Toyota: custom longitudinal for TSS2",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "not_engaged"
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "ToyotaDriveMode",
|
||||
"widget": "toggle",
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "Enable drive mode btn link",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "not_engaged"
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "ToyotaEnforceStockLongitudinal",
|
||||
"widget": "toggle",
|
||||
|
||||
@@ -43,29 +43,6 @@ sections:
|
||||
label: Relaxed
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: AccelPersonalityEnabled
|
||||
widget: toggle
|
||||
title: Enable Accel Controller
|
||||
description: Sets your preferred acceleration and cruise-deceleration limits by profile. Lead following, braking,
|
||||
and stopping behavior remain independent of this setting.
|
||||
visibility:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: AccelPersonality
|
||||
widget: multiple_button
|
||||
title: Acceleration Profile
|
||||
description: Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across
|
||||
profiles.
|
||||
options:
|
||||
- value: 0
|
||||
label: Eco
|
||||
- value: 1
|
||||
label: Normal
|
||||
- value: 2
|
||||
label: Sport
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: IntelligentCruiseButtonManagement
|
||||
widget: toggle
|
||||
title: Intelligent Cruise Button Management (ICBM) (Alpha)
|
||||
|
||||
@@ -82,30 +82,6 @@ sections:
|
||||
title: Toyota / Lexus Settings
|
||||
description: ''
|
||||
items:
|
||||
- key: ToyotaAutoHold
|
||||
widget: toggle
|
||||
needs_onroad_cycle: true
|
||||
title: 'Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS'
|
||||
enablement:
|
||||
- $ref: '#/macros/not_engaged'
|
||||
- key: ToyotaEnhancedBsm
|
||||
widget: toggle
|
||||
needs_onroad_cycle: true
|
||||
title: 'Toyota: Prius TSS2 BSM and some tssp'
|
||||
enablement:
|
||||
- $ref: '#/macros/not_engaged'
|
||||
- key: ToyotaTSS2Long
|
||||
widget: toggle
|
||||
needs_onroad_cycle: true
|
||||
title: 'Toyota: custom longitudinal for TSS2'
|
||||
enablement:
|
||||
- $ref: '#/macros/not_engaged'
|
||||
- key: ToyotaDriveMode
|
||||
widget: toggle
|
||||
needs_onroad_cycle: true
|
||||
title: Enable drive mode btn link
|
||||
enablement:
|
||||
- $ref: '#/macros/not_engaged'
|
||||
- key: ToyotaEnforceStockLongitudinal
|
||||
widget: toggle
|
||||
needs_onroad_cycle: true
|
||||
|
||||
@@ -10,10 +10,9 @@ change and must be intentional. KNOWN_PROTOCOL_VERSIONS pins the set we
|
||||
explicitly support — when the constant is bumped, this list must be edited in
|
||||
the same commit so the bump shows up in code review.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
|
||||
from openpilot.sunnypilot.sunnylink.capabilities import (
|
||||
CAPABILITY_DEFAULTS,
|
||||
CAPABILITY_FIELDS,
|
||||
@@ -21,23 +20,13 @@ from openpilot.sunnypilot.sunnylink.capabilities import (
|
||||
PROTOCOL_VERSION,
|
||||
generate_capabilities,
|
||||
)
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
|
||||
|
||||
KNOWN_PROTOCOL_VERSIONS = (1,)
|
||||
LATEST_KNOWN = max(KNOWN_PROTOCOL_VERSIONS)
|
||||
|
||||
|
||||
class FakeParams:
|
||||
def __init__(self, values=None):
|
||||
self.values = values or {}
|
||||
|
||||
def get(self, key, *args, **kwargs):
|
||||
return self.values.get(key)
|
||||
|
||||
def get_bool(self, key):
|
||||
return bool(self.values.get(key, False))
|
||||
|
||||
|
||||
def caps():
|
||||
return generate_capabilities()
|
||||
|
||||
@@ -63,12 +52,14 @@ class TestProtocolVersion(OpenpilotTestCase):
|
||||
def test_protocol_version_is_known(self):
|
||||
"""Sentinel against accidental bumps. Edit KNOWN_PROTOCOL_VERSIONS if intentional."""
|
||||
assert PROTOCOL_VERSION in KNOWN_PROTOCOL_VERSIONS, (
|
||||
f"PROTOCOL_VERSION={PROTOCOL_VERSION} is not in KNOWN_PROTOCOL_VERSIONS={KNOWN_PROTOCOL_VERSIONS}. "
|
||||
+ "If this bump is intentional, add it to KNOWN_PROTOCOL_VERSIONS."
|
||||
f"PROTOCOL_VERSION={PROTOCOL_VERSION} is not in KNOWN_PROTOCOL_VERSIONS={KNOWN_PROTOCOL_VERSIONS}. " +
|
||||
"If this bump is intentional, add it to KNOWN_PROTOCOL_VERSIONS."
|
||||
)
|
||||
|
||||
def test_protocol_version_matches_latest_known(self):
|
||||
assert PROTOCOL_VERSION == LATEST_KNOWN, "Test invariant: PROTOCOL_VERSION must equal max(KNOWN_PROTOCOL_VERSIONS)."
|
||||
assert PROTOCOL_VERSION == LATEST_KNOWN, (
|
||||
"Test invariant: PROTOCOL_VERSION must equal max(KNOWN_PROTOCOL_VERSIONS)."
|
||||
)
|
||||
|
||||
|
||||
class TestOpaquePerBrandFlags(OpenpilotTestCase):
|
||||
|
||||
@@ -9,7 +9,6 @@ isolates one of the gating bugs that the design-overhaul branch fixes so a
|
||||
future regression is loud and obvious. These tests are intentionally narrow
|
||||
and additive — they do not replace the broader test_settings_schema.py.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
@@ -25,13 +24,14 @@ from openpilot.sunnypilot.sunnylink.tools.generate_settings_schema import (
|
||||
_load_torque_versions,
|
||||
generate_schema,
|
||||
)
|
||||
from openpilot.sunnypilot.sunnylink.tools.validate_settings_ui import validate as validate_settings_ui
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
|
||||
|
||||
SCHEMA_VALIDATOR_PATH = os.path.join(os.path.dirname(DEFINITION_PATH), "settings_ui.schema.json")
|
||||
|
||||
|
||||
def _walk_items(schema: dict[str, Any]):
|
||||
"""Yield every item dict from the schema."""
|
||||
|
||||
def _yield(item: dict[str, Any]):
|
||||
yield item
|
||||
for sub in item.get("sub_items", []):
|
||||
@@ -149,13 +149,22 @@ class TestTestManeuversSection(OpenpilotTestCase):
|
||||
assert "is_sp_release" in vis_refs
|
||||
enablement = section.get("enablement") or []
|
||||
enable_refs = json.dumps(enablement)
|
||||
assert "ShowAdvancedControls" in enable_refs, "test_maneuvers must gate ShowAdvancedControls via enablement"
|
||||
assert "ShowAdvancedControls" in enable_refs, \
|
||||
"test_maneuvers must gate ShowAdvancedControls via enablement"
|
||||
|
||||
|
||||
class TestValidator(OpenpilotTestCase):
|
||||
def test_validator_accepts_real_json(self):
|
||||
"""settings_ui.json passes the repository's production schema validator."""
|
||||
self.assertTrue(validate_settings_ui(DEFINITION_PATH))
|
||||
"""settings_ui.json validates against settings_ui.schema.json."""
|
||||
try:
|
||||
import jsonschema
|
||||
except ImportError:
|
||||
self.skipTest("jsonschema not installed")
|
||||
with open(DEFINITION_PATH) as f:
|
||||
data = json.load(f)
|
||||
with open(SCHEMA_VALIDATOR_PATH) as f:
|
||||
validator = json.load(f)
|
||||
jsonschema.validate(instance=data, schema=validator)
|
||||
|
||||
|
||||
class TestTorqueOptionGeneration(OpenpilotTestCase):
|
||||
@@ -168,17 +177,16 @@ class TestTorqueOptionGeneration(OpenpilotTestCase):
|
||||
assert item.get("options") == expected
|
||||
|
||||
def test_torque_versions_path_resolves(self):
|
||||
assert os.path.exists(TORQUE_VERSIONS_PATH), f"latcontrol_torque_versions.json not found at {TORQUE_VERSIONS_PATH}"
|
||||
assert os.path.exists(TORQUE_VERSIONS_PATH), (
|
||||
f"latcontrol_torque_versions.json not found at {TORQUE_VERSIONS_PATH}"
|
||||
)
|
||||
|
||||
|
||||
class TestReleaseBranchGates(OpenpilotTestCase):
|
||||
@parameterized.expand(
|
||||
[
|
||||
"EnableGithubRunner",
|
||||
"QuickBootToggle",
|
||||
],
|
||||
names=["key"],
|
||||
)
|
||||
@parameterized.expand([
|
||||
"EnableGithubRunner",
|
||||
"QuickBootToggle",
|
||||
], names=["key"])
|
||||
def test_sp_dev_items_gate_on_is_sp_release(self, schema, key):
|
||||
"""sunnypilot dev items must hide on sunnypilot release branches (is_sp_release gate)."""
|
||||
item = _find_item(schema, key)
|
||||
@@ -200,14 +208,11 @@ class TestSpuriousOffroadGatesDropped(OpenpilotTestCase):
|
||||
|
||||
|
||||
class TestNotEngagedReplacement(OpenpilotTestCase):
|
||||
@parameterized.expand(
|
||||
[
|
||||
"AlphaLongitudinalEnabled",
|
||||
"ToyotaEnforceStockLongitudinal",
|
||||
"ToyotaStopAndGoHack",
|
||||
],
|
||||
names=["key"],
|
||||
)
|
||||
@parameterized.expand([
|
||||
"AlphaLongitudinalEnabled",
|
||||
"ToyotaEnforceStockLongitudinal",
|
||||
"ToyotaStopAndGoHack",
|
||||
], names=["key"])
|
||||
def test_offroad_only_replaced_with_not_engaged(self, schema, key):
|
||||
"""These items should use not_engaged, not offroad_only."""
|
||||
item = _find_item(schema, key)
|
||||
@@ -215,5 +220,3 @@ class TestNotEngagedReplacement(OpenpilotTestCase):
|
||||
rule_types = _flatten_rule_types(item.get("enablement"))
|
||||
assert "offroad_only" not in rule_types, f"{key} still uses offroad_only"
|
||||
assert "not_engaged" in rule_types, f"{key} missing not_engaged"
|
||||
|
||||
|
||||
|
||||
@@ -276,36 +276,13 @@ class TestKnownPanels(OpenpilotTestCase):
|
||||
enhanced_enable_keys = {r.get("key") for r in enhanced.get("enablement", []) if r.get("type") == "param"}
|
||||
assert "NeuralNetworkLateralControl" in enhanced_enable_keys
|
||||
|
||||
def test_accel_controller_profile_mapping_and_enablement(self, schema):
|
||||
cruise = next(p for p in schema["panels"] if p["id"] == "cruise")
|
||||
items = {item["key"]: item for item in _iter_panel_items(cruise)}
|
||||
|
||||
assert items["AccelPersonalityEnabled"]["widget"] == "toggle"
|
||||
assert items["AccelPersonality"]["options"] == [
|
||||
{"value": 0, "label": "Eco"},
|
||||
{"value": 1, "label": "Normal"},
|
||||
{"value": 2, "label": "Sport"},
|
||||
]
|
||||
assert {
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": True,
|
||||
} in items["AccelPersonalityEnabled"]["enablement"]
|
||||
assert {
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": True,
|
||||
} in items["AccelPersonality"]["enablement"]
|
||||
profile_enable_keys = {rule.get("key") for rule in items["AccelPersonality"]["enablement"] if rule.get("type") == "param"}
|
||||
assert "AccelPersonalityEnabled" not in profile_enable_keys
|
||||
|
||||
|
||||
class TestKnownVehicleSettings(OpenpilotTestCase):
|
||||
def test_hyundai_has_longitudinal_tuning(self, schema):
|
||||
keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("hyundai"))}
|
||||
assert "HyundaiLongitudinalTuning" in keys
|
||||
|
||||
def test_toyota_has_enforce_stock_stop_go(self, schema):
|
||||
def test_toyota_has_enforce_stock_and_stop_go(self, schema):
|
||||
keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("toyota"))}
|
||||
assert "ToyotaEnforceStockLongitudinal" in keys
|
||||
assert "ToyotaStopAndGoHack" in keys
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,40 +0,0 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
# Define the service name
|
||||
SERVICE_NAME="actions.runner.sunnypilot.$(uname -n)"
|
||||
|
||||
# Function to control the service
|
||||
control_service() {
|
||||
local action=$1 # Store the function argument in a local variable
|
||||
sudo systemctl $action ${SERVICE_NAME}
|
||||
}
|
||||
|
||||
service_exists_and_is_loaded() {
|
||||
sudo systemctl status ${SERVICE_NAME} &>/dev/null
|
||||
if [[ $? -ne 4 ]]; then
|
||||
return 0 # Service is known to systemd (i.e., loaded)
|
||||
else
|
||||
return 1 # Service is unknown to systemd (i.e., not loaded)
|
||||
fi
|
||||
}
|
||||
|
||||
# Check for required argument
|
||||
if [[ -z $1 ]] || { [[ $1 != "start" ]] && [[ $1 != "stop" ]]; }; then
|
||||
echo "Usage: $0 {start|stop}"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# Store the script argument in a descriptive variable
|
||||
ACTION=$1
|
||||
|
||||
# Trap EXIT signal (Ctrl+C) and stop the service
|
||||
trap 'control_service stop ; exit' SIGINT SIGKILL EXIT
|
||||
|
||||
# Enter the main loop
|
||||
while true; do
|
||||
# Check if the service is actually present on the system
|
||||
if service_exists_and_is_loaded; then
|
||||
control_service $ACTION # Call the function with the specified action
|
||||
fi
|
||||
sleep 1 # Pause before the next iteration
|
||||
done
|
||||
@@ -68,10 +68,6 @@ def only_offroad(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
def livestream(started: bool, params: Params, CP: car.CarParams) -> bool:
|
||||
return params.get_bool("IsLiveStreaming")
|
||||
|
||||
def use_github_runner(started, params, CP: car.CarParams) -> bool:
|
||||
return not PC and params.get_bool("EnableGithubRunner") and (
|
||||
not params.get_bool("NetworkMetered") and not params.get_bool("GithubRunnerSufficientVoltage"))
|
||||
|
||||
def use_copyparty(started, params, CP: car.CarParams) -> bool:
|
||||
return bool(params.get_bool("EnableCopyparty"))
|
||||
|
||||
@@ -189,10 +185,6 @@ procs += [
|
||||
NativeProcess("locationd_llk", "openpilot/sunnypilot/selfdrive/locationd", ["./locationd"], only_onroad),
|
||||
]
|
||||
|
||||
if os.path.exists("./github_runner.sh"):
|
||||
procs += [NativeProcess("github_runner_start", "openpilot/system/manager",
|
||||
["./github_runner.sh", "start"], and_(only_offroad, use_github_runner), sigkill=False)]
|
||||
|
||||
if os.path.exists("../../sunnypilot/sunnylink/uploader.py"):
|
||||
procs += [PythonProcess("sunnylink_uploader", "openpilot.sunnypilot.sunnylink.uploader", use_sunnylink_uploader_shim)]
|
||||
|
||||
|
||||
@@ -45,9 +45,8 @@ class ScrollState(Enum):
|
||||
|
||||
|
||||
class GuiScrollPanel2:
|
||||
def __init__(self, horizontal: bool = True, handle_out_of_bounds: bool = True) -> None:
|
||||
def __init__(self, horizontal: bool = True) -> None:
|
||||
self._horizontal = horizontal
|
||||
self._handle_out_of_bounds = handle_out_of_bounds
|
||||
self._state = ScrollState.STEADY
|
||||
self._offset: rl.Vector2 = rl.Vector2(0, 0)
|
||||
self._initial_click_event: MouseEvent | None = None
|
||||
@@ -86,20 +85,6 @@ class GuiScrollPanel2:
|
||||
"""Returns (max_offset, min_offset) for the given bounds and content size."""
|
||||
return 0.0, min(0.0, bounds_size - content_size)
|
||||
|
||||
def _clamp_offset(self, bounds_size: float, content_size: float) -> None:
|
||||
if self._handle_out_of_bounds:
|
||||
return
|
||||
|
||||
max_offset, min_offset = self._get_offset_bounds(bounds_size, content_size)
|
||||
offset = self.get_offset()
|
||||
clamped_offset = max(min_offset, min(max_offset, offset))
|
||||
if clamped_offset == offset:
|
||||
return
|
||||
|
||||
self.set_offset(clamped_offset)
|
||||
if (clamped_offset == max_offset and self._velocity > 0) or (clamped_offset == min_offset and self._velocity < 0):
|
||||
self._velocity = 0.0
|
||||
|
||||
def _update_state(self, bounds_size: float, content_size: float, snap_target: float | None) -> None:
|
||||
"""Runs per render frame, independent of mouse events. Updates auto-scrolling state and velocity."""
|
||||
max_offset, min_offset = self._get_offset_bounds(bounds_size, content_size)
|
||||
@@ -153,8 +138,6 @@ class GuiScrollPanel2:
|
||||
factor = 1.0 - math.exp(-SNAP_RATE * dt)
|
||||
self.set_offset(self.get_offset() + dist * factor)
|
||||
|
||||
self._clamp_offset(bounds_size, content_size)
|
||||
|
||||
def _handle_mouse_event(self, mouse_event: MouseEvent, bounds: rl.Rectangle, bounds_size: float,
|
||||
content_size: float) -> None:
|
||||
max_offset, min_offset = self._get_offset_bounds(bounds_size, content_size)
|
||||
|
||||
@@ -75,6 +75,7 @@ class _Scroller(Widget):
|
||||
self._items: list[Widget] = []
|
||||
self._horizontal = horizontal
|
||||
self._snap_items = snap_items
|
||||
assert not self._snap_items or self._horizontal, "Snapping is only supported for horizontal scrolling"
|
||||
self._spacing = spacing
|
||||
self._pad = pad
|
||||
|
||||
@@ -190,20 +191,12 @@ class _Scroller(Widget):
|
||||
snap_target: float | None = None
|
||||
if self._snap_items and visible_items and self._scrolling_to[0] is None:
|
||||
# TODO: this doesn't handle two small buttons at the edges well
|
||||
center_pos = (self._rect.x + self._rect.width / 2) if self._horizontal else (self._rect.y + self._rect.height / 2)
|
||||
closest_delta_pos = min(
|
||||
(self._item_center_pos(item) - center_pos for item in visible_items),
|
||||
key=abs,
|
||||
)
|
||||
center_pos = self._rect.x + self._rect.width / 2
|
||||
closest_delta_pos = min((((item.rect.x + item.rect.width / 2) - center_pos) for item in visible_items), key=abs)
|
||||
snap_target = self.scroll_panel.get_offset() - closest_delta_pos
|
||||
|
||||
return self.scroll_panel.update(self._rect, content_size, snap_target=snap_target)
|
||||
|
||||
def _item_center_pos(self, item: Widget) -> float:
|
||||
if self._horizontal:
|
||||
return item.rect.x + item.rect.width / 2
|
||||
return item.rect.y + item.rect.height / 2
|
||||
|
||||
@property
|
||||
def moving_items(self) -> bool:
|
||||
return len(self._move_animations) > 0 or len(self._move_lift) > 0
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user