mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-23 23:02:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master-priv' into accel-profile

Browse Source
This commit is contained in:
Jason Wen
2024-07-01 16:18:04 -04:00
parent 0ae9eb274c 46c1362511
commit 750f9c8233
14 changed files with 176 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOGS.md
+8
View File
@@ -19,6 +19,14 @@ sunnypilot - 0.9.8.0 (2024-xx-xx)
* NOTE: Only enable this feature if your Toyota/Lexus vehicle has factory Blind Spot Monitor equipped, and mentioned in the supported platforms list
* UPDATED: Toyota: TSS2 longitudinal: Custom Tuning
* Re-tuned and tested by the community (July 1, 2024)
* NEW❗: Toyota - Automatic Brake Hold (AHB) thanks to AlexandreSato!
* When you stop the vehicle completely by depressing the brake pedal, sunnypilot will activate Auto Brake Hold
* NOTE: Only for Toyota/Lexus vehicles with TSS2/LSS2
* NEW❗: Toyota - Automatic Door Locking and Unlocking thanks to AlexandreSato, cydia2020, and dragonpilot-community!
* Auto Lock by Speed: All doors are automatically locked when vehicle speed is approximately 6 mph (10 km/h) or higher
* 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
* Kia Ceed Plug-in Hybrid Non-SCC 2022 support thanks to TerminatorNL!
sunnypilot - 0.9.7.1 (2024-06-13)
cereal/car.capnp
+1
View File
@@ -136,6 +136,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
speedLimitPreActive @138;
speedLimitConfirmed @139;
torqueNNLoad @140;
spAutoBrakeHold @141;
radarCanErrorDEPRECATED @15;
communityFeatureDisallowedDEPRECATED @62;
common/params.cc
+3
View File
@@ -318,6 +318,9 @@ std::unordered_map<std::string, uint32_t> keys = {
{"TorqueFriction", PERSISTENT | BACKUP},
{"TorqueMaxLatAccel", PERSISTENT | BACKUP},
{"TorquedOverride", PERSISTENT | BACKUP},
{"ToyotaAutoHold", PERSISTENT | BACKUP},
{"ToyotaAutoLockBySpeed", PERSISTENT | BACKUP},
{"ToyotaAutoUnlockByShifter", PERSISTENT | BACKUP},
{"ToyotaEnhancedBsm", PERSISTENT | BACKUP},
{"ToyotaSnG", PERSISTENT | BACKUP},
{"ToyotaTSS2Long", PERSISTENT | BACKUP},
panda
+1 -1
Submodule panda updated: 2b1869bb1b...9b76d69484
selfdrive/car/card.py
+3
View File
@@ -56,6 +56,7 @@ class Car:
self.mads_disengage_lateral_on_brake = self.params.get_bool("DisengageLateralOnBrake")
self.mads_dlob = self.enable_mads and self.mads_disengage_lateral_on_brake
self.mads_ndlob = self.enable_mads and not self.mads_disengage_lateral_on_brake
self.sp_toyota_auto_brake_hold = self.params.get_bool("ToyotaAutoHold")
self.CP.alternativeExperience = 0
if not self.disengage_on_accelerator:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
@@ -63,6 +64,8 @@ class Car:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS
elif self.mads_ndlob:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.MADS_DISABLE_DISENGAGE_LATERAL_ON_BRAKE
if self.sp_toyota_auto_brake_hold:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
if self.CP.customStockLongAvailable and self.CP.pcmCruise and self.params.get_bool("CustomStockLong"):
self.CP.pcmCruiseSpeed = False
selfdrive/car/toyota/carcontroller.py
+64
View File
@@ -1,4 +1,5 @@
from cereal import car
from common.conversions import Conversions as CV
from openpilot.common.numpy_fast import clip, interp
from openpilot.common.params import Params
from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \
@@ -10,6 +11,7 @@ from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_
UNSUPPORTED_DSU_CAR
from opendbc.can.packer import CANPacker
GearShifter = car.CarState.GearShifter
SteerControlType = car.CarParams.SteerControlType
VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -29,6 +31,9 @@ MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows
LEFT_BLINDSPOT = b"\x41"
RIGHT_BLINDSPOT = b"\x42"
UNLOCK_CMD = b"\x40\x05\x30\x11\x00\x40\x00\x00"
LOCK_CMD = b"\x40\x05\x30\x11\x00\x80\x00\x00"
class CarController(CarControllerBase):
def __init__(self, dbc_name, CP, VM):
@@ -48,6 +53,7 @@ class CarController(CarControllerBase):
self.accel = 0
self.param_s = Params()
self._is_metric = self.param_s.get_bool("IsMetric")
self._reverse_acc_change = self.param_s.get_bool("ReverseAccChange")
self._sng_hack = self.param_s.get_bool("ToyotaSnG")
@@ -55,7 +61,25 @@ class CarController(CarControllerBase):
self.right_blindspot_debug_enabled = False
self.last_blindspot_frame = 0
if CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
self.brake_hold_active: bool = False
self._brake_hold_counter: int = 0
self._brake_hold_reset: bool = False
self._prev_brake_pressed: bool = False
self._auto_lock_by_speed = self.param_s.get_bool("ToyotaAutoLockBySpeed")
self._auto_unlock_by_shifter = self.param_s.get_bool("ToyotaAutoUnlockByShifter")
self._auto_lock_speed = 10 * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)
self._auto_lock_once = False
self._gear_prev = GearShifter.park
def update(self, CC, CS, now_nanos):
if self.frame % 200 == 0:
self._is_metric = self.param_s.get_bool("IsMetric")
self._auto_lock_by_speed = self.param_s.get_bool("ToyotaAutoLockBySpeed")
self._auto_unlock_by_shifter = self.param_s.get_bool("ToyotaAutoUnlockByShifter")
self._auto_lock_speed = 10 * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)
actuators = CC.actuators
hud_control = CC.hudControl
pcm_cancel_cmd = CC.cruiseControl.cancel
@@ -64,6 +88,21 @@ class CarController(CarControllerBase):
# *** control msgs ***
can_sends = []
# automatic door locking and unlocking logic (@dragonpilot-community)
# thanks to AlexandreSato & cydia2020
# https://github.com/AlexandreSato/animalpilot/blob/personal/doors.py
gear = CS.out.gearShifter
if not CS.out.doorOpen:
if gear == GearShifter.park and self._gear_prev != gear:
if self._auto_unlock_by_shifter:
can_sends.append(make_can_msg(0x750, UNLOCK_CMD, 0))
self._auto_lock_once = False
elif gear == GearShifter.drive and not self._auto_lock_once and CS.out.vEgo >= self._auto_lock_speed:
if self._auto_lock_by_speed:
can_sends.append(make_can_msg(0x750, LOCK_CMD, 0))
self._auto_lock_once = True
self._gear_prev = gear
# *** steer torque ***
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params)
@@ -148,6 +187,9 @@ class CarController(CarControllerBase):
self.last_standstill = CS.out.standstill
if self.CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
can_sends.extend(self.create_auto_brake_hold_messages(CS))
# handle UI messages
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
@@ -266,3 +308,25 @@ class CarController(CarControllerBase):
# print("bsm poll right")
return can_sends
# auto brake hold (https://github.com/AlexandreSato/)
def create_auto_brake_hold_messages(self, CS: car.CarState, brake_hold_allowed_timer: int = 100):
can_sends = []
disallowed_gears = [GearShifter.park, GearShifter.reverse]
brake_hold_allowed = CS.out.standstill and CS.out.cruiseState.available and not CS.out.gasPressed and \
not CS.out.cruiseState.enabled and (CS.out.gearShifter not in disallowed_gears)
if brake_hold_allowed:
self._brake_hold_counter += 1
self.brake_hold_active = self._brake_hold_counter > brake_hold_allowed_timer and not self._brake_hold_reset
self._brake_hold_reset = not self._prev_brake_pressed and CS.out.brakePressed and not self._brake_hold_reset
else:
self._brake_hold_counter = 0
self.brake_hold_active = False
self._brake_hold_reset = False
self._prev_brake_pressed = CS.out.brakePressed
if self.frame % 2 == 0:
can_sends.append(toyotacan.create_brake_hold_command(self.packer, self.frame, CS.pre_collision_2, self.brake_hold_active))
return can_sends
selfdrive/car/toyota/carstate.py
+9
View File
@@ -79,6 +79,9 @@ class CarState(CarStateBase):
self._right_blindspot_d2 = 0
self._right_blindspot_counter = 0
if CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
self.pre_collision_2 = {}
self.frame = 0
def update(self, cp, cp_cam):
@@ -260,6 +263,9 @@ class CarState(CarStateBase):
if self.CP.spFlags & ToyotaFlagsSP.SP_ENHANCED_BSM and self.frame > 199:
ret.leftBlindspot, ret.rightBlindspot = self.sp_get_enhanced_bsm(cp)
if self.CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
self.pre_collision_2 = copy.copy(cp_cam.vl["PRE_COLLISION_2"])
self._update_traffic_signals(cp_cam)
ret.cruiseState.speedLimit = self._calculate_speed_limit()
self.frame += 1
@@ -463,4 +469,7 @@ class CarState(CarStateBase):
("PCS_HUD", 1),
]
if CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
messages.append(("PRE_COLLISION_2", 33))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
selfdrive/car/toyota/interface.py
+8
View File
@@ -195,6 +195,9 @@ class CarInterface(CarInterfaceBase):
if candidate == CAR.TOYOTA_PRIUS_TSS2:
ret.spFlags |= ToyotaFlagsSP.SP_NEED_DEBUG_BSM.value
if Params().get_bool("ToyotaAutoHold") and candidate in (TSS2_CAR - RADAR_ACC_CAR):
ret.spFlags |= ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD.value
return ret
@staticmethod
@@ -286,6 +289,11 @@ class CarInterface(CarInterfaceBase):
# while in standstill, send a user alert
events.add(EventName.manualRestart)
# auto brake hold
if self.CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
if self.CC.brake_hold_active and not ret.brakeHoldActive:
events.add(EventName.spAutoBrakeHold)
ret.events = events.to_msg()
return ret
selfdrive/car/toyota/toyotacan.py
+34
View File
@@ -130,3 +130,37 @@ def create_set_bsm_debug_mode(lr_blindspot, enabled):
def create_bsm_polling_status(lr_blindspot):
return make_can_msg(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)
selfdrive/car/toyota/values.py
+1
View File
@@ -64,6 +64,7 @@ class ToyotaFlagsSP(IntFlag):
SP_ZSS = 1
SP_ENHANCED_BSM = 2
SP_NEED_DEBUG_BSM = 4
SP_AUTO_BRAKE_HOLD = 8
class Footnote(Enum):
selfdrive/controls/lib/events.py
+8
View File
@@ -744,6 +744,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"),
},
EventName.spAutoBrakeHold: {
ET.PERMANENT: Alert(
"sunnypilot Brake Hold Active",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOWEST, VisualAlert.none, AudibleAlert.prompt, 0.),
},
EventName.parkBrake: {
ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"),
selfdrive/controls/lib/longitudinal_planner.py
+2 -2
View File
@@ -191,11 +191,11 @@ class LongitudinalPlanner:
accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsState'].personality)
self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsStateSP'].personality)
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsStateSP'].personality)
self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc
+31
View File
@@ -124,6 +124,18 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
toyotaTss2LongTune->setConfirmation(true, false);
addItem(toyotaTss2LongTune);
auto toyotaAbh = new ParamControl(
"ToyotaAutoHold",
tr("Enable Automatic Brake Hold (AHB)"),
QString("<b>%1</b><br><br>%2<br><br><b>%3</b>")
.arg(tr("WARNING: Only for Toyota/Lexus vehicles with TSS2/LSS2. USE AT YOUR OWN RISK."))
.arg(tr("When you stop the vehicle completely by depressing the brake pedal, sunnypilot will activate Auto Brake Hold."))
.arg(tr("Changing this setting takes effect when the car is powered off.")),
"../assets/offroad/icon_blank.png"
);
toyotaAbh->setConfirmation(true, false);
addItem(toyotaAbh);
toyotaEnhancedBsm = new ParamControl(
"ToyotaEnhancedBsm",
tr("Enable Enhanced Blind Spot Monitor"),
@@ -142,6 +154,24 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
toyotaSngHack->setConfirmation(true, false);
addItem(toyotaSngHack);
auto toyotaAutoLock = new ParamControl(
"ToyotaAutoLock",
tr("Enable Toyota Door Auto Locking"),
tr("sunnypilot will attempt to lock the doors when drive above 10 km/h (6.2 mph).\nReboot Required."),
"../assets/offroad/icon_blank.png"
);
toyotaAutoLock->setConfirmation(true, false);
addItem(toyotaAutoLock);
auto toyotaAutoUnlock = new ParamControl(
"ToyotaAutoUnlockByShifter",
tr("Enable Toyota Door Auto Unlocking"),
tr("sunnypilot will attempt to unlock the doors when shift to gear P.\nReboot Required."),
"../assets/offroad/icon_blank.png"
);
toyotaAutoUnlock->setConfirmation(true, false);
addItem(toyotaAutoUnlock);
// Volkswagen
addItem(new LabelControl(tr("Volkswagen")));
auto volkswagenCCOnly = new ParamControl(
@@ -158,6 +188,7 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
is_onroad = !offroad;
hkgSmoothStop->setEnabled(offroad);
toyotaTss2LongTune->setEnabled(offroad);
toyotaAbh->setEnabled(offroad);
toyotaEnhancedBsm->setEnabled(offroad);
toyotaSngHack->setEnabled(offroad);
volkswagenCCOnly->setEnabled(offroad);
system/manager/manager.py
+3
View File
@@ -97,6 +97,9 @@ def manager_init() -> None:
("TorqueDeadzoneDeg", "0"),
("TorqueFriction", "1"),
("TorqueMaxLatAccel", "250"),
("ToyotaAutoHold", "0"),
("ToyotaAutoLockBySpeed", "0"),
("ToyotaAutoUnlockByShifter", "0"),
("ToyotaEnhancedBsm", "0"),
("TrueVEgoUi", "0"),
("TurnSpeedControl", "0"),