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

Merge branch 'hkg-can-jerk' into lt

Browse Source
This commit is contained in:
Jason Wen
2024-08-04 09:11:32 -04:00
parent 0c76a761ce 5002f3fc73
commit ca5fa0e6d9
7 changed files with 111 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params.cc
+1
View File
@@ -261,6 +261,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"HandsOnWheelMonitoring", PERSISTENT | BACKUP},
{"HasAcceptedTermsSP", PERSISTENT},
{"HideVEgoUi", PERSISTENT | BACKUP},
{"HkgCustomLongTuning", PERSISTENT | BACKUP},
{"HkgSmoothStop", PERSISTENT | BACKUP},
{"HotspotOnBoot", PERSISTENT},
{"HotspotOnBootConfirmed", PERSISTENT},
selfdrive/car/hyundai/carcontroller.py
+84 -4
View File
@@ -1,7 +1,7 @@
from cereal import car
import cereal.messaging as messaging
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import clip
from openpilot.common.numpy_fast import clip, interp
from openpilot.common.params import Params
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg
@@ -97,6 +97,19 @@ class CarController(CarControllerBase):
self.hkg_can_smooth_stop = self.param_s.get_bool("HkgSmoothStop")
self.lead_distance = 0
self.jerk = 0.0
self.jerk_l = 0.0
self.jerk_u = 0.0
self.jerkStartLimit = 2.0
self.cb_upper = 0.0
self.cb_lower = 0.0
self.jerk_count = 0.0
self.accel_raw = 0
self.accel_val = 0
self.accel_last_jerk = 0
self.hkg_custom_long_tuning = self.param_s.get_bool("HkgCustomLongTuning")
def calculate_lead_distance(self, hud_control: car.CarControl.HUDControl) -> float:
lead_one = self.sm["radarState"].leadOne
lead_two = self.sm["radarState"].leadTwo
@@ -199,6 +212,9 @@ class CarController(CarControllerBase):
if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.append(make_tester_present_msg(0x7b1, self.CAN.ECAN, suppress_response=True))
if self.CP.openpilotLongitudinalControl:
self.make_jerk(CS, accel, actuators)
# CAN-FD platforms
if self.CP.carFingerprint in CANFD_CAR:
hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
@@ -227,7 +243,7 @@ class CarController(CarControllerBase):
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
if self.frame % 2 == 0:
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CS, CC.enabled and CS.out.cruiseState.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
set_speed_in_units, hud_control))
set_speed_in_units, hud_control, self.jerk_u, self.jerk_l))
self.accel_last = accel
else:
# button presses
@@ -277,9 +293,10 @@ class CarController(CarControllerBase):
# TODO: unclear if this is needed
jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, accel, jerk, int(self.frame / 2),
self.make_accel(actuators)
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled and CS.out.cruiseState.enabled, self.accel_raw, self.accel_val, self.jerk_l, self.jerk_u, int(self.frame / 2),
hud_control, set_speed_in_units, stopping,
CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance))
CC.cruiseControl.override, use_fca, CS, escc, self.CP, self.lead_distance, self.cb_lower, self.cb_upper))
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
@@ -453,3 +470,66 @@ class CarController(CarControllerBase):
cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev) # MPH/KPH based button presses
return cruise_button
# jerk calculations thanks to apilot!
def cal_jerk(self, accel, actuators):
self.accel_raw = accel
if actuators.longControlState == LongCtrlState.off:
accel_diff = 0.0
elif actuators.longControlState == LongCtrlState.stopping:# or hud_control.softHold > 0:
accel_diff = 0.0
else:
accel_diff = self.accel_raw - self.accel_last_jerk
accel_diff /= DT_CTRL
self.jerk = self.jerk * 0.9 + accel_diff * 0.1
return self.jerk
def make_jerk(self, CS, accel, actuators):
jerk = self.cal_jerk(accel, actuators)
a_error = accel - CS.out.aEgo
jerk = jerk + (a_error * 2.0)
if not self.hkg_custom_long_tuning:
self.jerk_u = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
self.jerk_l = 5.0
elif True: #self.CP.carFingerprint in CANFD_CAR or self.CP.carFingerprint == CAR.HYUNDAI_KONA_EV_2022:
startingJerk = 0.5
jerkLimit = 5.0
self.jerk_count += DT_CTRL
jerk_max = interp(self.jerk_count, [0, 1.5, 2.5], [startingJerk, startingJerk, jerkLimit])
if actuators.longControlState == LongCtrlState.off:
self.jerk_u = jerkLimit
self.jerk_l = jerkLimit
self.jerk_count = 0
else:
self.jerk_u = min(max(0.5, jerk * 2.0), jerk_max)
self.jerk_l = min(max(1.0, -jerk * 3.0), jerkLimit)
else:
startingJerk = self.jerkStartLimit
jerkLimit = 5.0
self.jerk_count += DT_CTRL
jerk_max = interp(self.jerk_count, [0, 1.5, 2.5], [startingJerk, startingJerk, jerkLimit])
self.cb_upper = self.cb_lower = 0
if actuators.longControlState == LongCtrlState.off:
self.jerk_u = jerkLimit
self.jerk_l = jerkLimit
self.jerk_count = 0
else:
self.jerk_u = min(max(0.5, jerk * 2.0), jerk_max)
self.jerk_l = min(max(0.5, -jerk * 2.0), jerkLimit)
self.cb_upper = clip(0.9 + accel * 0.2, 0, 1.2)
self.cb_lower = clip(0.8 + accel * 0.2, 0, 1.2)
def make_accel(self, actuators):
long_control = actuators.longControlState
is_ice = not self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV)
rate_up = 0.1
rate_down = 0.1
if long_control == LongCtrlState.off:
self.accel_raw, self.accel_val = 0, 0
else:
#self.accel_val = clip(self.accel_raw, self.accel_last - rate_down, self.accel_last + rate_up)
self.accel_val = self.accel_raw
self.accel_last = self.accel_val
self.accel_last_jerk = self.accel_val
selfdrive/car/hyundai/hyundaican.py
+8 -8
View File
@@ -130,8 +130,8 @@ def create_lfahda_mfc(packer, enabled, lat_active, lateral_paused, blinking_icon
}
return packer.make_can_msg("LFAHDA_MFC", 0, values)
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca,
CS, escc, CP, lead_distance):
def create_acc_commands(packer, enabled, accel_raw, accel_val, lower_jerk, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca,
CS, escc, CP, lead_distance, cb_lower, cb_upper):
commands = []
scc11_values = {
@@ -150,8 +150,8 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, se
scc12_values = {
"ACCMode": 2 if enabled and long_override else 1 if enabled else 0,
"StopReq": 1 if stopping else 0,
"aReqRaw": accel,
"aReqValue": accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
"aReqRaw": accel_raw,
"aReqValue": accel_val, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
"CR_VSM_Alive": idx % 0xF,
}
@@ -172,10 +172,10 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, se
commands.append(packer.make_can_msg("SCC12", 0, scc12_values))
scc14_values = {
"ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values
"ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values
"ComfortBandUpper": cb_upper, # stock usually is 0 but sometimes uses higher values
"ComfortBandLower": cb_lower, # stock usually is 0 but sometimes uses higher values
"JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values
"JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values
"JerkLowerLimit": lower_jerk, # stock usually is 0.5 but sometimes uses higher values
"ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
"ObjGap": get_object_gap(lead_distance), # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
}
@@ -207,7 +207,7 @@ def create_acc_opt(packer, escc, CS, CP):
commands = []
scc13_values = {
"SCCDrvModeRValue": 2,
"SCCDrvModeRValue": 3,
"SCC_Equip": 1,
"Lead_Veh_Dep_Alert_USM": 2,
}
selfdrive/car/hyundai/hyundaicanfd.py
+3 -3
View File
@@ -121,7 +121,7 @@ def create_lfahda_cluster(packer, CAN, enabled, lat_active, lateral_paused, blin
return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)
def create_acc_control(packer, CAN, CS, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control):
def create_acc_control(packer, CAN, CS, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, upper_jerk, lower_jerk):
jerk = 5
jn = jerk / 50
if not enabled or gas_override:
@@ -137,8 +137,8 @@ def create_acc_control(packer, CAN, CS, enabled, accel_last, accel, stopping, ga
"aReqValue": a_val,
"aReqRaw": a_raw,
"VSetDis": set_speed,
"JerkLowerLimit": jerk if enabled else 1,
"JerkUpperLimit": 3.0,
"JerkLowerLimit": lower_jerk,
"JerkUpperLimit": upper_jerk,
"ACC_ObjDist": 1,
"ObjValid": 0,
selfdrive/car/hyundai/interface.py
+3 -1
View File
@@ -104,7 +104,9 @@ class CarInterface(CarInterfaceBase):
ret.stoppingControl = True
ret.startingState = True
ret.vEgoStarting = 0.1
ret.startAccel = 1.0
ret.startAccel = 1.8
ret.stopAccel = 0.0
ret.stoppingDecelRate = 10
ret.longitudinalActuatorDelay = 0.5
if DBC[ret.carFingerprint]["radar"] is None:
selfdrive/controls/lib/longcontrol.py
+3
View File
@@ -75,6 +75,9 @@ class LongControl:
if output_accel > self.CP.stopAccel:
output_accel = min(output_accel, 0.0)
output_accel -= self.CP.stoppingDecelRate * DT_CTRL
elif output_accel < self.CP.stopAccel == 0.0:
output_accel = min(output_accel, 0.0)
output_accel += self.CP.stoppingDecelRate * DT_CTRL
self.reset()
elif self.long_control_state == LongCtrlState.starting:
selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle_settings.cc
+9 -8
View File
@@ -101,13 +101,14 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
// Hyundai/Kia/Genesis
addItem(new LabelControlSP(tr("Hyundai/Kia/Genesis")));
auto hkgSmoothStop = new ParamControlSP(
"HkgSmoothStop",
tr("HKG CAN: Smoother Stopping Performance (Beta)"),
tr("Smoother stopping behind a stopped car or desired stopping event. This is only applicable to HKG CAN platforms using openpilot longitudinal control."),
"../assets/offroad/icon_blank.png");
hkgSmoothStop->setConfirmation(true, false);
addItem(hkgSmoothStop);
auto hkgCustomLongTuning = new ParamControlSP(
"HkgCustomLongTuning",
tr("HKG: Custom Tuning for New Longitudinal API"),
tr(""),
"../assets/offroad/icon_blank.png"
);
hkgCustomLongTuning->setConfirmation(true, false);
addItem(hkgCustomLongTuning);
// Subaru
addItem(new LabelControlSP(tr("Subaru")));
@@ -205,7 +206,7 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
// trigger offroadTransition when going onroad/offroad
connect(uiStateSP(), &UIStateSP::offroadTransition, [=](bool offroad) {
is_onroad = !offroad;
hkgSmoothStop->setEnabled(offroad);
hkgCustomLongTuning->setEnabled(offroad);
toyotaTss2LongTune->setEnabled(offroad);
toyotaAbh->setEnabled(offroad);
toyotaEnhancedBsm->setEnabled(offroad);