Compare commits

...

26 Commits

Author SHA1 Message Date
Jason Wen 14bceeb172 rename 2024-09-29 14:02:09 -04:00
Jason Wen 124d1bb12f revert 2024-09-29 13:44:09 -04:00
Kumar 8ee80deb96 maybe>? 2024-09-28 15:01:41 -07:00
Jason Wen d4b79ff36a Revert "temp: build tf"
This reverts commit 0570833ad3.
2024-09-28 13:38:58 -04:00
Jason Wen 7ea4096155 Revert "tf"
This reverts commit 5cda10e74a.
2024-09-28 13:38:58 -04:00
Jason Wen 5cda10e74a tf 2024-09-28 13:31:57 -04:00
Jason Wen 0570833ad3 temp: build tf 2024-09-28 13:31:48 -04:00
Jason Wen bbbeb93cc2 Make sure it updates 2024-09-28 13:25:18 -04:00
Jason Wen 32ce8e58e9 Flipped 2024-09-28 13:08:28 -04:00
Jason Wen 0084010d55 get DMS version 2024-09-28 12:40:34 -04:00
Jason Wen 8f173083c2 gen7 model 2024-09-28 12:33:00 -04:00
Jason Wen 524a5f2f24 long planner: use temporalPose for v_model_error calibration 2024-09-28 11:31:30 -04:00
Harald Schäfer 44349a829a Safe exp in parsing (#33640)
(cherry picked from commit deb6b72091)
2024-09-28 11:21:41 -04:00
Harald Schäfer 8c04dcde59 Clip sigmoid overlfow (#33635)
* No more C sigmoid

* Clipping

* Need to cast to float

* Update ref

* Clip

* Model refg

(cherry picked from commit 251e2e9400)
2024-09-28 11:21:30 -04:00
ZwX1616 d7ec35a986 onnxmodel fp16_to_fp32: misc improvements (#33615)
(cherry picked from commit b2976631d2)
2024-09-28 11:20:57 -04:00
Kacper Rączy e26a805476 Add modelExecutionTime to DrivingModelData (#33606)
* Add model execution time to DrivingModelData

* Update model replay ref commit

* Update ref commit again

* Ignore this field in model replay

* Back to original ref commit

* Bring back

---------

Co-authored-by: Comma Device <device@comma.ai>
(cherry picked from commit 48938098e0)
2024-09-28 11:19:39 -04:00
Mitchell Goff 51d862921b Add gas/brake press probs to ModelDataV2 (#33382)
* Add gas/brake press probs to ModelDataV2

* Updated model_replay_ref_commit
old-commit-hash: 40fb90a8a2

(cherry picked from commit 6bb9e72c64)
2024-09-28 11:18:30 -04:00
Jason Wen 8e8463a996 Tomb Raider 2024-09-28 10:02:23 -04:00
Jason Wen 306df5935a new infra 2024-09-28 09:19:19 -04:00
Jason Wen b2cdfc09ae Longitudinal: Refactor Experimental Mode toggle with distance button hold (#409)
* init

* deprecated

* in controlsd directly

* update

* unused

* combine

* only once

* fix

* better

* cleanup

* more

* Update CHANGELOGS.md
2024-08-13 16:16:40 -04:00
Jason Wen 5b674d2231 car: Interface cleanup (#396)
* deprecated

* cruise main state

* v cruise non pcm state

* acc mads combo

* cancel cruise state

* sp common state

* sp event

* custom stock long

* started mads

* v cruise non pcm state old

* acc mads old

* started mads old
2024-08-13 15:57:27 -04:00
Jason Wen ddf69de7e0 Hyundai Longitudinal: Enable radar tracks based on availability (#406)
* Hyundai Longitudinal: Enable radar tracks based on availability

* don't loop

* just 2

* missed

* missed

* fix

* do this

* every time

* quicker

* less

* nope

* more

* cleanup
2024-08-13 15:50:35 -04:00
Jason Wen 69f88da9c5 Hyundai Longitudinal: Enable Cruise Main when started (#391)
* Hyundai Longitudinal: Enable Cruise Main when started

* don't do this for the ancients (@devtekve 😉)

* okay, toggle per @devtekve xD

* update texts

* non pcm only

* translations
2024-08-11 23:32:43 -04:00
Jason Wen 117ad3b3ef Hyundai Longitudinal: Match stock timestep for ESCC and Camera-based SCC (#407)
Hyundai ESCC: Match stock timestep
2024-08-11 23:29:42 -04:00
Jason Wen 6f3e44f76e MADS: Hyundai: Skip cruise main button check with pcmCruise (#405)
* MADS: Hyundai: Skip cruise main button check with `pcmCruise`

* 2 modes!
2024-08-11 23:21:31 -04:00
Jason Wen c70db1030c Toyota: comma Pedal: Fix upstream merge conflicts (#393) 2024-08-04 12:09:00 -04:00
55 changed files with 536 additions and 234 deletions
+6
View File
@@ -14,6 +14,9 @@ sunnypilot - 0.9.8.0 (2024-xx-xx)
* Provides a more responsive and tailored driving experience compared to predefined settings
* UPDATED: Driving Personality: Updated mode names
* Aggressive, Moderate, Standard, Relaxed
* NEW❗: Hyundai CAN: Enable Cruise Main by Default
* Set CRUISE MAIN to ON by default when the car starts, without engaging MADS
* This feature only applies when "openpilot Longitudinal Control (Alpha)" is enabled under the "Toggles" menu
* NEW❗: Toyota - Enhanced Blind Spot Monitor (BSM) thanks to arne182, rav4kumar, and eFiniLan!
* Enables Blind Spot Monitor (BSM) signals parsing in sunnypilot using the factory Blind Spot Monitor (BSM)
* sunnypilot will use debugging CAN messages to receive unfiltered BSM signals, allowing detection of more objects
@@ -37,6 +40,9 @@ sunnypilot - 0.9.8.0 (2024-xx-xx)
* Auto Unlock by Shift to P: All doors are automatically unlocked when shifting the shift lever to P
* FIXED: Driving Personality:
* Maniac mode now correctly enforced when selected
* FIXED: Experimental Model Distance Button Hold
* Experimental Model toggle with distance button hold no longer changes Personality
* Personality setting remains consistent when switching between Chill and Experimental Mode
* UI Updates
* Display Metrics Below Chevron
* NEW❗: Time to Lead Car
+2 -1
View File
@@ -137,6 +137,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
speedLimitPreActive @139;
speedLimitConfirmed @140;
torqueNNLoad @141;
hyundaiRadarTracksAvailable @142;
radarCanErrorDEPRECATED @15;
communityFeatureDisallowedDEPRECATED @62;
@@ -250,7 +251,7 @@ struct CarState {
struct CustomStockLong {
cruiseButton @0 :Int16;
finalSpeedKph @1 :Float32;
vCruiseKphPrev @2 :Float32;
vCruiseKphPrevDEPRECATED @2 :Float32;
targetSpeed @3 :Float32;
vSetDis @4 :Float32;
speedDiff @5 :Float32;
+2
View File
@@ -32,6 +32,8 @@ enum ModelGeneration {
three @3;
four @4;
five @5;
six @6;
seven @7;
}
struct ControlsStateSP @0x81c2f05a394cf4af {
+3
View File
@@ -871,6 +871,7 @@ struct DrivingModelData {
frameId @0 :UInt32;
frameIdExtra @1 :UInt32;
frameDropPerc @6 :Float32;
modelExecutionTime @7 :Float32;
action @2 :ModelDataV2.Action;
@@ -1010,6 +1011,8 @@ struct ModelDataV2 {
brake3MetersPerSecondSquaredProbs @4 :List(Float32);
brake4MetersPerSecondSquaredProbs @5 :List(Float32);
brake5MetersPerSecondSquaredProbs @6 :List(Float32);
gasPressProbs @7 :List(Float32);
brakePressProbs @8 :List(Float32);
}
struct Pose {
+5
View File
@@ -239,6 +239,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"DrivingModelGeneration", PERSISTENT},
{"DrivingModelMetadataText", PERSISTENT},
{"DrivingModelName", PERSISTENT},
{"DrivingModelSelectorVersion", CLEAR_ON_MANAGER_START},
{"DrivingModelText", PERSISTENT},
{"DrivingModelUrl", PERSISTENT},
{"DynamicExperimentalControl", PERSISTENT | BACKUP},
@@ -261,9 +262,13 @@ std::unordered_map<std::string, uint32_t> keys = {
{"HandsOnWheelMonitoring", PERSISTENT | BACKUP},
{"HasAcceptedTermsSP", PERSISTENT},
{"HideVEgoUi", PERSISTENT | BACKUP},
{"HyundaiCruiseMainDefault", PERSISTENT | BACKUP},
{"HkgSmoothStop", PERSISTENT | BACKUP},
{"HotspotOnBoot", PERSISTENT},
{"HotspotOnBootConfirmed", PERSISTENT},
{"HyundaiRadarTracksAvailable", PERSISTENT},
{"HyundaiRadarTracksAvailableCache", PERSISTENT},
{"HyundaiRadarTracksAvailablePersistent", PERSISTENT},
{"LastCarModel", PERSISTENT | BACKUP},
{"LastSpeedLimitSignTap", PERSISTENT},
{"LastSunnylinkPingTime", CLEAR_ON_MANAGER_START},
+1 -1
View File
@@ -218,7 +218,7 @@ def create_gas_interceptor_command(packer, gas_amount, idx):
values["GAS_COMMAND"] = gas_amount * 255.
values["GAS_COMMAND2"] = gas_amount * 255.
dat = packer.make_can_msg("GAS_COMMAND", 0, values)[2]
dat = packer.make_can_msg("GAS_COMMAND", 0, values)[1]
checksum = crc8_pedal(dat[:-1])
values["CHECKSUM_PEDAL"] = checksum
+9 -12
View File
@@ -98,10 +98,9 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.lkas_enabled, self.CS.prev_lkas_enabled, {1: ButtonType.altButton1}),
]
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
self.CS.button_events, c.vCruise,
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled,
enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.resumeCruise) if not self.CP.pcmCruiseSpeed else
(ButtonType.accelCruise, ButtonType.decelCruise),
resume_button=(ButtonType.resumeCruise,) if not self.CP.pcmCruiseSpeed else
@@ -114,16 +113,16 @@ class CarInterface(CarInterfaceBase):
if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.lkas_disabled = not self.CS.lkas_disabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS)
self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS.madsEnabled)
if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed:
if any(b.type == ButtonType.cancel for b in self.CS.button_events):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
@@ -136,7 +135,7 @@ class CarInterface(CarInterfaceBase):
self.CS.accEnabled = False
self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
@@ -146,7 +145,7 @@ class CarInterface(CarInterfaceBase):
# events
events = self.create_common_events(ret, c, extra_gears=[car.CarState.GearShifter.low], pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
events, ret = self.create_sp_events(ret, events)
# Low speed steer alert hysteresis logic
if self.CP.carFingerprint in RAM_DT:
@@ -162,9 +161,7 @@ class CarInterface(CarInterfaceBase):
if self.low_speed_alert:
events.add(car.CarEvent.EventName.belowSteerSpeed)
ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
self.CC.target_speed, self.CC.v_set_dis,
self.CC.speed_diff, self.CC.button_type)
ret.customStockLong = self.update_custom_stock_long()
ret.events = events.to_msg()
+7 -8
View File
@@ -79,10 +79,9 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.lkas_enabled, self.CS.prev_lkas_enabled, {1: ButtonType.altButton1}),
]
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
self.CS.button_events, c.vCruise)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled)
if ret.cruiseState.available:
if self.enable_mads:
@@ -90,15 +89,15 @@ class CarInterface(CarInterfaceBase):
self.CS.madsEnabled = True
if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0):
if any(b.type == ButtonType.cancel for b in self.CS.button_events):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
@@ -106,7 +105,7 @@ class CarInterface(CarInterfaceBase):
self.CS.accEnabled = False
self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
@@ -115,7 +114,7 @@ class CarInterface(CarInterfaceBase):
events = self.create_common_events(ret, c, extra_gears=[GearShifter.manumatic], pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
events, ret = self.create_sp_events(ret, events)
if not self.CS.vehicle_sensors_valid:
events.add(car.CarEvent.EventName.vehicleSensorsInvalid)
+7 -11
View File
@@ -204,8 +204,6 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback)
distance_button = 0
# Don't add event if transitioning from INIT, unless it's to an actual button
if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT:
self.CS.button_events = [
@@ -214,21 +212,19 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button,
{1: ButtonType.gapAdjustCruise})
]
distance_button = self.CS.distance_button
self.CS.button_events = [
*self.CS.button_events,
*create_button_events(self.CS.lkas_enabled, self.CS.prev_lkas_enabled, {1: ButtonType.altButton1}),
]
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
if not self.CP.pcmCruise:
if any(b.type == ButtonType.accelCruise and b.pressed for b in self.CS.button_events):
self.CS.accEnabled = True
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
self.CS.button_events, c.vCruise)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled)
if ret.cruiseState.available:
if self.enable_mads:
@@ -236,15 +232,15 @@ class CarInterface(CarInterfaceBase):
self.CS.madsEnabled = True
if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0):
if any(b.type == ButtonType.cancel for b in self.CS.button_events):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
@@ -252,7 +248,7 @@ class CarInterface(CarInterfaceBase):
self.CS.accEnabled = False
self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(distance_button))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
@@ -267,7 +263,7 @@ class CarInterface(CarInterfaceBase):
# if any(b.type == ButtonType.accelCruise and b.pressed for b in ret.buttonEvents):
# events.add(EventName.buttonEnable)
events, ret = self.create_sp_events(self.CS, ret, events, enable_pressed=self.CS.accEnabled,
events, ret = self.create_sp_events(ret, events, enable_pressed=self.CS.accEnabled,
enable_buttons=(ButtonType.decelCruise,))
# Enabling at a standstill with brake is allowed
+8 -11
View File
@@ -267,10 +267,9 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, SETTINGS_BUTTONS_DICT),
]
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
self.CS.button_events, c.vCruise)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled)
if ret.cruiseState.available:
if self.enable_mads:
@@ -278,7 +277,7 @@ class CarInterface(CarInterfaceBase):
self.CS.madsEnabled = True
if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
@@ -286,9 +285,9 @@ class CarInterface(CarInterfaceBase):
if not self.CP.pcmCruise or min_enable_speed_pcm or not self.CP.pcmCruiseSpeed:
if any(b.type == ButtonType.cancel for b in self.CS.button_events):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads or min_enable_speed_pcm \
else False if self.CP.pcmCruise \
else self.CS.accEnabled
@@ -301,7 +300,7 @@ class CarInterface(CarInterfaceBase):
elif not ret.cruiseState.enabled:
self.CS.accEnabled = False
ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=(self.CS.cruise_setting == 3))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
@@ -313,7 +312,7 @@ class CarInterface(CarInterfaceBase):
if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed and not self.CS.madsEnabled:
events.add(EventName.belowEngageSpeed)
events, ret = self.create_sp_events(self.CS, ret, events)
events, ret = self.create_sp_events(ret, events)
#if self.CP.pcmCruise:
# # we engage when pcm is active (rising edge)
@@ -330,9 +329,7 @@ class CarInterface(CarInterfaceBase):
if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
events.add(EventName.manualRestart)
ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
self.CC.target_speed, self.CC.v_set_dis,
self.CC.speed_diff, self.CC.button_type)
ret.customStockLong = self.update_custom_stock_long()
ret.events = events.to_msg()
+35 -15
View File
@@ -1,6 +1,8 @@
import cereal.messaging as messaging
from cereal import car
from panda import Panda
from openpilot.common.params import Params
from openpilot.selfdrive.car.sunnypilot.fingerprinting import can_fingerprint, get_one_can
from openpilot.selfdrive.car.hyundai.enable_radar_tracks import enable_radar_tracks
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsSP, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \
@@ -109,6 +111,7 @@ class CarInterface(CarInterfaceBase):
if DBC[ret.carFingerprint]["radar"] is None:
if ret.spFlags & (HyundaiFlagsSP.SP_ENHANCED_SCC | HyundaiFlagsSP.SP_CAMERA_SCC_LEAD):
ret.radarTimeStep = 0.02
ret.radarUnavailable = False
# *** feature detection ***
@@ -125,7 +128,8 @@ class CarInterface(CarInterfaceBase):
if ret.flags & HyundaiFlags.MANDO_RADAR and ret.radarUnavailable:
ret.spFlags |= HyundaiFlagsSP.SP_RADAR_TRACKS.value
ret.radarUnavailable = False
if Params().get_bool("HyundaiRadarTracksAvailable"):
ret.radarUnavailable = False
# *** panda safety config ***
if candidate in CANFD_CAR:
@@ -201,7 +205,23 @@ class CarInterface(CarInterfaceBase):
if CP.spFlags & HyundaiFlagsSP.SP_RADAR_TRACKS:
enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, config_data_id=b'\x01\x42')
params = Params()
rt_avail = params.get_bool("HyundaiRadarTracksAvailable")
rt_avail_persist = params.get_bool("HyundaiRadarTracksAvailablePersistent")
params.put_bool_nonblocking("HyundaiRadarTracksAvailableCache", rt_avail)
if not rt_avail_persist:
messaging.drain_sock_raw(logcan)
fingerprint = can_fingerprint(lambda: get_one_can(logcan))
radar_unavailable = RADAR_START_ADDR not in fingerprint[1] or DBC[CP.carFingerprint]["radar"] is None
params.put_bool_nonblocking("HyundaiRadarTracksAvailable", not radar_unavailable)
params.put_bool_nonblocking("HyundaiRadarTracksAvailablePersistent", True)
def _update(self, c):
if not self.CS.control_initialized and not self.CP.pcmCruise:
can_cruise_main_default = self.CP.spFlags & HyundaiFlagsSP.SP_CAN_LFA_BTN and not self.CP.flags & HyundaiFlags.CANFD and \
self.CS.params_list.hyundai_cruise_main_default
self.CS.mainEnabled = True if can_cruise_main_default or self.CP.carFingerprint in CANFD_CAR else False
ret = self.CS.update(self.cp, self.cp_cam)
self.CS.button_events = [
@@ -210,10 +230,9 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.main_buttons[-1], self.CS.prev_main_buttons, {1: ButtonType.altButton3}),
]
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
self.CS.button_events, c.vCruise)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled)
if ret.cruiseState.available:
if not self.CP.pcmCruiseSpeed:
@@ -221,12 +240,12 @@ class CarInterface(CarInterfaceBase):
self.CS.accEnabled = True
if self.enable_mads:
if not self.CS.prev_mads_enabled and self.CS.mads_enabled and \
any(b.type == ButtonType.altButton3 for b in self.CS.button_events):
if not self.CS.prev_mads_enabled and self.CS.mads_enabled and (self.CP.pcmCruise or
(any(b.type == ButtonType.altButton3 for b in self.CS.button_events) and not self.CP.pcmCruise)):
self.CS.madsEnabled = True
if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
if not ret.cruiseState.available and self.CS.out.cruiseState.available:
self.CS.madsEnabled = False
@@ -234,15 +253,15 @@ class CarInterface(CarInterfaceBase):
if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed:
if not self.CP.pcmCruise:
if any(b.type == ButtonType.cancel for b in self.CS.button_events):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if not self.CP.pcmCruiseSpeed:
if not ret.cruiseState.enabled:
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=(self.CS.cruise_buttons[-1] == 3))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
@@ -256,7 +275,7 @@ class CarInterface(CarInterfaceBase):
events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.manumatic],
pcm_enable=False, allow_enable=allow_enable)
events, ret = self.create_sp_events(self.CS, ret, events, main_enabled=True, allow_enable=allow_enable)
events, ret = self.create_sp_events(ret, events, main_enabled=True, allow_enable=allow_enable)
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
@@ -266,9 +285,10 @@ class CarInterface(CarInterfaceBase):
if self.low_speed_alert and self.CS.madsEnabled:
events.add(car.CarEvent.EventName.belowSteerSpeed)
ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
self.CC.target_speed, self.CC.v_set_dis,
self.CC.speed_diff, self.CC.button_type)
if self.CS.params_list.hyundai_radar_tracks_available and not self.CS.params_list.hyundai_radar_tracks_available_cache:
events.add(car.CarEvent.EventName.hyundaiRadarTracksAvailable)
ret.customStockLong = self.update_custom_stock_long()
ret.events = events.to_msg()
+53 -66
View File
@@ -244,8 +244,6 @@ class CarInterfaceBase(ABC):
self.cruise_cancelled_btn = True
self.prev_acc_mads_combo = False
self.mads_event_lock = True
self.gap_button_counter = 0
self.experimental_mode_hold = False
self.last_mads_init = 0.
self.madsEnabledInit = False
self.madsEnabledInitPrev = False
@@ -550,20 +548,20 @@ class CarInterfaceBase(ABC):
def sp_v_cruise_initialized(v_cruise):
return v_cruise != V_CRUISE_UNSET
def get_acc_mads(self, cruiseState_enabled, acc_enabled, mads_enabled):
def get_acc_mads(self, cs_out, mads_enabled):
if self.CS.params_list.acc_mads_combo:
if not self.prev_acc_mads_combo and (cruiseState_enabled or acc_enabled):
if not self.prev_acc_mads_combo and (cs_out.cruiseState.enabled or self.CS.accEnabled):
mads_enabled = True
self.prev_acc_mads_combo = (cruiseState_enabled or acc_enabled)
self.prev_acc_mads_combo = (cs_out.cruiseState.enabled or self.CS.accEnabled)
return mads_enabled
def get_sp_v_cruise_non_pcm_state(self, cs_out, acc_enabled, button_events, vCruise,
def get_sp_v_cruise_non_pcm_state(self, cs_out, vCruise, acc_enabled,
enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise),
resume_button=(ButtonType.accelCruise, ButtonType.resumeCruise)):
if cs_out.cruiseState.available:
for b in button_events:
for b in self.CS.button_events:
if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed:
if b.type in enable_buttons and not b.pressed:
acc_enabled = True
@@ -578,9 +576,9 @@ class CarInterfaceBase(ABC):
return acc_enabled
def get_sp_cancel_cruise_state(self, mads_enabled, acc_enabled=False):
mads_enabled = False if not self.enable_mads or self.disengage_on_accelerator else mads_enabled
return mads_enabled, acc_enabled
def get_sp_cancel_cruise_state(self):
self.CS.madsEnabled = False if not self.enable_mads or self.disengage_on_accelerator else self.CS.madsEnabled
self.CS.accEnabled = False
def get_sp_pedal_disengage(self, cs_out):
accel_pedal = cs_out.gasPressed and not self.CS.out.gasPressed and self.disengage_on_accelerator
@@ -588,24 +586,22 @@ class CarInterfaceBase(ABC):
regen = cs_out.regenBraking and (not self.CS.out.regenBraking or not cs_out.standstill)
return accel_pedal or brake or regen
def get_sp_cruise_main_state(self, cs_out, CS):
if not CS.control_initialized:
mads_enabled = False
def get_sp_cruise_main_state(self, cs_out):
if not self.CS.control_initialized:
return False
elif not self.CS.params_list.mads_main_toggle:
mads_enabled = False
return False
else:
mads_enabled = cs_out.cruiseState.available
return cs_out.cruiseState.available
return mads_enabled
def get_sp_started_mads(self, cs_out, CS):
if not cs_out.cruiseState.available and CS.out.cruiseState.available:
def get_sp_started_mads(self, cs_out, mads_enabled):
if not cs_out.cruiseState.available and self.CS.out.cruiseState.available:
self.madsEnabledInit = False
self.madsEnabledInitPrev = False
return False
if not self.CS.params_list.mads_main_toggle or self.prev_acc_mads_combo:
return CS.madsEnabled
if not self.madsEnabledInit and CS.madsEnabled:
return mads_enabled
if not self.madsEnabledInit and self.CS.madsEnabled:
self.madsEnabledInit = True
self.last_mads_init = time.monotonic()
if cs_out.gearShifter not in FORWARD_GEARS:
@@ -616,19 +612,16 @@ class CarInterfaceBase(ABC):
self.madsEnabledInitPrev = True
return cs_out.cruiseState.available
else:
return CS.madsEnabled
return mads_enabled
def get_sp_common_state(self, cs_out, CS, gear_allowed=True, gap_button=False):
cs_out.cruiseState.enabled = CS.accEnabled if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed else cs_out.cruiseState.enabled
def get_sp_common_state(self, cs_out, gear_allowed=True):
cs_out.cruiseState.enabled = self.CS.accEnabled if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed else cs_out.cruiseState.enabled
if not self.enable_mads:
if cs_out.cruiseState.enabled and not CS.out.cruiseState.enabled:
CS.madsEnabled = True
elif not cs_out.cruiseState.enabled and CS.out.cruiseState.enabled:
CS.madsEnabled = False
if self.CP.openpilotLongitudinalControl:
self.toggle_exp_mode(gap_button)
if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
self.CS.madsEnabled = True
elif not cs_out.cruiseState.enabled and self.CS.out.cruiseState.enabled:
self.CS.madsEnabled = False
lane_change_speed_min = get_min_lateral_speed(self.CS.params_list.pause_lateral_speed, self.CS.params_list.is_metric)
@@ -640,37 +633,24 @@ class CarInterfaceBase(ABC):
cs_out.latActive = gear_allowed
if not CS.control_initialized:
CS.control_initialized = True
if not self.CS.control_initialized:
self.CS.control_initialized = True
# Disable on rising edge of gas or brake. Also disable on brake when speed > 0.
if (cs_out.gasPressed and not self.CS.out.gasPressed and self.disengage_on_accelerator) or \
(cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill)) or \
(cs_out.regenBraking and (not self.CS.out.regenBraking or not cs_out.standstill)):
if CS.madsEnabled:
CS.disengageByBrake = True
if self.CS.madsEnabled:
self.CS.disengageByBrake = True
cs_out.madsEnabled = CS.madsEnabled
cs_out.accEnabled = CS.accEnabled
cs_out.disengageByBrake = CS.disengageByBrake
cs_out.madsEnabled = self.CS.madsEnabled
cs_out.accEnabled = self.CS.accEnabled
cs_out.disengageByBrake = self.CS.disengageByBrake
cs_out.brakeLightsDEPRECATED |= cs_out.brakePressed or cs_out.brakeHoldActive or cs_out.parkingBrake or cs_out.regenBraking
return cs_out, CS
return cs_out
# TODO: SP: use upstream's buttonEvents counter checks from controlsd
def toggle_exp_mode(self, gap_pressed):
if gap_pressed:
if not self.experimental_mode_hold:
self.gap_button_counter += 1
if self.gap_button_counter > 50:
self.gap_button_counter = 0
self.experimental_mode_hold = True
self.param_s.put_bool_nonblocking("ExperimentalMode", not self.CS.params_list.experimental_mode)
else:
self.gap_button_counter = 0
self.experimental_mode_hold = False
def create_sp_events(self, CS, cs_out, events, main_enabled=False, allow_enable=True, enable_pressed=False,
def create_sp_events(self, cs_out, events, main_enabled=False, allow_enable=True, enable_pressed=False,
enable_from_brake=False, enable_pressed_long=False,
enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):
@@ -678,7 +658,7 @@ class CarInterfaceBase(ABC):
if cs_out.disengageByBrake and cs_out.madsEnabled:
enable_pressed = True
enable_from_brake = True
CS.disengageByBrake = False
self.CS.disengageByBrake = False
cs_out.disengageByBrake = False
for b in cs_out.buttonEvents:
@@ -707,11 +687,11 @@ class CarInterfaceBase(ABC):
if self.CP.pcmCruise:
# do disable on button down
if main_enabled:
if any(CS.main_buttons) and not cs_out.cruiseState.enabled:
if any(self.CS.main_buttons) and not cs_out.cruiseState.enabled:
if not cs_out.madsEnabled:
events.add(EventName.buttonCancel)
# do enable on both accel and decel buttons
if cs_out.cruiseState.enabled and not CS.out.cruiseState.enabled and allow_enable:
if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled and allow_enable:
enable_pressed = True
enable_pressed_long = True
elif not cs_out.cruiseState.enabled:
@@ -731,6 +711,16 @@ class CarInterfaceBase(ABC):
return events, cs_out
def update_custom_stock_long(self):
customStockLong = car.CarState.CustomStockLong.new_message()
customStockLong.cruiseButton = 0 if self.CC.cruise_button is None else int(self.CC.cruise_button)
customStockLong.finalSpeedKph = float(self.CC.final_speed_kph)
customStockLong.targetSpeed = float(self.CC.target_speed)
customStockLong.vSetDis = float(self.CC.v_set_dis)
customStockLong.speedDiff = float(self.CC.speed_diff)
customStockLong.buttonType = int(self.CC.button_type)
return customStockLong
class RadarInterfaceBase(ABC):
def __init__(self, CP):
self.CP = CP
@@ -834,16 +824,6 @@ class CarStateBase(ABC):
return bool(left_blinker_stalk or self.left_blinker_cnt > 0), bool(right_blinker_stalk or self.right_blinker_cnt > 0)
def update_custom_stock_long(self, cruise_button, final_speed_kph, target_speed, v_set_dis, speed_diff, button_type):
customStockLong = car.CarState.CustomStockLong.new_message()
customStockLong.cruiseButton = 0 if cruise_button is None else cruise_button
customStockLong.finalSpeedKph = final_speed_kph
customStockLong.targetSpeed = target_speed
customStockLong.vSetDis = v_set_dis
customStockLong.speedDiff = speed_diff
customStockLong.buttonType = button_type
return customStockLong
@staticmethod
def parse_gear_shifter(gear: str | None) -> car.CarState.GearShifter:
if gear is None:
@@ -876,6 +856,13 @@ class CarControllerBase(ABC):
self.CP = CP
self.frame = 0
self.cruise_button = 0
self.final_speed_kph = 0.0
self.target_speed = 0.0
self.v_set_dis = 0.0
self.speed_diff = 0.0
self.button_type = 0
@abstractmethod
def update(self, CC: car.CarControl.Actuators, CS: car.CarState, now_nanos: int) -> tuple[car.CarControl.Actuators, list[SendCan]]:
pass
+8 -11
View File
@@ -45,10 +45,9 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.lkas_enabled, self.CS.prev_lkas_enabled, {1: ButtonType.altButton1}),
]
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
self.CS.button_events, c.vCruise)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled)
if ret.cruiseState.available:
if self.enable_mads:
@@ -56,15 +55,15 @@ class CarInterface(CarInterfaceBase):
self.CS.madsEnabled = True
if any(b.type == ButtonType.altButton1 and b.pressed for b in self.CS.button_events):
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed:
if any(b.type == ButtonType.cancel for b in self.CS.button_events):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
@@ -72,7 +71,7 @@ class CarInterface(CarInterfaceBase):
self.CS.accEnabled = False
self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
@@ -83,16 +82,14 @@ class CarInterface(CarInterfaceBase):
events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake],
pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
events, ret = self.create_sp_events(ret, events)
#if self.CS.lkas_disabled:
# events.add(EventName.lkasDisabled)
if self.CS.low_speed_alert:
events.add(EventName.belowSteerSpeed)
ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
self.CC.target_speed, self.CC.v_set_dis,
self.CC.speed_diff, self.CC.button_type)
ret.customStockLong = self.update_custom_stock_long()
ret.events = events.to_msg()
+5 -5
View File
@@ -35,22 +35,22 @@ class CarInterface(CarInterfaceBase):
self.CS.button_events = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
if ret.cruiseState.available:
if self.enable_mads:
if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
self.CS.madsEnabled = True
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
if (not ret.cruiseState.enabled and self.CS.out.cruiseState.enabled) or \
self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
@@ -61,7 +61,7 @@ class CarInterface(CarInterfaceBase):
events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake],
pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
events, ret = self.create_sp_events(ret, events)
if self.CS.lkas_enabled:
events.add(car.CarEvent.EventName.invalidLkasSetting)
+6
View File
@@ -9,8 +9,11 @@ class ParamManager:
"acc_mads_combo": False,
"below_speed_pause": False,
"experimental_mode": False,
"hyundai_radar_tracks_available": False,
"hyundai_radar_tracks_available_cache": False,
"is_metric": False,
"last_speed_limit_sign_tap": False,
"hyundai_cruise_main_default": False,
"mads_main_toggle": False,
"pause_lateral_speed": 0,
"reverse_acc_change": False,
@@ -35,8 +38,11 @@ class ParamManager:
"acc_mads_combo": params.get_bool("AccMadsCombo"),
"below_speed_pause": params.get_bool("BelowSpeedPause"),
"experimental_mode": params.get_bool("ExperimentalMode"),
"hyundai_radar_tracks_available": params.get_bool("HyundaiRadarTracksAvailable"),
"hyundai_radar_tracks_available_cache": params.get_bool("HyundaiRadarTracksAvailableCache"),
"is_metric": params.get_bool("IsMetric"),
"last_speed_limit_sign_tap": params.get_bool("LastSpeedLimitSignTap"),
"hyundai_cruise_main_default": params.get_bool("HyundaiCruiseMainDefault"),
"mads_main_toggle": params.get_bool("MadsCruiseMain"),
"pause_lateral_speed": int(params.get("PauseLateralSpeed", encoding="utf8")),
"reverse_acc_change": params.get_bool("ReverseAccChange"),
+5 -5
View File
@@ -117,7 +117,7 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
if ret.cruiseState.available:
if self.enable_mads:
@@ -128,7 +128,7 @@ class CarInterface(CarInterfaceBase):
self.CS.madsEnabled = not self.CS.madsEnabled
elif self.CS.prev_lkas_enabled != self.CS.lkas_enabled and self.CS.prev_lkas_enabled == 2 and self.CS.lkas_enabled != 1:
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
@@ -137,10 +137,10 @@ class CarInterface(CarInterfaceBase):
if not self.enable_mads:
self.CS.madsEnabled = False
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS)
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
@@ -150,7 +150,7 @@ class CarInterface(CarInterfaceBase):
events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low], pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
events, ret = self.create_sp_events(ret, events)
ret.events = events.to_msg()
@@ -0,0 +1,37 @@
from collections.abc import Callable
import cereal.messaging as messaging
from openpilot.selfdrive.car import gen_empty_fingerprint
FRAME_FINGERPRINT = 25 # 0.25s
def get_one_can(logcan):
while True:
can = messaging.recv_one_retry(logcan)
if len(can.can) > 0:
return can
def can_fingerprint(next_can: Callable) -> tuple[str | None, dict[int, dict]]:
finger = gen_empty_fingerprint()
frame = 0
done = False
while not done:
a = next_can()
for can in a.can:
# The fingerprint dict is generated for all buses, this way the car interface
# can use it to detect a (valid) multipanda setup and initialize accordingly
if can.src < 128:
if can.src not in finger:
finger[can.src] = {}
finger[can.src][can.address] = len(can.dat)
# bail if we've been waiting for more than 2s
done = frame > 100
frame += 1
return finger
+5 -8
View File
@@ -208,13 +208,10 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
distance_button = 0
if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER):
self.CS.button_events = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
distance_button = self.CS.distance_button
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
if ret.cruiseState.available:
if self.enable_mads:
@@ -229,16 +226,16 @@ class CarInterface(CarInterfaceBase):
if (not self.CS.prev_lkas_enabled and self.CS.lkas_enabled) or \
(self.CS.prev_lkas_enabled == 1 and not self.CS.lkas_enabled):
self.CS.madsEnabled = not self.CS.madsEnabled
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if not self.CP.pcmCruise:
ret.cruiseState.enabled = self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(distance_button))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
@@ -250,7 +247,7 @@ class CarInterface(CarInterfaceBase):
events = self.create_common_events(ret, c, extra_gears=[GearShifter.sport, GearShifter.low, GearShifter.brake],
pcm_enable=False)
events, ret = self.create_sp_events(self.CS, ret, events)
events, ret = self.create_sp_events(ret, events)
# Lane Tracing Assist control is unavailable (EPS_STATUS->LTA_STATE=0) until
# the more accurate angle sensor signal is initialized
+9 -13
View File
@@ -114,26 +114,25 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret, self.CS)
self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, self.CS.accEnabled,
self.CS.button_events, c.vCruise,
self.CS.accEnabled = self.get_sp_v_cruise_non_pcm_state(ret, c.vCruise, self.CS.accEnabled,
enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
if ret.cruiseState.available:
if self.enable_mads:
if not self.CS.prev_mads_enabled and self.CS.mads_enabled:
self.CS.madsEnabled = True
self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
self.CS.madsEnabled = self.get_acc_mads(ret, self.CS.madsEnabled)
else:
self.CS.madsEnabled = False
self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS)
self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS.madsEnabled)
if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed:
if any(b.type == ButtonType.cancel for b in self.CS.button_events):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
if self.get_sp_pedal_disengage(ret):
self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
self.get_sp_cancel_cruise_state()
ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
@@ -141,8 +140,7 @@ class CarInterface(CarInterfaceBase):
self.CS.accEnabled = False
self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
ret, self.CS = self.get_sp_common_state(ret, self.CS,
gap_button=any(b.type == ButtonType.gapAdjustCruise and b.pressed for b in self.CS.button_events))
ret = self.get_sp_common_state(ret)
ret.buttonEvents = [
*self.CS.button_events,
@@ -153,7 +151,7 @@ class CarInterface(CarInterfaceBase):
pcm_enable=False,
enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
events, ret = self.create_sp_events(self.CS, ret, events,
events, ret = self.create_sp_events(ret, events,
enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
# Low speed steer alert hysteresis logic
@@ -173,9 +171,7 @@ class CarInterface(CarInterfaceBase):
if self.CC.eps_timer_soft_disable_alert:
events.add(EventName.steerTimeLimit)
ret.customStockLong = self.CS.update_custom_stock_long(self.CC.cruise_button, self.CC.final_speed_kph,
self.CC.target_speed, self.CC.v_set_dis,
self.CC.speed_diff, self.CC.button_type)
ret.customStockLong = self.update_custom_stock_long()
ret.events = events.to_msg()
+14 -3
View File
@@ -20,7 +20,7 @@ from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.car.car_helpers import get_car_interface, get_startup_event
from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature, get_lag_adjusted_curvature
from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature, get_lag_adjusted_curvature, CRUISE_LONG_PRESS
from openpilot.selfdrive.controls.lib.events import Events, ET
from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
@@ -179,6 +179,7 @@ class Controls:
self.mads_disengage_lateral_on_brake = self.params.get_bool("DisengageLateralOnBrake")
self.mads_ndlob = self.enable_mads and not self.mads_disengage_lateral_on_brake
self.process_not_running = False
self.experimental_mode_update = False
self.custom_model_metadata = CustomModelMetadata(params=self.params, init_only=True)
self.model_use_lateral_planner = self.custom_model_metadata.valid and \
@@ -717,11 +718,21 @@ class Controls:
cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
setattr(actuators, p, 0.0)
# toggle experimental mode once on distance button hold
if self.CP.openpilotLongitudinalControl:
if self.v_cruise_helper.button_timers[ButtonType.gapAdjustCruise] == CRUISE_LONG_PRESS and \
not self.experimental_mode_update:
self.experimental_mode = not self.experimental_mode
self.params.put_bool_nonblocking("ExperimentalMode", self.experimental_mode)
self.experimental_mode_update = True
# decrement personality on distance button press
if self.CP.openpilotLongitudinalControl:
if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
self.personality = (self.personality - 1) % 3
self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
if not self.experimental_mode_update:
self.personality = (self.personality - 1) % 3
self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
self.experimental_mode_update = False
return CC, lac_log
+3 -3
View File
@@ -75,7 +75,7 @@ class VCruiseHelper:
self.v_cruise_kph = V_CRUISE_UNSET
self.v_cruise_cluster_kph = V_CRUISE_UNSET
self.v_cruise_kph_last = 0
self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0}
self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.gapAdjustCruise: 0}
self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}
self.is_metric_prev = None
@@ -101,10 +101,10 @@ class VCruiseHelper:
self._update_v_cruise_non_pcm(CS, enabled, is_metric, reverse_acc)
self._update_v_cruise_slc(long_plan_sp)
self.v_cruise_cluster_kph = self.v_cruise_kph
self.update_button_timers(CS, enabled)
else:
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
self.update_button_timers(CS, enabled)
else:
self.v_cruise_kph = V_CRUISE_UNSET
self.v_cruise_cluster_kph = V_CRUISE_UNSET
@@ -137,7 +137,7 @@ class VCruiseHelper:
long_press = True
break
if button_type is None:
if button_type is None or button_type == ButtonType.gapAdjustCruise:
return
resume_button = ButtonType.accelCruise
+3
View File
@@ -1141,6 +1141,9 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
ET.PERMANENT: torque_nn_load_alert,
},
EventName.hyundaiRadarTracksAvailable: {
ET.PERMANENT: NormalPermanentAlert("Radar tracks available. Restart the car to initialize")
}
}
+2 -2
View File
@@ -255,8 +255,8 @@ class RadarD:
self.radar_state.radarErrors = list(radar_errors)
self.radar_state.carStateMonoTime = sm.logMonoTime['carState']
if len(sm['modelV2'].temporalPose.trans):
model_v_ego = sm['modelV2'].temporalPose.trans[0]
if len(sm['modelV2'].velocity.x):
model_v_ego = sm['modelV2'].velocity.x[0]
else:
model_v_ego = self.v_ego
leads_v3 = sm['modelV2'].leadsV3
+10 -9
View File
@@ -76,13 +76,14 @@ class Plan:
class Meta:
ENGAGED = slice(0, 1)
# next 2, 4, 6, 8, 10 seconds
GAS_DISENGAGE = slice(1, 36, 7)
BRAKE_DISENGAGE = slice(2, 36, 7)
STEER_OVERRIDE = slice(3, 36, 7)
HARD_BRAKE_3 = slice(4, 36, 7)
HARD_BRAKE_4 = slice(5, 36, 7)
HARD_BRAKE_5 = slice(6, 36, 7)
GAS_PRESS = slice(7, 36, 7)
GAS_DISENGAGE = slice(1, 31, 6)
BRAKE_DISENGAGE = slice(2, 31, 6)
STEER_OVERRIDE = slice(3, 31, 6)
HARD_BRAKE_3 = slice(4, 31, 6)
HARD_BRAKE_4 = slice(5, 31, 6)
HARD_BRAKE_5 = slice(6, 31, 6)
# next 0, 2, 4, 6, 8, 10 seconds
LEFT_BLINKER = slice(36, 48, 2)
RIGHT_BLINKER = slice(37, 48, 2)
GAS_PRESS = slice(31, 55, 4)
BRAKE_PRESS = slice(32, 55, 4)
LEFT_BLINKER = slice(33, 55, 4)
RIGHT_BLINKER = slice(34, 55, 4)
+39 -2
View File
@@ -1,9 +1,10 @@
from enum import IntFlag
import os
import os, sys
from cereal import custom
from openpilot.common.params import Params
DRIVING_MODEL_SELECTOR_VERSION = 6
SIMULATION = "SIMULATION" in os.environ
ModelGeneration = custom.ModelGeneration
@@ -33,6 +34,10 @@ class ModelCapabilities(IntFlag):
In V2, 'prev_desired_curv' (no plural) is used as the input for the same 'desired_curvature' output.
"""
PlanVelocity = 2 ** 5
ModelOutputSlicesV1 = 2 ** 6
class CustomModelMetadata:
def __init__(self, params=None, init_only=False) -> None:
@@ -45,13 +50,19 @@ class CustomModelMetadata:
self.capabilities: ModelCapabilities = self.get_model_capabilities()
self.valid: bool = self.params.get_bool("CustomDrivingModel") and not SIMULATION and \
self.capabilities != ModelCapabilities.Default
self.initialized = False
def read_model_generation_param(self) -> ModelGeneration:
return int(self.params.get('DrivingModelGeneration') or ModelGeneration.default)
def get_model_capabilities(self) -> ModelCapabilities:
"""Returns the model capabilities for a given generation."""
if self.generation == ModelGeneration.five:
if self.generation == ModelGeneration.seven:
return ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.PlanVelocity | \
ModelCapabilities.ModelOutputSlicesV1
elif self.generation == ModelGeneration.six:
return ModelCapabilities.DesiredCurvatureV2 | ModelCapabilities.PlanVelocity
elif self.generation == ModelGeneration.five:
return ModelCapabilities.DesiredCurvatureV2
elif self.generation == ModelGeneration.four:
return ModelCapabilities.DesiredCurvatureV2
@@ -64,3 +75,29 @@ class CustomModelMetadata:
else:
# Default model is meant to represent the capabilities of the prebuilt model
return ModelCapabilities.Default
def custom_meta(self):
if not self.initialized:
if self.capabilities & ModelCapabilities.ModelOutputSlicesV1:
class Meta:
ENGAGED = slice(0, 1)
# next 2, 4, 6, 8, 10 seconds
GAS_DISENGAGE = slice(1, 41, 8)
BRAKE_DISENGAGE = slice(2, 41, 8)
STEER_OVERRIDE = slice(3, 41, 8)
HARD_BRAKE_3 = slice(4, 41, 8)
HARD_BRAKE_4 = slice(5, 41, 8)
HARD_BRAKE_5 = slice(6, 41, 8)
GAS_PRESS = slice(7, 41, 8)
BRAKE_PRESS = slice(8, 41, 8)
# next 0, 2, 4, 6, 8, 10 seconds
LEFT_BLINKER = slice(41, 53, 2)
RIGHT_BLINKER = slice(42, 53, 2)
module = 'openpilot.selfdrive.modeld.constants'
if module in sys.modules:
sys.modules[module].Meta = Meta
else:
raise ImportError(f"The module '{module}' is not imported yet or does not exist.")
self.initialized = True
+12 -12
View File
@@ -14,7 +14,7 @@ from openpilot.common.swaglog import cloudlog
from openpilot.common.params import Params
from openpilot.common.realtime import set_realtime_priority
from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
from openpilot.selfdrive.modeld.models.commonmodel_pyx import sigmoid
from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid
CALIB_LEN = 3
REG_SCALE = 0.25
@@ -88,15 +88,15 @@ def fill_driver_state(msg, ds_result: DriverStateResult):
msg.faceOrientationStd = [math.exp(x) for x in ds_result.face_orientation_std]
msg.facePosition = [x * REG_SCALE for x in ds_result.face_position[:2]]
msg.facePositionStd = [math.exp(x) for x in ds_result.face_position_std[:2]]
msg.faceProb = sigmoid(ds_result.face_prob)
msg.leftEyeProb = sigmoid(ds_result.left_eye_prob)
msg.rightEyeProb = sigmoid(ds_result.right_eye_prob)
msg.leftBlinkProb = sigmoid(ds_result.left_blink_prob)
msg.rightBlinkProb = sigmoid(ds_result.right_blink_prob)
msg.sunglassesProb = sigmoid(ds_result.sunglasses_prob)
msg.occludedProb = sigmoid(ds_result.occluded_prob)
msg.readyProb = [sigmoid(x) for x in ds_result.ready_prob]
msg.notReadyProb = [sigmoid(x) for x in ds_result.not_ready_prob]
msg.faceProb = float(sigmoid(ds_result.face_prob))
msg.leftEyeProb = float(sigmoid(ds_result.left_eye_prob))
msg.rightEyeProb = float(sigmoid(ds_result.right_eye_prob))
msg.leftBlinkProb = float(sigmoid(ds_result.left_blink_prob))
msg.rightBlinkProb = float(sigmoid(ds_result.right_blink_prob))
msg.sunglassesProb = float(sigmoid(ds_result.sunglasses_prob))
msg.occludedProb = float(sigmoid(ds_result.occluded_prob))
msg.readyProb = [float(sigmoid(x)) for x in ds_result.ready_prob]
msg.notReadyProb = [float(sigmoid(x)) for x in ds_result.not_ready_prob]
def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: int, execution_time: float, dsp_execution_time: float):
model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(DMonitoringModelResult)).contents
@@ -105,8 +105,8 @@ def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts:
ds.frameId = frame_id
ds.modelExecutionTime = execution_time
ds.dspExecutionTime = dsp_execution_time
ds.poorVisionProb = sigmoid(model_result.poor_vision_prob)
ds.wheelOnRightProb = sigmoid(model_result.wheel_on_right_prob)
ds.poorVisionProb = float(sigmoid(model_result.poor_vision_prob))
ds.wheelOnRightProb = float(sigmoid(model_result.wheel_on_right_prob))
ds.rawPredictions = model_output.tobytes() if SEND_RAW_PRED else b''
fill_driver_state(ds.leftDriverData, model_result.driver_state_lhd)
fill_driver_state(ds.rightDriverData, model_result.driver_state_rhd)
+24 -8
View File
@@ -3,7 +3,7 @@ import capnp
import numpy as np
from cereal import log
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
from openpilot.selfdrive.modeld.custom_model_metadata import ModelCapabilities
from openpilot.selfdrive.modeld.custom_model_metadata import CustomModelMetadata, ModelCapabilities
SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -55,7 +55,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float,
timestamp_eof: int, timestamp_llk: int, model_execution_time: float,
nav_enabled: bool, valid: bool,
custom_model_valid: bool, custom_model_capabilities: ModelCapabilities) -> None:
custom_model: CustomModelMetadata) -> None:
custom_model.custom_meta()
frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
frame_drop_perc = frame_drop * 100
extended_msg.valid = valid
@@ -66,9 +67,10 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
driving_model_data.frameId = vipc_frame_id
driving_model_data.frameIdExtra = vipc_frame_id_extra
driving_model_data.frameDropPerc = frame_drop_perc
driving_model_data.modelExecutionTime = model_execution_time
action = driving_model_data.action
model_use_lateral_planner = custom_model_valid and custom_model_capabilities & ModelCapabilities.LateralPlannerSolution
model_use_lateral_planner = custom_model.valid and custom_model.capabilities & ModelCapabilities.LateralPlannerSolution
if not model_use_lateral_planner:
action.desiredCurvature = float(net_output_data['desired_curvature'][0,0])
@@ -78,7 +80,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
modelV2.frameAge = frame_age
modelV2.frameDropPerc = frame_drop_perc
modelV2.timestampEof = timestamp_eof
model_use_nav = custom_model_valid and custom_model_capabilities & ModelCapabilities.NoO
model_use_nav = custom_model.valid and custom_model.capabilities & ModelCapabilities.NoO
if model_use_nav:
modelV2.locationMonoTimeDEPRECATED = timestamp_llk
modelV2.modelExecutionTime = model_execution_time
@@ -172,6 +174,8 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_3].tolist()
disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_4].tolist()
disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist()
disengage_predictions.gasPressProbs = net_output_data['meta'][0,Meta.GAS_PRESS].tolist()
disengage_predictions.brakePressProbs = net_output_data['meta'][0,Meta.BRAKE_PRESS].tolist()
publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:]
publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_5][0]
@@ -183,10 +187,22 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
# temporal pose
temporal_pose = modelV2.temporalPose
temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
if custom_model.valid:
if custom_model.capabilities & ModelCapabilities.PlanVelocity:
temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
else:
temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
else:
temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
# confidence
if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:
+1 -1
View File
@@ -377,7 +377,7 @@ def main(demo=False):
posenet_send = messaging.new_message('cameraOdometry')
fill_model_msg(drivingdata_send, modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
frame_drop_ratio, meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen,
custom_model_metadata.valid, custom_model_metadata.capabilities)
custom_model_metadata)
if not (custom_model_metadata.valid and custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution):
desire_state = modelv2_send.modelV2.meta.desireState
+1 -5
View File
@@ -47,8 +47,4 @@ ModelFrame::~ModelFrame() {
CL_CHECK(clReleaseMemObject(u_cl));
CL_CHECK(clReleaseMemObject(y_cl));
CL_CHECK(clReleaseCommandQueue(q));
}
float sigmoid(float input) {
return 1 / (1 + expf(-input));
}
}
-2
View File
@@ -16,8 +16,6 @@
#include "selfdrive/modeld/transforms/loadyuv.h"
#include "selfdrive/modeld/transforms/transform.h"
float sigmoid(float input);
class ModelFrame {
public:
ModelFrame(cl_device_id device_id, cl_context context);
-2
View File
@@ -12,8 +12,6 @@ cdef extern from "common/clutil.h":
cl_context cl_create_context(cl_device_id)
cdef extern from "selfdrive/modeld/models/commonmodel.h":
float sigmoid(float)
cppclass ModelFrame:
int buf_size
ModelFrame(cl_device_id, cl_context)
+1 -3
View File
@@ -8,10 +8,8 @@ from libc.string cimport memcpy
from msgq.visionipc.visionipc cimport cl_mem
from msgq.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext
from .commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context
from .commonmodel cimport mat3, sigmoid as cppSigmoid, ModelFrame as cppModelFrame
from .commonmodel cimport mat3, ModelFrame as cppModelFrame
def sigmoid(x):
return cppSigmoid(x)
cdef class CLContext(BaseCLContext):
def __cinit__(self):
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:39786068cae1ed8c0dc34ef80c281dfcc67ed18a50e06b90765c49bcfdbf7db4
size 51453312
oid sha256:0dab94dc94db5e372f56d26f9dda98bd31fd76e4c16027da611132263848d0c1
size 62486347
+10 -5
View File
@@ -1,15 +1,19 @@
import numpy as np
from openpilot.selfdrive.modeld.constants import ModelConstants
def safe_exp(x, out=None):
# -11 is around 10**14, more causes float16 overflow
return np.exp(np.clip(x, -np.inf, 11), out=out)
def sigmoid(x):
return 1. / (1. + np.exp(-x))
return 1. / (1. + safe_exp(-x))
def softmax(x, axis=-1):
x -= np.max(x, axis=axis, keepdims=True)
if x.dtype == np.float32 or x.dtype == np.float64:
np.exp(x, out=x)
safe_exp(x, out=x)
else:
x = np.exp(x)
x = safe_exp(x)
x /= np.sum(x, axis=axis, keepdims=True)
return x
@@ -44,7 +48,7 @@ class Parser:
n_values = (raw.shape[2] - out_N)//2
pred_mu = raw[:,:,:n_values]
pred_std = np.exp(raw[:,:,n_values: 2*n_values])
pred_std = safe_exp(raw[:,:,n_values: 2*n_values])
if in_N > 1:
weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype)
@@ -87,7 +91,8 @@ class Parser:
self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
if 'sim_pose' in outs:
self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
+8 -2
View File
@@ -14,8 +14,12 @@ def attributeproto_fp16_to_fp32(attr):
attr.data_type = 1
attr.raw_data = float32_list.astype(np.float32).tobytes()
def convert_fp16_to_fp32(path):
model = onnx.load(path)
def convert_fp16_to_fp32(onnx_path_or_bytes):
if isinstance(onnx_path_or_bytes, bytes):
model = onnx.load_from_string(onnx_path_or_bytes)
elif isinstance(onnx_path_or_bytes, str):
model = onnx.load(onnx_path_or_bytes)
for i in model.graph.initializer:
if i.data_type == 10:
attributeproto_fp16_to_fp32(i)
@@ -23,6 +27,8 @@ def convert_fp16_to_fp32(path):
if i.type.tensor_type.elem_type == 10:
i.type.tensor_type.elem_type = 1
for i in model.graph.node:
if i.op_type == 'Cast' and i.attribute[0].i == 10:
i.attribute[0].i = 1
for a in i.attribute:
if hasattr(a, 't'):
if a.t.data_type == 10:
@@ -130,6 +130,7 @@ if __name__ == "__main__":
ignore = [
'logMonoTime',
'drivingModelData.frameDropPerc',
'drivingModelData.modelExecutionTime',
'modelV2.frameDropPerc',
'modelV2.modelExecutionTime',
'driverStateV2.modelExecutionTime',
@@ -1 +1 @@
01e8432830805165527d1dbadd5f009d19430b39
9d4d653d8cc361fe2ba53f74fd8fcfe1f1d559ed
@@ -566,7 +566,7 @@ CONFIGS = [
proc_name="modeld",
pubs=["deviceState", "roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState"],
subs=["modelV2", "drivingModelData", "cameraOdometry"],
ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime"],
ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime", "drivingModelData.frameDropPerc", "drivingModelData.modelExecutionTime"],
should_recv_callback=ModeldCameraSyncRcvCallback(),
tolerance=NUMPY_TOLERANCE,
processing_time=0.020,
@@ -177,5 +177,18 @@ std::vector<Model> ModelsFetcher::getModelsFromURL(const QString&url) {
}
std::vector<Model> ModelsFetcher::getModelsFromURL() {
return getModelsFromURL("https://docs.sunnypilot.ai/models_v5.json");
return getModelsFromURL("https://docs.sunnypilot.ai/driving_models.json");
}
std::vector<Model> ModelsFetcher::getCompatibleModels(const QString &selector_version) {
std::vector<Model> allModels = getModelsFromURL();
std::vector<Model> compatibleModels;
for (const auto &model : allModels) {
if (model.isCompatible(selector_version)) {
compatibleModels.push_back(model);
}
}
return compatibleModels;
}
@@ -34,6 +34,7 @@ Last updated: July 29, 2024
#include <QDir>
#include <QJsonObject>
#include <QTimer>
#include <QVersionNumber>
#include "common/swaglog.h"
#include "common/util.h"
@@ -86,6 +87,8 @@ public:
index = json["index"].toString();
environment = json["environment"].toString();
generation = json["generation"].toString();
minDrivingModelSelectorVersion = json["minimum_selector_version"].toString();
}
// Method to convert model back to QJsonObject, if needed
@@ -117,6 +120,9 @@ public:
json["index"] = index;
json["environment"] = environment;
json["generation"] = generation;
json["minimum_selector_version"] = minDrivingModelSelectorVersion;
return json;
}
@@ -134,6 +140,13 @@ public:
QString index;
QString environment;
QString generation;
QString minDrivingModelSelectorVersion;
bool isCompatible(const QString &selector_version) const {
return QVersionNumber::fromString(selector_version) >=
QVersionNumber::fromString(minDrivingModelSelectorVersion);
}
};
class ModelsFetcher : public QObject {
@@ -145,6 +158,7 @@ public:
static std::vector<Model> getModelsFromURL(const QUrl&url);
static std::vector<Model> getModelsFromURL(const QString&url);
static std::vector<Model> getModelsFromURL();
static std::vector<Model> getCompatibleModels(const QString &selector_version);
signals:
void downloadProgress(double percentage);
@@ -210,12 +210,13 @@ void SoftwarePanelSP::handleCurrentModelLblBtnClicked() {
checkNetwork();
const auto currentModelName = QString::fromStdString(params.get("DrivingModelName"));
const bool is_release_sp = params.getBool("IsReleaseSPBranch");
const auto models = ModelsFetcher::getModelsFromURL();
const QString selector_version = QString::fromStdString(params.get("DrivingModelSelectorVersion"));
const auto compatible_models = ModelsFetcher::getCompatibleModels(selector_version);
QMap<QString, QString> index_to_model;
// Collecting indices with display names
for (const auto &model : models) {
for (const auto &model : compatible_models) {
if ((is_release_sp && model.environment == "release") || !is_release_sp) {
index_to_model.insert(model.index, model.displayName);
}
@@ -243,7 +244,7 @@ void SoftwarePanelSP::handleCurrentModelLblBtnClicked() {
}
// Finding and setting the selected model
for (auto &model : models) {
for (auto &model : compatible_models) {
if (model.displayName == selectedModelName) {
selectedModelToDownload = model;
selectedNavModelToDownload = model;
@@ -109,6 +109,16 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
hkgSmoothStop->setConfirmation(true, false);
addItem(hkgSmoothStop);
hyundaiCruiseMainDefault = new ParamControlSP(
"HyundaiCruiseMainDefault",
tr("HKG CAN: Enable Cruise Main by Default"),
QString("<b>%1</b><br><br>%2")
.arg(tr("WARNING: This feature only applies when \"openpilot Longitudinal Control (Alpha)\" is enabled under \"Toggles\""))
.arg(tr("Enabling this toggle sets CRUISE MAIN to ON by default when the car starts, without engaging MADS. The user still needs to manually engage MADS.")),
"../assets/offroad/icon_blank.png");
hyundaiCruiseMainDefault->setConfirmation(true, false);
addItem(hyundaiCruiseMainDefault);
// Subaru
addItem(new LabelControlSP(tr("Subaru")));
auto subaruManualParkingBrakeSng = new ParamControlSP(
@@ -194,6 +204,7 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
connect(uiStateSP(), &UIStateSP::offroadTransition, [=](bool offroad) {
is_onroad = !offroad;
hkgSmoothStop->setEnabled(offroad);
hyundaiCruiseMainDefault->setEnabled(offroad);
toyotaTss2LongTune->setEnabled(offroad);
toyotaEnhancedBsm->setEnabled(offroad);
toyotaSngHack->setEnabled(offroad);
@@ -223,6 +234,17 @@ void SPVehiclesTogglesPanel::updateToggles() {
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
// Hyundai/Kia/Genesis
{
if (CP.getCarName() == "hyundai") {
if ((CP.getSpFlags() & 2) and !(CP.getFlags() & 8192)) {
hyundaiCruiseMainDefault->setEnabled(true);
} else {
hyundaiCruiseMainDefault->setEnabled(false);
}
}
}
// Toyota: Enhanced Blind Spot Monitor
{
if (CP.getCarName() == "toyota") {
@@ -67,6 +67,7 @@ private:
Params params;
bool is_onroad = false;
ParamControlSP *hyundaiCruiseMainDefault;
ParamControlSP *stockLongToyota;
ParamControlSP *toyotaEnhancedBsm;
+12
View File
@@ -1583,6 +1583,18 @@ Reboot Required.</source>
<source>Tested on RAV4 TSS1, Lexus LSS1, Toyota TSS1/1.5, and Prius TSS2.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>HKG CAN: Enable Cruise Main by Default</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>WARNING: This feature only applies when &quot;openpilot Longitudinal Control (Alpha)&quot; is enabled under &quot;Toggles&quot;</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enabling this toggle sets CRUISE MAIN to ON by default when the car starts, without engaging MADS. The user still needs to manually engage MADS.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>SettingsWindow</name>
+12
View File
@@ -1565,6 +1565,18 @@ Reboot Required.</source>
<source>Tested on RAV4 TSS1, Lexus LSS1, Toyota TSS1/1.5, and Prius TSS2.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>HKG CAN: Enable Cruise Main by Default</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>WARNING: This feature only applies when &quot;openpilot Longitudinal Control (Alpha)&quot; is enabled under &quot;Toggles&quot;</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enabling this toggle sets CRUISE MAIN to ON by default when the car starts, without engaging MADS. The user still needs to manually engage MADS.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>SettingsWindow</name>
+12
View File
@@ -1563,6 +1563,18 @@ Reboot Required.</source>
<source>Tested on RAV4 TSS1, Lexus LSS1, Toyota TSS1/1.5, and Prius TSS2.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>HKG CAN: Enable Cruise Main by Default</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>WARNING: This feature only applies when &quot;openpilot Longitudinal Control (Alpha)&quot; is enabled under &quot;Toggles&quot;</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enabling this toggle sets CRUISE MAIN to ON by default when the car starts, without engaging MADS. The user still needs to manually engage MADS.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>SettingsWindow</name>
+12
View File
@@ -1567,6 +1567,18 @@ Reboot Required.</source>
<source>Tested on RAV4 TSS1, Lexus LSS1, Toyota TSS1/1.5, and Prius TSS2.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>HKG CAN: Enable Cruise Main by Default</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>WARNING: This feature only applies when &quot;openpilot Longitudinal Control (Alpha)&quot; is enabled under &quot;Toggles&quot;</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enabling this toggle sets CRUISE MAIN to ON by default when the car starts, without engaging MADS. The user still needs to manually engage MADS.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>SettingsWindow</name>
+12
View File
@@ -1561,6 +1561,18 @@ Reboot Required.</source>
<source>Tested on RAV4 TSS1, Lexus LSS1, Toyota TSS1/1.5, and Prius TSS2.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>HKG CAN: Enable Cruise Main by Default</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>WARNING: This feature only applies when &quot;openpilot Longitudinal Control (Alpha)&quot; is enabled under &quot;Toggles&quot;</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enabling this toggle sets CRUISE MAIN to ON by default when the car starts, without engaging MADS. The user still needs to manually engage MADS.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>SettingsWindow</name>
+12
View File
@@ -1563,6 +1563,18 @@ Reboot Required.</source>
<source>Tested on RAV4 TSS1, Lexus LSS1, Toyota TSS1/1.5, and Prius TSS2.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>HKG CAN: Enable Cruise Main by Default</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>WARNING: This feature only applies when &quot;openpilot Longitudinal Control (Alpha)&quot; is enabled under &quot;Toggles&quot;</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enabling this toggle sets CRUISE MAIN to ON by default when the car starts, without engaging MADS. The user still needs to manually engage MADS.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>SettingsWindow</name>
+12
View File
@@ -1567,6 +1567,18 @@ Reboot Required.</source>
<source>Tested on RAV4 TSS1, Lexus LSS1, Toyota TSS1/1.5, and Prius TSS2.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>HKG CAN: Enable Cruise Main by Default</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>WARNING: This feature only applies when &quot;openpilot Longitudinal Control (Alpha)&quot; is enabled under &quot;Toggles&quot;</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enabling this toggle sets CRUISE MAIN to ON by default when the car starts, without engaging MADS. The user still needs to manually engage MADS.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>SettingsWindow</name>
+12
View File
@@ -1563,6 +1563,18 @@ Reboot Required.</source>
<source>Tested on RAV4 TSS1, Lexus LSS1, Toyota TSS1/1.5, and Prius TSS2.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>HKG CAN: Enable Cruise Main by Default</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>WARNING: This feature only applies when &quot;openpilot Longitudinal Control (Alpha)&quot; is enabled under &quot;Toggles&quot;</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enabling this toggle sets CRUISE MAIN to ON by default when the car starts, without engaging MADS. The user still needs to manually engage MADS.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>SettingsWindow</name>
+12
View File
@@ -1561,6 +1561,18 @@ Reboot Required.</source>
<source>Tested on RAV4 TSS1, Lexus LSS1, Toyota TSS1/1.5, and Prius TSS2.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>HKG CAN: Enable Cruise Main by Default</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>WARNING: This feature only applies when &quot;openpilot Longitudinal Control (Alpha)&quot; is enabled under &quot;Toggles&quot;</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enabling this toggle sets CRUISE MAIN to ON by default when the car starts, without engaging MADS. The user still needs to manually engage MADS.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>SettingsWindow</name>
+12
View File
@@ -1563,6 +1563,18 @@ Reboot Required.</source>
<source>Tested on RAV4 TSS1, Lexus LSS1, Toyota TSS1/1.5, and Prius TSS2.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>HKG CAN: Enable Cruise Main by Default</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>WARNING: This feature only applies when &quot;openpilot Longitudinal Control (Alpha)&quot; is enabled under &quot;Toggles&quot;</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enabling this toggle sets CRUISE MAIN to ON by default when the car starts, without engaging MADS. The user still needs to manually engage MADS.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>SettingsWindow</name>
+12
View File
@@ -1563,6 +1563,18 @@ Reboot Required.</source>
<source>Tested on RAV4 TSS1, Lexus LSS1, Toyota TSS1/1.5, and Prius TSS2.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>HKG CAN: Enable Cruise Main by Default</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>WARNING: This feature only applies when &quot;openpilot Longitudinal Control (Alpha)&quot; is enabled under &quot;Toggles&quot;</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enabling this toggle sets CRUISE MAIN to ON by default when the car starts, without engaging MADS. The user still needs to manually engage MADS.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>SettingsWindow</name>
+3
View File
@@ -11,6 +11,7 @@ import openpilot.system.sentry as sentry
from openpilot.common.api.sunnylink import UNREGISTERED_SUNNYLINK_DONGLE_ID
from openpilot.common.params import Params, ParamKeyType
from openpilot.common.text_window import TextWindow
from openpilot.selfdrive.modeld.custom_model_metadata import DRIVING_MODEL_SELECTOR_VERSION
from openpilot.system.hardware import HARDWARE, PC
from openpilot.system.manager.helpers import unblock_stdout, write_onroad_params, save_bootlog
from openpilot.system.manager.mapd_installer import VERSION
@@ -64,6 +65,7 @@ def manager_init() -> None:
("DisableOnroadUploads", "0"),
("DisengageLateralOnBrake", "0"),
("DrivingModelGeneration", "0"),
("DrivingModelSelectorVersion", str(DRIVING_MODEL_SELECTOR_VERSION)),
("DynamicLaneProfile", "1"),
("DynamicPersonality", "0"),
("EnableMads", "1"),
@@ -72,6 +74,7 @@ def manager_init() -> None:
("HandsOnWheelMonitoring", "0"),
("HasAcceptedTermsSP", "0"),
("HideVEgoUi", "0"),
("HyundaiCruiseMainDefault", "0"),
("LastSpeedLimitSignTap", "0"),
("LkasToggle", "0"),
("MadsIconToggle", "1"),