brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Switch personalities via steering wheel / onroad UI

Browse Source 
Added toggle to switch between the personalities via the steering wheel or an onroad UI button.

Co-Authored-By: henryccy <104284652+henryccy@users.noreply.github.com>
Co-Authored-By: Jason Jackrel <23621790+thinkpad4by3@users.noreply.github.com>
Co-Authored-By: Eric Brown <13560103+nworb-cire@users.noreply.github.com>
Co-Authored-By: Kevin Robert Keegan <3046315+krkeegan@users.noreply.github.com>
Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
Co-Authored-By: mike8643 <98910897+mike8643@users.noreply.github.com>
This commit is contained in:
FrogAi
2024-02-27 16:34:47 -07:00
parent 71eecd224c
commit 437ecc73ca
38 changed files with 406 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
selfdrive/car/toyota/carcontroller.py
View File

@@ -193,11 +193,11 @@ class CarController:
can_sends.append(toyotacan.create_acc_cancel_command(self.packer))
elif self.CP.openpilotLongitudinalControl:
can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, accel_raw, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert,
frogpilot_variables))
CS.distance_button, frogpilot_variables))
self.accel = pcm_accel_cmd
else:
can_sends.append(toyotacan.create_accel_command(self.packer, 0, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False,
frogpilot_variables))
CS.distance_button, frogpilot_variables))
if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.

55
selfdrive/car/toyota/carstate.py
View File

@@ -48,6 +48,7 @@ class CarState(CarStateBase):
self.lkas_hud = {}
# FrogPilot variables
self.profile_restored = False
self.traffic_signals = {}
@@ -174,34 +175,75 @@ class CarState(CarStateBase):
if self.CP.carFingerprint != CAR.PRIUS_V:
self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
# Experimental Mode via double clicking the LKAS button function
# FrogPilot functions
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):
# distance button is wired to the ACC module (camera or radar)
if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR):
distance_pressed = cp_acc.vl["ACC_CONTROL"]["DISTANCE"]
else:
distance_pressed = cp.vl["SDSU"]["FD_BUTTON"]
# Switch the current state of Experimental Mode if the LKAS button is double pressed
if frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available and self.CP.carFingerprint != CAR.PRIUS_V:
message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"]
lkas_pressed = any(self.lkas_hud.get(key) == 1 for key in message_keys)
if lkas_pressed and not self.lkas_previously_pressed:
if frogpilot_variables.conditional_experimental_mode:
self.fpf.update_cestatus_distance()
self.fpf.update_cestatus_lkas()
else:
self.fpf.update_experimental_mode()
self.lkas_previously_pressed = lkas_pressed
# Experimental Mode via long pressing the distance button function
# Distance button functions
if ret.cruiseState.available:
if distance_pressed:
self.distance_pressed_counter += 1
elif self.distance_previously_pressed:
# Set the distance lines on the dash to match the new personality if the button was held down for less than 0.5 seconds
if self.distance_pressed_counter < CRUISE_LONG_PRESS:
self.previous_personality_profile = (self.personality_profile + 2) % 3
self.fpf.distance_button_function(self.previous_personality_profile)
self.profile_restored = False
self.distance_pressed_counter = 0
# Switch the current state of Experimental Mode if the button is held down for 0.5 seconds
if self.distance_pressed_counter == CRUISE_LONG_PRESS and frogpilot_variables.experimental_mode_via_distance:
if frogpilot_variables.conditional_experimental_mode:
self.fpf.update_cestatus_lkas()
self.fpf.update_cestatus_distance()
else:
self.fpf.update_experimental_mode()
# Switch the current state of Traffic Mode if the button is held down for 2.5 seconds
if self.distance_pressed_counter == CRUISE_LONG_PRESS * 5 and frogpilot_variables.traffic_mode:
self.fpf.update_traffic_mode()
# Revert the previous changes to Experimental Mode
if frogpilot_variables.conditional_experimental_mode:
self.fpf.update_cestatus_distance()
else:
self.fpf.update_experimental_mode()
self.distance_previously_pressed = distance_pressed
# Update the distance lines on the dash upon ignition/onroad UI button clicked
if frogpilot_variables.personalities_via_wheel and ret.cruiseState.available:
# Need to subtract by 1 to comply with the personality profiles of "0", "1", and "2"
self.personality_profile = cp.vl["PCM_CRUISE_SM"]["DISTANCE_LINES"] - 1
# Sync with the onroad UI button
if self.fpf.personality_changed_via_ui:
self.profile_restored = False
self.previous_personality_profile = self.fpf.current_personality
self.fpf.reset_personality_changed_param()
# Set personality to the previous drive's personality or when the user changes it via the UI
if self.personality_profile == self.previous_personality_profile:
self.profile_restored = True
if not self.profile_restored:
self.distance_button = not self.distance_button
# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team!
self.update_traffic_signals(cp_cam)
SpeedLimitController.car_speed_limit = self.calculate_speed_limit(frogpilot_variables)
@@ -275,6 +317,11 @@ class CarState(CarStateBase):
("PRE_COLLISION", 33),
]
if CP.flags & ToyotaFlags.SMART_DSU and not CP.flags & ToyotaFlags.RADAR_CAN_FILTER:
messages += [
("SDSU", 100),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
@staticmethod

8
selfdrive/car/toyota/interface.py
View File

@@ -47,6 +47,14 @@ class CarInterface(CarInterfaceBase):
stop_and_go = candidate in TSS2_CAR
# 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):
ret.flags |= ToyotaFlags.SMART_DSU.value
if 0x2AA in fingerprint[0] and candidate in NO_DSU_CAR:
ret.flags |= ToyotaFlags.RADAR_CAN_FILTER.value
if candidate == CAR.PRIUS:
stop_and_go = True
ret.wheelbase = 2.70

4
selfdrive/car/toyota/toyotacan.py
View File

@@ -33,12 +33,12 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req,
return packer.make_can_msg("STEERING_LTA", 0, values)
def create_accel_command(packer, accel, accel_raw, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, frogpilot_variables):
def create_accel_command(packer, accel, accel_raw, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance_button, frogpilot_variables):
# TODO: find the exact canceling bit that does not create a chime
values = {
"ACCEL_CMD": accel, # compensated accel command
"ACC_TYPE": acc_type,
"DISTANCE": 0,
"DISTANCE": distance_button,
"MINI_CAR": lead,
"PERMIT_BRAKING": 1,
"RELEASE_STANDSTILL": not standstill_req,

1
selfdrive/car/toyota/values.py
View File

@@ -46,6 +46,7 @@ class ToyotaFlags(IntFlag):
HYBRID = 1
SMART_DSU = 2
DISABLE_RADAR = 4
RADAR_CAN_FILTER = 16
class CAR(StrEnum):