GM Stock Custom Longitudinal Control
Co-Authored-By: garrettpall <76917194+garrettpall@users.noreply.github.com>
This commit is contained in:
FrogAi
parent
c6319a4aa6
commit
1b9fc72d80
@ -253,6 +253,8 @@ std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"Compass", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
|
||||
{"ConditionalExperimental", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
|
||||
{"CrosstrekTorque", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES},
|
||||
{"CSLCEnabled", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
|
||||
{"CSLCSpeed", PERSISTENT},
|
||||
{"CurrentHolidayTheme", PERSISTENT},
|
||||
{"CurrentRandomEvent", PERSISTENT},
|
||||
{"CurveSensitivity", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
|
||||
|
||||
@ -165,6 +165,7 @@ BO_ 481 ASCMSteeringButton: 7 K124_ASCM
|
||||
SG_ DistanceButton : 22|1@0+ (1,0) [0|0] "" NEO
|
||||
SG_ LKAButton : 23|1@0+ (1,0) [0|0] "" NEO
|
||||
SG_ ACCAlwaysOne : 24|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ ACCByFive : 26|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ ACCButtons : 46|3@0+ (1,0) [0|0] "" NEO
|
||||
SG_ DriveModeButton : 39|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ RollingCounter : 33|2@0+ (1,0) [0|3] "" NEO
|
||||
|
||||
@ -58,7 +58,7 @@ const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8
|
||||
{0x1E1, 2, 7}, {0x184, 2, 8}}; // camera bus
|
||||
|
||||
const CanMsg GM_SDGM_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, // pt bus
|
||||
{0x184, 2, 8}}; // camera bus
|
||||
{0x184, 2, 8}, {0x1E1, 2, 7}}; // camera bus
|
||||
|
||||
const CanMsg GM_CC_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x1E1, 0, 7}, // pt bus
|
||||
{0x184, 2, 8}, {0x1E1, 2, 7}}; // camera bus
|
||||
@ -84,6 +84,7 @@ const uint16_t GM_PARAM_NO_CAMERA = 32;
|
||||
const uint16_t GM_PARAM_NO_ACC = 64;
|
||||
const uint16_t GM_PARAM_PEDAL_LONG = 128; // TODO: this can be inferred
|
||||
const uint16_t GM_PARAM_PEDAL_INTERCEPTOR = 256;
|
||||
const uint16_t GM_PARAM_CSLC = 512;
|
||||
|
||||
enum {
|
||||
GM_BTN_UNPRESS = 1,
|
||||
@ -105,6 +106,7 @@ bool gm_pedal_long = false;
|
||||
bool gm_cc_long = false;
|
||||
bool gm_skip_relay_check = false;
|
||||
bool gm_force_ascm = false;
|
||||
bool gm_cslc = false;
|
||||
|
||||
static void handle_gm_wheel_buttons(const CANPacket_t *to_push) {
|
||||
int button = (GET_BYTE(to_push, 5) & 0x70U) >> 4;
|
||||
@ -261,8 +263,8 @@ static bool gm_tx_hook(const CANPacket_t *to_send) {
|
||||
int button = (GET_BYTE(to_send, 5) >> 4) & 0x7U;
|
||||
|
||||
bool allowed_btn = (button == GM_BTN_CANCEL) && cruise_engaged_prev;
|
||||
// For standard CC, allow spamming of SET / RESUME
|
||||
if (gm_cc_long) {
|
||||
// For standard CC and CSLC, allow spamming of SET / RESUME
|
||||
if (gm_cc_long || (((gm_hw == GM_SDGM) || (gm_hw == GM_CAM)) && gm_cslc)) {
|
||||
allowed_btn |= cruise_engaged_prev && (button == GM_BTN_SET || button == GM_BTN_RESUME || button == GM_BTN_UNPRESS);
|
||||
}
|
||||
|
||||
@ -335,6 +337,7 @@ static safety_config gm_init(uint16_t param) {
|
||||
gm_skip_relay_check = GET_FLAG(param, GM_PARAM_NO_CAMERA);
|
||||
gm_has_acc = !GET_FLAG(param, GM_PARAM_NO_ACC);
|
||||
enable_gas_interceptor = GET_FLAG(param, GM_PARAM_PEDAL_INTERCEPTOR);
|
||||
gm_cslc = GET_FLAG(param, GM_PARAM_CSLC);
|
||||
|
||||
safety_config ret = BUILD_SAFETY_CFG(gm_rx_checks, GM_ASCM_TX_MSGS);
|
||||
if (gm_hw == GM_CAM) {
|
||||
|
||||
@ -239,6 +239,7 @@ class Panda:
|
||||
FLAG_GM_NO_ACC = 64
|
||||
FLAG_GM_PEDAL_LONG = 128 # TODO: This can be inferred
|
||||
FLAG_GM_GAS_INTERCEPTOR = 256
|
||||
FLAG_GM_CSLC = 512
|
||||
|
||||
FLAG_FORD_LONG_CONTROL = 1
|
||||
FLAG_FORD_CANFD = 2
|
||||
|
||||
@ -59,7 +59,7 @@ class CarD:
|
||||
if self.always_on_lateral:
|
||||
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL
|
||||
|
||||
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX
|
||||
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX # Also needed for GM CSLC
|
||||
|
||||
car_recognized = self.CP.carName != 'mock'
|
||||
openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle")
|
||||
|
||||
@ -7,9 +7,9 @@ from openpilot.common.realtime import DT_CTRL
|
||||
from opendbc.can.packer import CANPacker
|
||||
from openpilot.selfdrive.car import apply_driver_steer_torque_limits, create_gas_interceptor_command
|
||||
from openpilot.selfdrive.car.gm import gmcan
|
||||
from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR
|
||||
from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR, EV_CAR, CAMERA_ACC_CAR
|
||||
from openpilot.selfdrive.car.interfaces import CarControllerBase
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import apply_deadzone
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import apply_deadzone, V_CRUISE_MAX
|
||||
from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
@ -18,6 +18,8 @@ LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
GearShifter = car.CarState.GearShifter
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
|
||||
params_memory = Params("/dev/shm/params")
|
||||
|
||||
# Camera cancels up to 0.1s after brake is pressed, ECM allows 0.5s
|
||||
CAMERA_CANCEL_DELAY_FRAMES = 10
|
||||
# Enforce a minimum interval between steering messages to avoid a fault
|
||||
@ -117,7 +119,7 @@ class CarController(CarControllerBase):
|
||||
idx = self.lka_steering_cmd_counter % 4
|
||||
can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, CC.latActive))
|
||||
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
if self.CP.openpilotLongitudinalControl and not frogpilot_variables.CSLC:
|
||||
# Gas/regen, brakes, and UI commands - all at 25Hz
|
||||
if self.frame % 4 == 0:
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
@ -244,6 +246,15 @@ class CarController(CarControllerBase):
|
||||
else:
|
||||
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL))
|
||||
|
||||
# ACC Spam
|
||||
if (self.CP.carFingerprint in SDGM_CAR or self.CP.carFingerprint in CAMERA_ACC_CAR) and frogpilot_variables.CSLC:
|
||||
bus = CanBus.CAMERA
|
||||
if CC.enabled and self.frame % 3 == 0 and CS.cruise_buttons == CruiseButtons.UNPRESS and not CS.out.gasPressed and not CS.distance_button:
|
||||
slcSet = get_set_speed(self, hud_v_cruise)
|
||||
can_sends.extend(gmcan.create_gm_acc_spam_command(self.packer_pt, self, CS, slcSet, bus, CS.out.vEgo, frogpilot_variables, accel, self.CP.carFingerprint in SDGM_CAR))
|
||||
elif CC.enabled and self.frame % 51 == 0 and CS.cruise_buttons == CruiseButtons.UNPRESS and CS.out.gasPressed and CS.out.cruiseState.speed < CS.out.vEgo < hud_v_cruise and not CS.distance_button:
|
||||
can_sends.extend([gmcan.create_buttons(self.packer_pt, bus, (CS.buttons_counter + 1) % 4, CruiseButtons.DECEL_SET)] * (25 if self.CP.carFingerprint in SDGM_CAR else 1))
|
||||
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
# Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1
|
||||
if self.frame % 10 == 0:
|
||||
@ -259,3 +270,13 @@ class CarController(CarControllerBase):
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
def get_set_speed(self, hud_v_cruise):
|
||||
v_cruise = min(hud_v_cruise, V_CRUISE_MAX * CV.KPH_TO_MS)
|
||||
|
||||
v_cruise_slc: float = 0.
|
||||
v_cruise_slc = params_memory.get_float("CSLCSpeed")
|
||||
|
||||
if v_cruise_slc > 0.:
|
||||
v_cruise = v_cruise_slc
|
||||
return v_cruise
|
||||
|
||||
@ -13,6 +13,7 @@ def create_buttons(packer, bus, idx, button):
|
||||
"RollingCounter": idx,
|
||||
"ACCAlwaysOne": 1,
|
||||
"DistanceButton": 0,
|
||||
"ACCByFive": 0,
|
||||
}
|
||||
|
||||
checksum = 240 + int(values["ACCAlwaysOne"] * 0xf)
|
||||
@ -23,6 +24,37 @@ def create_buttons(packer, bus, idx, button):
|
||||
values["SteeringButtonChecksum"] = checksum
|
||||
return packer.make_can_msg("ASCMSteeringButton", bus, values)
|
||||
|
||||
def create_buttons_five(packer, bus, idx, button, byfive):
|
||||
if byfive and button == CruiseButtons.RES_ACCEL:
|
||||
accbyfive = 2
|
||||
ACCAlwaysOne = 1
|
||||
checkoffset = -4
|
||||
elif byfive and button == CruiseButtons.DECEL_SET:
|
||||
accbyfive = 3
|
||||
ACCAlwaysOne = 0
|
||||
checkoffset = 255*idx+10
|
||||
else:
|
||||
accbyfive = 0
|
||||
ACCAlwaysOne = 1
|
||||
checkoffset = 0
|
||||
|
||||
values = {
|
||||
"ACCButtons": button,
|
||||
"RollingCounter": idx,
|
||||
"ACCAlwaysOne": ACCAlwaysOne,
|
||||
"DistanceButton": 0,
|
||||
"ACCByFive": accbyfive,
|
||||
}
|
||||
|
||||
checksum = 240 + int(values["ACCAlwaysOne"] * 0xf)
|
||||
checksum += values["RollingCounter"] * (0x4ef if values["ACCAlwaysOne"] != 0 else 0x3f0)
|
||||
checksum -= int(values["ACCButtons"] - 1) << 4 # not correct if value is 0
|
||||
checksum -= 2 * values["DistanceButton"]
|
||||
checksum += checkoffset
|
||||
|
||||
values["SteeringButtonChecksum"] = checksum
|
||||
return packer.make_can_msg("ASCMSteeringButton", bus, values)
|
||||
|
||||
|
||||
def create_pscm_status(packer, bus, pscm_status):
|
||||
values = {s: pscm_status[s] for s in [
|
||||
@ -222,3 +254,44 @@ def create_gm_cc_spam_command(packer, controller, CS, actuators):
|
||||
return [create_buttons(packer, CanBus.POWERTRAIN, idx, cruiseBtn)]
|
||||
else:
|
||||
return []
|
||||
|
||||
def create_gm_acc_spam_command(packer, controller, CS, slcSet, bus, Vego, frogpilot_variables, accel, sdgm):
|
||||
cruiseBtn = CruiseButtons.INIT
|
||||
byfive = 0
|
||||
|
||||
MS_CONVERT = CV.MS_TO_KPH if frogpilot_variables.is_metric else CV.MS_TO_MPH
|
||||
|
||||
speedSetPoint = int(round(CS.out.cruiseState.speed * MS_CONVERT))
|
||||
slcSet = int(round(slcSet * MS_CONVERT))
|
||||
|
||||
FRAMES_ON = 6
|
||||
FRAMES_OFF = (30 if sdgm else 12) - FRAMES_ON
|
||||
|
||||
if not frogpilot_variables.experimentalMode:
|
||||
if slcSet + 5 < Vego * MS_CONVERT:
|
||||
slcSet = slcSet - 10 # 10 lower to increase deceleration until with 5
|
||||
else:
|
||||
slcSet = int(round((Vego * 1.01 + 4.6 * accel + 0.7 * accel ** 3 - 1 / 35 * accel ** 5) * MS_CONVERT)) # 1.01 factor to match cluster speed better
|
||||
|
||||
if slcSet <= int(math.floor((speedSetPoint - 1)/5.0)*5.0) and speedSetPoint > (25 if frogpilot_variables.is_metric else 20) and sdgm:
|
||||
cruiseBtn = CruiseButtons.DECEL_SET
|
||||
byfive = 1
|
||||
elif slcSet >= int(math.ceil((speedSetPoint + 1)/5.0)*5.0) and sdgm:
|
||||
cruiseBtn = CruiseButtons.RES_ACCEL
|
||||
byfive = 1
|
||||
elif slcSet < speedSetPoint and speedSetPoint > (25 if frogpilot_variables.is_metric else 16):
|
||||
cruiseBtn = CruiseButtons.DECEL_SET
|
||||
byfive = 0
|
||||
elif slcSet > speedSetPoint:
|
||||
cruiseBtn = CruiseButtons.RES_ACCEL
|
||||
byfive = 0
|
||||
else:
|
||||
cruiseBtn = CruiseButtons.INIT
|
||||
byfive = 0
|
||||
|
||||
if (cruiseBtn != CruiseButtons.INIT) and controller.frame % (FRAMES_ON + FRAMES_OFF) < FRAMES_ON:
|
||||
controller.last_button_frame = controller.frame
|
||||
idx = (CS.buttons_counter + (1 if sdgm else 2)) % 4
|
||||
return [create_buttons_five(packer, bus, idx, cruiseBtn, byfive)]*(23 if sdgm else 3)
|
||||
else:
|
||||
return []
|
||||
|
||||
@ -12,6 +12,10 @@ from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerP
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs, NanoFFModel
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import get_friction
|
||||
|
||||
from openpilot.common.params import Params
|
||||
|
||||
params = Params()
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
EventName = car.CarEvent.EventName
|
||||
GearShifter = car.CarState.GearShifter
|
||||
@ -95,6 +99,8 @@ class CarInterface(CarInterfaceBase):
|
||||
def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs):
|
||||
params.put_bool("HideDisableOpenpilotLongitudinal", candidate not in (SDGM_CAR | CAMERA_ACC_CAR))
|
||||
|
||||
cslcEnabled = params.get_bool("CSLCEnabled")
|
||||
|
||||
ret.carName = "gm"
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)]
|
||||
ret.autoResumeSng = False
|
||||
@ -134,11 +140,25 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.longitudinalTuning.kpV = [1.5, 1.125]
|
||||
ret.stopAccel = -0.25
|
||||
|
||||
if experimental_long:
|
||||
if experimental_long and not cslcEnabled:
|
||||
ret.pcmCruise = False
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG
|
||||
|
||||
if cslcEnabled:
|
||||
# Used for CEM with CSLC
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
ret.longitudinalTuning.deadzoneV = [0.9] # == 2 mph allowable delta
|
||||
ret.stoppingDecelRate = 3.25 # == 8.33 mph/s (OFF + ON = 12 frames)
|
||||
ret.longitudinalActuatorDelayLowerBound = 1.
|
||||
ret.longitudinalActuatorDelayUpperBound = 2.
|
||||
|
||||
ret.longitudinalTuning.kpBP = [7.15, 7.2, 28.] # 7.15 m/s == 16 mph
|
||||
ret.longitudinalTuning.kpV = [0., 4., 2.] # set lower end to 0 since we can't drive below that speed
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
ret.longitudinalTuning.kiV = [0.1]
|
||||
|
||||
elif candidate in SDGM_CAR:
|
||||
ret.experimentalLongitudinalAvailable = False
|
||||
ret.networkLocation = NetworkLocation.fwdCamera
|
||||
@ -148,6 +168,19 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.minSteerSpeed = 30 * CV.MPH_TO_MS
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_SDGM
|
||||
|
||||
# Used for CEM with CSLC
|
||||
ret.openpilotLongitudinalControl = cslcEnabled
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
ret.longitudinalTuning.deadzoneV = [0.9] # == 2 mph allowable delta
|
||||
ret.stoppingDecelRate = 7.45 # == 16.67 mph/s (OFF + ON = 30 frames)
|
||||
ret.longitudinalActuatorDelayLowerBound = 1.
|
||||
ret.longitudinalActuatorDelayUpperBound = 2.
|
||||
|
||||
ret.longitudinalTuning.kpBP = [7.15, 7.2, 28.] # 7.15 m/s == 16 mph
|
||||
ret.longitudinalTuning.kpV = [0., 4., 2.] # set lower end to 0 since we can't drive below that speed
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
ret.longitudinalTuning.kiV = [0.1]
|
||||
|
||||
else: # ASCM, OBD-II harness
|
||||
ret.openpilotLongitudinalControl = not disable_openpilot_long
|
||||
ret.networkLocation = NetworkLocation.gateway
|
||||
@ -307,6 +340,9 @@ class CarInterface(CarInterfaceBase):
|
||||
if ACCELERATOR_POS_MSG not in fingerprint[CanBus.POWERTRAIN]:
|
||||
ret.flags |= GMFlags.NO_ACCELERATOR_POS_MSG.value
|
||||
|
||||
if cslcEnabled:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_CSLC
|
||||
|
||||
return ret
|
||||
|
||||
# returns a car.CarState
|
||||
|
||||
@ -501,6 +501,13 @@ class Controls:
|
||||
def state_transition(self, CS):
|
||||
"""Compute conditional state transitions and execute actions on state transitions"""
|
||||
|
||||
resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
|
||||
set_pressed = any(be.type in (ButtonType.decelCruise, ButtonType.setCruise) for be in CS.buttonEvents)
|
||||
if resume_pressed:
|
||||
self.frogpilot_toggles.prev_button = ButtonType.resumeCruise
|
||||
elif set_pressed:
|
||||
self.frogpilot_toggles.prev_button = ButtonType.setCruise
|
||||
|
||||
self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.FPCC.speedLimitChanged, self.frogpilot_toggles)
|
||||
|
||||
# decrement the soft disable timer at every step, as it's reset on
|
||||
@ -855,6 +862,7 @@ class Controls:
|
||||
controlsState.canErrorCounter = self.card.can_rcv_cum_timeout_counter
|
||||
controlsState.experimentalMode = self.experimental_mode
|
||||
controlsState.personality = self.personality
|
||||
self.frogpilot_toggles.experimentalMode = self.experimental_mode
|
||||
|
||||
lat_tuning = self.CP.lateralTuning.which()
|
||||
if self.joystick_mode:
|
||||
|
||||
@ -57,7 +57,7 @@ class VCruiseHelper:
|
||||
self.v_cruise_kph_last = self.v_cruise_kph
|
||||
|
||||
if CS.cruiseState.available:
|
||||
if not self.CP.pcmCruise:
|
||||
if not self.CP.pcmCruise or frogpilot_variables.CSLC:
|
||||
# if stock cruise is completely disabled, then we can use our own set speed logic
|
||||
self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, frogpilot_variables)
|
||||
self.v_cruise_cluster_kph = self.v_cruise_kph
|
||||
@ -148,11 +148,26 @@ class VCruiseHelper:
|
||||
|
||||
def initialize_v_cruise(self, CS, experimental_mode: bool, desired_speed_limit, frogpilot_variables) -> None:
|
||||
# initializing is handled by the PCM
|
||||
if self.CP.pcmCruise:
|
||||
if self.CP.pcmCruise and not frogpilot_variables.CSLC:
|
||||
return
|
||||
|
||||
initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL
|
||||
|
||||
# CSLC resume/set logic
|
||||
if frogpilot_variables.CSLC:
|
||||
if frogpilot_variables.prev_button == ButtonType.resumeCruise and self.v_cruise_kph_last < 250:
|
||||
self.v_cruise_kph = self.v_cruise_kph_last
|
||||
else:
|
||||
# Initial set speed
|
||||
if desired_speed_limit != 0 and frogpilot_variables.set_speed_limit:
|
||||
# If there's a known speed limit and the corresponding FP toggle is set, push it to the car
|
||||
self.v_cruise_kph = int(round(desired_speed_limit * CV.MS_TO_KPH))
|
||||
else:
|
||||
# Use fixed initial set speed from mode etc.
|
||||
self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
|
||||
self.v_cruise_cluster_kph = self.v_cruise_kph
|
||||
return
|
||||
|
||||
# 250kph or above probably means we never had a set speed
|
||||
if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250:
|
||||
self.v_cruise_kph = self.v_cruise_kph_last
|
||||
|
||||
@ -173,6 +173,8 @@ class FrogPilotPlanner:
|
||||
self.road_curvature = calculate_road_curvature(modelData, v_ego)
|
||||
self.v_cruise = self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, v_cruise, v_ego, frogpilot_toggles)
|
||||
|
||||
self.params_memory.put_float("CSLCSpeed", self.v_cruise)
|
||||
|
||||
if frogpilot_toggles.conditional_experimental_mode or frogpilot_toggles.green_light_alert:
|
||||
self.cem.update(carState, controlsState.enabled, frogpilotNavigation, lead_distance, self.lead_one, modelData, self.road_curvature, self.slower_lead, v_ego, v_lead, frogpilot_toggles)
|
||||
|
||||
@ -212,6 +214,9 @@ class FrogPilotPlanner:
|
||||
v_cruise_diff = v_cruise_cluster - v_cruise
|
||||
|
||||
v_ego_cluster = max(carState.vEgoCluster, v_ego)
|
||||
if frogpilot_toggles.CSLC:
|
||||
v_ego_diff = 0
|
||||
else:
|
||||
v_ego_diff = v_ego_cluster - v_ego
|
||||
|
||||
# Pfeiferj's Map Turn Speed Controller
|
||||
|
||||
@ -39,14 +39,16 @@ class FrogPilotVariables:
|
||||
|
||||
if msg_bytes is None:
|
||||
car_name = "mock"
|
||||
toggles.CSLC = False
|
||||
openpilot_longitudinal = False
|
||||
pcm_cruise = False
|
||||
else:
|
||||
with car.CarParams.from_bytes(msg_bytes) as msg:
|
||||
CP = msg
|
||||
car_name = CP.carName
|
||||
toggles.CSLC = self.params.get_bool("CSLCEnabled")
|
||||
openpilot_longitudinal = CP.openpilotLongitudinalControl
|
||||
pcm_cruise = CP.pcmCruise
|
||||
pcm_cruise = CP.pcmCruise and not toggles.CSLC
|
||||
|
||||
toggles.is_metric = self.params.get_bool("IsMetric")
|
||||
distance_conversion = 1 if toggles.is_metric else CV.FOOT_TO_METER
|
||||
|
||||
@ -1011,8 +1011,8 @@ void FrogPilotControlsPanel::updateCarToggles() {
|
||||
hasCommaNNFFSupport = checkCommaNNFFSupport(carFingerprint);
|
||||
hasDashSpeedLimits = carName == "hyundai" || carName == "toyota";
|
||||
hasNNFFLog = checkNNFFLogFileExists(carFingerprint);
|
||||
hasOpenpilotLongitudinal = CP.getOpenpilotLongitudinalControl() && !params.getBool("DisableOpenpilotLongitudinal");
|
||||
hasPCMCruise = CP.getPcmCruise();
|
||||
hasOpenpilotLongitudinal = (CP.getOpenpilotLongitudinalControl() && !params.getBool("DisableOpenpilotLongitudinal")) || params.getBool("CSLCEnabled");
|
||||
hasPCMCruise = CP.getPcmCruise() && !params.getBool("CSLCEnabled");
|
||||
isGM = carName == "gm";
|
||||
isHKGCanFd = (carName == "hyundai") && (safetyModel == cereal::CarParams::SafetyModel::HYUNDAI_CANFD);
|
||||
isToyota = carName == "toyota";
|
||||
|
||||
@ -113,6 +113,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil
|
||||
std::vector<std::tuple<QString, QString, QString, QString>> vehicleToggles {
|
||||
{"LongPitch", tr("Long Pitch Compensation"), tr("Smoothen out the gas and pedal controls."), ""},
|
||||
{"GasRegenCmd", tr("Truck Tune"), tr("Increase the acceleration and smoothen out the brake control when coming to a stop. For use on Silverado/Sierra only."), ""},
|
||||
{"CSLCEnabled", tr("GM CSLC"), tr("Set cars cruise speed based on SLC, MTSC, VTSC, & CEM.\n\nTurns OpenPilot Longitudnal Control off for camera ACC cars."), ""},
|
||||
|
||||
{"CrosstrekTorque", tr("Subaru Crosstrek Torque Increase"), tr("Increases the maximum allowed torque for the Subaru Crosstrek."), ""},
|
||||
|
||||
@ -165,7 +166,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil
|
||||
});
|
||||
}
|
||||
|
||||
std::set<QString> rebootKeys = {"CrosstrekTorque", "GasRegenCmd"};
|
||||
std::set<QString> rebootKeys = {"CrosstrekTorque", "GasRegenCmd", "CSLCEnabled"};
|
||||
for (const QString &key : rebootKeys) {
|
||||
QObject::connect(static_cast<ToggleControl*>(toggles[key.toStdString().c_str()]), &ToggleControl::toggleFlipped, [this]() {
|
||||
if (started) {
|
||||
|
||||
@ -29,7 +29,7 @@ private:
|
||||
|
||||
QStringList models;
|
||||
|
||||
std::set<QString> gmKeys = {"GasRegenCmd", "LongPitch"};
|
||||
std::set<QString> gmKeys = {"CSLCEnabled", "GasRegenCmd", "LongPitch"};
|
||||
std::set<QString> subaruKeys = {"CrosstrekTorque"};
|
||||
std::set<QString> toyotaKeys = {"ClusterOffset", "ToyotaTune", "SNGHack", "ToyotaDoors"};
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user