mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-14 10:14:01 +08:00
firestar5683
1368 lines
62 KiB
Python
1368 lines
62 KiB
Python
import math
|
|
import numpy as np
|
|
from opendbc.can import CANPacker
|
|
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, create_gas_interceptor_command, structs
|
|
from opendbc.car.lateral import apply_driver_steer_torque_limits
|
|
from opendbc.car.gm import gmcan
|
|
from opendbc.car.common.conversions import Conversions as CV
|
|
from opendbc.car.gm.values import (
|
|
ASCM_INT, CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, CC_REGEN_PADDLE_CAR, DBC, EV_CAR, SDGM_CAR, AccState, CanBus, CarControllerParams,
|
|
CruiseButtons, GMFlags, GMSafetyFlags,
|
|
)
|
|
from opendbc.car.interfaces import CarControllerBase
|
|
from openpilot.common.pid import PIDController
|
|
from openpilot.common.params import Params, UnknownKeyName
|
|
from openpilot.starpilot.common.testing_grounds import testing_ground
|
|
|
|
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
|
NetworkLocation = structs.CarParams.NetworkLocation
|
|
TransmissionType = structs.CarParams.TransmissionType
|
|
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
|
GearShifter = structs.CarState.GearShifter
|
|
|
|
# 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
|
|
MIN_STEER_MSG_INTERVAL_MS = 15
|
|
AUTO_HOLD_VOLT_CARS = {
|
|
CAR.CHEVROLET_VOLT,
|
|
CAR.CHEVROLET_VOLT_2019,
|
|
CAR.CHEVROLET_VOLT_ASCM,
|
|
CAR.CHEVROLET_VOLT_CAMERA,
|
|
}
|
|
AUTO_HOLD_DRIVE_GEARS = (
|
|
GearShifter.drive,
|
|
GearShifter.low,
|
|
GearShifter.manumatic,
|
|
)
|
|
AUTO_HOLD_MIN_BRAKE = 80
|
|
AUTO_HOLD_MAX_BRAKE = 240
|
|
AUTO_HOLD_MIN_DRIVE_TIME_S = 3.0
|
|
AUTO_HOLD_STOPPED_SPEED = 0.02
|
|
AUTO_HOLD_2019_MIN_BRAKE = 100
|
|
BOLT_ACC_PEDAL_FRICTION_RELEASE_FRAMES = 5
|
|
BOLT_PEDAL_LONG_ACCEL_LIMIT_BP = [0.0, 1.5, 4.0, 8.0, 15.0, 30.0]
|
|
BOLT_PEDAL_LONG_ACCEL_LIMIT_V = [-0.93, -1.28, -1.98, -2.58, -2.86, -2.95]
|
|
VOLT_ONE_PEDAL_DECEL_BP = [0.5 * CV.MPH_TO_MS, 6.0 * CV.MPH_TO_MS]
|
|
VOLT_ONE_PEDAL_DECEL_V = [-1.0, -1.1]
|
|
VOLT_ONE_PEDAL_REGEN_PADDLE_DECEL_V = [-1.5, -1.6]
|
|
VOLT_ONE_PEDAL_MAX_DECEL = min((*VOLT_ONE_PEDAL_DECEL_V, *VOLT_ONE_PEDAL_REGEN_PADDLE_DECEL_V)) - 0.5
|
|
VOLT_ONE_PEDAL_PID_NEG_LIMIT = -3.5
|
|
VOLT_ONE_PEDAL_SPEED_ERROR_FACTOR_BP = [1.5, 20.0]
|
|
VOLT_ONE_PEDAL_SPEED_ERROR_FACTOR_V = [0.4, 0.2]
|
|
VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_SPEED_FACTOR_BP = [0.0, 10.0 * CV.MPH_TO_MS]
|
|
VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_SPEED_FACTOR_V = [0.2, 1.0]
|
|
VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_STEER_FACTOR_BP = [20.0, 120.0]
|
|
VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_STEER_FACTOR_V = [1.0, 0.2]
|
|
VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_UP = 0.8 * DT_CTRL * 4
|
|
VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_DOWN = 0.8 * DT_CTRL * 4
|
|
VOLT_ONE_PEDAL_ACCEL_PITCH_FACTOR_BP = [4.0, 8.0]
|
|
VOLT_ONE_PEDAL_ACCEL_PITCH_FACTOR_V = [0.4, 1.0]
|
|
VOLT_ONE_PEDAL_ACCEL_PITCH_FACTOR_INCLINE_V = [0.2, 1.0]
|
|
VOLT_ONE_PEDAL_LIFT_BRAKE_BP = [0.0, CarControllerParams.NEAR_STOP_BRAKE_PHASE, 2.0 * CV.MPH_TO_MS]
|
|
VOLT_ONE_PEDAL_LIFT_BRAKE_V = [AUTO_HOLD_MIN_BRAKE, AUTO_HOLD_MIN_BRAKE, 20.0]
|
|
VOLT_ONE_PEDAL_LIFT_BRAKE_FRAMES = 8
|
|
TRUCK_LONG_SMOOTH_CARS = {
|
|
CAR.CHEVROLET_SILVERADO,
|
|
CAR.CHEVROLET_SILVERADO_CC,
|
|
}
|
|
TRUCK_FRICTION_BRAKE_ENGAGE = 40
|
|
TRUCK_FRICTION_BRAKE_RELEASE = 8
|
|
TRUCK_FRICTION_BRAKE_IMMEDIATE_ACCEL = -0.85
|
|
TRUCK_FOLLOW_MICRO_ACCEL_MAX = 0.55
|
|
TRUCK_FOLLOW_MICRO_ACCEL_MIN = -0.10
|
|
TRUCK_FOLLOW_MICRO_ACCEL_SLEW = 1.5
|
|
ACC_DASHBOARD_ZERO_RESERVED_CARS = {
|
|
CAR.CHEVROLET_BLAZER,
|
|
CAR.CHEVROLET_EQUINOX,
|
|
CAR.CHEVROLET_SILVERADO,
|
|
CAR.CHEVROLET_TRAILBLAZER,
|
|
CAR.CHEVROLET_TRAX,
|
|
}
|
|
|
|
|
|
def get_stock_cc_active_for_cancel(CP, CS):
|
|
stock_cc_active = CS.out.cruiseState.enabled or CS.pcm_acc_status != AccState.OFF
|
|
if CP.carFingerprint == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL:
|
|
return CS.out.cruiseState.enabled
|
|
return stock_cc_active
|
|
|
|
|
|
def use_interceptor_sng_launch(CP, CS, maneuver_mode=False):
|
|
# Restrict the fixed standstill-launch gas to actual near-zero motion
|
|
# so higher accel requests can take over once the car has started moving.
|
|
launch_speed = max(CP.vEgoStarting, 0.3)
|
|
if maneuver_mode:
|
|
launch_speed = max(launch_speed, 2.0)
|
|
near_stop = CS.out.standstill or CS.out.vEgo < launch_speed
|
|
if (
|
|
getattr(CP, "carFingerprint", None) == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL and
|
|
getattr(CP, "enableGasInterceptorDEPRECATED", False)
|
|
):
|
|
return near_stop
|
|
return CS.out.cruiseState.standstill and near_stop
|
|
|
|
|
|
def should_spoof_dash_speed(CP, starpilot_toggles):
|
|
if not CP.openpilotLongitudinalControl:
|
|
return False
|
|
|
|
# Respect the current StarPilot stock-ACC toggles even before CarParams are
|
|
# rebuilt on reboot, so the cluster speed spoof doesn't leak in stock mode.
|
|
if getattr(starpilot_toggles, "disable_openpilot_long", False):
|
|
return False
|
|
if CP.enableGasInterceptorDEPRECATED and not getattr(starpilot_toggles, "gm_pedal_longitudinal", True):
|
|
return False
|
|
|
|
return True
|
|
|
|
|
|
def should_send_acc_dashboard_status(CP, dash_speed_spoof_active):
|
|
status_car = CP.carFingerprint not in CC_ONLY_CAR or CP.carFingerprint == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL
|
|
volt_camera_no_camera = (
|
|
CP.carFingerprint == CAR.CHEVROLET_VOLT_CAMERA and
|
|
bool(getattr(CP, "flags", 0) & GMFlags.NO_CAMERA.value)
|
|
)
|
|
return status_car and (dash_speed_spoof_active or volt_camera_no_camera)
|
|
|
|
|
|
def get_acc_dashboard_status_active(CP, CC):
|
|
if CC.enabled:
|
|
return True
|
|
|
|
return CP.carFingerprint == CAR.BUICK_LACROSSE_ASCM and CC.latActive
|
|
|
|
|
|
def get_acc_dashboard_always_one(CP):
|
|
return 0 if CP.carFingerprint in ACC_DASHBOARD_ZERO_RESERVED_CARS else 1
|
|
|
|
|
|
def get_acc_dashboard_fcw_alert(hud_alert, CS):
|
|
if hud_alert == VisualAlert.fcw:
|
|
return 0x3
|
|
|
|
stock_fcw_alert = int(getattr(CS, "stock_fcw_alert", 0)) & 0x3
|
|
if stock_fcw_alert != 0:
|
|
return stock_fcw_alert
|
|
|
|
cs_out = getattr(CS, "out", None)
|
|
if cs_out is not None and (getattr(cs_out, "stockAeb", False) or getattr(cs_out, "stockFcw", False)):
|
|
return 0x3
|
|
|
|
return 0
|
|
|
|
|
|
ECM_CRUISE_SPOOF_CARS = {
|
|
CAR.CHEVROLET_BOLT_CC_2017,
|
|
CAR.CHEVROLET_BOLT_CC_2018_2021,
|
|
CAR.CHEVROLET_BOLT_CC_2022_2023,
|
|
CAR.CHEVROLET_MALIBU_HYBRID_CC,
|
|
}
|
|
|
|
|
|
def should_spoof_ecm_cruise_status(CP):
|
|
return (
|
|
bool(CP.flags & GMFlags.PEDAL_LONG.value) and
|
|
CP.carFingerprint in ECM_CRUISE_SPOOF_CARS and
|
|
CP.enableGasInterceptorDEPRECATED
|
|
)
|
|
|
|
|
|
def should_send_cc_button_spam(CP, CC, CS):
|
|
return (
|
|
bool(CP.flags & GMFlags.CC_LONG.value) and
|
|
CC.longActive and
|
|
CS.out.vEgo > CP.minEnableSpeed
|
|
)
|
|
|
|
|
|
def get_adas_keepalive_step(CP, is_kaofui_car):
|
|
if CP.networkLocation == NetworkLocation.gateway:
|
|
base_step = CarControllerParams.ADAS_KEEPALIVE_STEP
|
|
return base_step if is_kaofui_car else base_step * 2
|
|
|
|
if CP.networkLocation == NetworkLocation.fwdCamera and bool(getattr(CP, "flags", 0) & GMFlags.NO_CAMERA.value):
|
|
return CarControllerParams.CAMERA_KEEPALIVE_STEP
|
|
|
|
return None
|
|
|
|
|
|
def should_send_adas_status(CP, is_kaofui_car):
|
|
if CP.radarUnavailable:
|
|
return False
|
|
|
|
if not is_kaofui_car:
|
|
return True
|
|
|
|
if CP.carFingerprint in ASCM_INT:
|
|
return False
|
|
|
|
return CP.networkLocation != NetworkLocation.fwdCamera and CP.carFingerprint not in SDGM_CAR
|
|
|
|
|
|
def should_send_acc_2cd(CP):
|
|
return (
|
|
CP.networkLocation == NetworkLocation.fwdCamera and
|
|
CP.carFingerprint in CAMERA_ACC_CAR and
|
|
CP.carFingerprint not in (CC_ONLY_CAR | SDGM_CAR) and
|
|
not bool(getattr(CP, "flags", 0) & GMFlags.NO_CAMERA.value)
|
|
)
|
|
|
|
|
|
def get_testing_ground_1_brake_switch_bias(v_ego: float) -> int:
|
|
return int(round(np.interp(v_ego, [0.0, 6.0, 15.0, 30.0], [40.0, 85.0, 130.0, 170.0])))
|
|
|
|
|
|
def shape_truck_positive_accel(accel: float, v_ego: float, enabled: bool,
|
|
lead_visible: bool = False, set_speed_error: float = 0.0) -> float:
|
|
if not enabled or accel <= 0.0 or v_ego < 12.0:
|
|
return accel
|
|
|
|
low_scale = float(np.interp(v_ego, [12.0, 18.0, 25.0, 35.0], [0.93, 0.84, 0.76, 0.76]))
|
|
mid_scale = float(np.interp(v_ego, [12.0, 18.0, 25.0, 35.0], [0.97, 0.91, 0.85, 0.85]))
|
|
|
|
if lead_visible and set_speed_error > 0.0:
|
|
follow_relief = float(np.interp(set_speed_error, [0.0, 1.0, 2.5, 4.0, 6.0], [0.0, 0.04, 0.10, 0.24, 0.42]))
|
|
low_scale += (1.0 - low_scale) * follow_relief
|
|
mid_scale += (1.0 - mid_scale) * follow_relief
|
|
|
|
if accel <= 0.12:
|
|
return accel * low_scale
|
|
if accel <= 0.35:
|
|
return float(np.interp(accel, [0.12, 0.35], [0.12 * low_scale, 0.35 * mid_scale]))
|
|
if accel <= 0.65:
|
|
return float(np.interp(accel, [0.35, 0.65], [0.35 * mid_scale, 0.65]))
|
|
return accel
|
|
|
|
|
|
def smooth_truck_follow_accel(accel: float, previous_accel: float, v_ego: float,
|
|
enabled: bool, lead_visible: bool, stopping: bool) -> float:
|
|
if (
|
|
not enabled or not lead_visible or stopping or v_ego < 25.0 or
|
|
accel > TRUCK_FOLLOW_MICRO_ACCEL_MAX or accel < TRUCK_FOLLOW_MICRO_ACCEL_MIN or
|
|
previous_accel > TRUCK_FOLLOW_MICRO_ACCEL_MAX or previous_accel < TRUCK_FOLLOW_MICRO_ACCEL_MIN
|
|
):
|
|
return accel
|
|
|
|
max_delta = TRUCK_FOLLOW_MICRO_ACCEL_SLEW * DT_CTRL * 4
|
|
return previous_accel + float(np.clip(accel - previous_accel, -max_delta, max_delta))
|
|
|
|
|
|
def shape_truck_pitch_accel(pitch_accel: float, v_ego: float, enabled: bool) -> float:
|
|
if not enabled:
|
|
return pitch_accel
|
|
|
|
scale = float(np.interp(v_ego, [8.0, 15.0, 25.0, 35.0], [0.60, 0.45, 0.30, 0.25]))
|
|
return pitch_accel * scale
|
|
|
|
|
|
def shape_truck_friction_brake(apply_brake: int, accel_cmd: float, stopping: bool, active: bool) -> tuple[int, bool]:
|
|
if apply_brake <= 0:
|
|
return 0, False
|
|
|
|
# Preserve full brake response for stop control and meaningful deceleration.
|
|
if stopping or accel_cmd <= TRUCK_FRICTION_BRAKE_IMMEDIATE_ACCEL:
|
|
return apply_brake, True
|
|
|
|
if active:
|
|
if apply_brake <= TRUCK_FRICTION_BRAKE_RELEASE:
|
|
return 0, False
|
|
return apply_brake, True
|
|
|
|
if apply_brake >= TRUCK_FRICTION_BRAKE_ENGAGE:
|
|
return apply_brake, True
|
|
|
|
# Keep tiny corrections in the continuous gas/regen torque path. Switching
|
|
# to friction also forces max regen, which makes a small request perceptible.
|
|
return 0, False
|
|
|
|
|
|
def get_lka_steering_cmd_counter(next_counter: int, CS) -> int:
|
|
if getattr(CS, "loopback_lka_steering_cmd_updated", False):
|
|
return (getattr(CS, "loopback_lka_steering_cmd_counter", next_counter) + 1) % 4
|
|
if next_counter < 0 and getattr(CS, "loopback_lka_steering_cmd_ts_nanos", 0) == 0:
|
|
return (getattr(CS, "pt_lka_steering_cmd_counter", next_counter) + 1) % 4
|
|
return next_counter
|
|
|
|
|
|
def should_send_stock_long_cancel(cancel_counter: int, CS) -> bool:
|
|
cs_out = getattr(CS, "out", None)
|
|
return cancel_counter > CAMERA_CANCEL_DELAY_FRAMES and not bool(getattr(cs_out, "accFaulted", False))
|
|
|
|
|
|
def supports_volt_auto_hold(CP, auto_hold_enabled: bool):
|
|
safety_cfg = getattr(CP, "safetyConfigs", ())
|
|
safety_param = safety_cfg[0].safetyParam if safety_cfg else 0
|
|
stock_hold_safety_ready = bool(safety_param & GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED.value)
|
|
return (
|
|
auto_hold_enabled and
|
|
getattr(CP, "openpilotLongitudinalControl", False) and
|
|
stock_hold_safety_ready and
|
|
CP.carFingerprint in AUTO_HOLD_VOLT_CARS
|
|
)
|
|
|
|
|
|
def supports_volt_one_pedal(CP, one_pedal_enabled: bool):
|
|
safety_cfg = getattr(CP, "safetyConfigs", ())
|
|
safety_param = safety_cfg[0].safetyParam if safety_cfg else 0
|
|
stock_hold_safety_ready = bool(safety_param & GMSafetyFlags.FLAG_GM_PANDA_PADDLE_SCHED.value)
|
|
return (
|
|
one_pedal_enabled and
|
|
getattr(CP, "openpilotLongitudinalControl", False) and
|
|
stock_hold_safety_ready and
|
|
getattr(CP, "transmissionType", None) == TransmissionType.direct and
|
|
CP.carFingerprint in AUTO_HOLD_VOLT_CARS
|
|
)
|
|
|
|
|
|
def estimate_auto_hold_brake(driver_brake: float, op_brake: float, CP=None) -> int:
|
|
driver_hold = np.interp(float(driver_brake), [8.0, 20.0, 40.0, 80.0], [80.0, 110.0, 150.0, 220.0])
|
|
hold_brake = max(float(op_brake), float(driver_hold))
|
|
min_brake = AUTO_HOLD_2019_MIN_BRAKE if getattr(CP, "carFingerprint", None) == CAR.CHEVROLET_VOLT_2019 else AUTO_HOLD_MIN_BRAKE
|
|
return int(round(np.clip(hold_brake, min_brake, AUTO_HOLD_MAX_BRAKE)))
|
|
|
|
|
|
def get_auto_hold_stop_threshold(CP, auto_hold_engaged: bool) -> float:
|
|
if auto_hold_engaged and getattr(CP, "carFingerprint", None) == CAR.CHEVROLET_VOLT_2019:
|
|
return CarControllerParams.NEAR_STOP_BRAKE_PHASE
|
|
return AUTO_HOLD_STOPPED_SPEED
|
|
|
|
|
|
def get_volt_one_pedal_target_decel(v_ego: float) -> float:
|
|
return float(np.interp(v_ego, VOLT_ONE_PEDAL_DECEL_BP, VOLT_ONE_PEDAL_DECEL_V))
|
|
|
|
|
|
def get_volt_one_pedal_lift_brake(v_ego: float) -> int:
|
|
if v_ego > VOLT_ONE_PEDAL_LIFT_BRAKE_BP[-1]:
|
|
return 0
|
|
|
|
return int(round(np.interp(v_ego, VOLT_ONE_PEDAL_LIFT_BRAKE_BP, VOLT_ONE_PEDAL_LIFT_BRAKE_V)))
|
|
|
|
|
|
def should_activate_volt_one_pedal(one_pedal_ready: bool, cruise_main: bool, long_active: bool,
|
|
gas_pressed: bool, brake_pressed: bool, regen_braking: bool,
|
|
single_pedal_mode: bool, gear_shifter, drive_time_s: float) -> bool:
|
|
# Volt rear wheel direction bits can falsely report reverse while stopping in L.
|
|
return (
|
|
one_pedal_ready and
|
|
cruise_main and
|
|
single_pedal_mode and
|
|
gear_shifter in AUTO_HOLD_DRIVE_GEARS and
|
|
drive_time_s >= AUTO_HOLD_MIN_DRIVE_TIME_S and
|
|
not long_active and
|
|
not gas_pressed and
|
|
not brake_pressed and
|
|
not regen_braking
|
|
)
|
|
|
|
|
|
def should_activate_auto_hold(hold_ready: bool, auto_hold_armed: bool, auto_hold_engaged: bool,
|
|
brake_pressed: bool, gas_pressed: bool, standstill: bool, long_active: bool,
|
|
regen_braking: bool, v_ego: float, stop_speed_threshold: float=AUTO_HOLD_STOPPED_SPEED) -> bool:
|
|
stopped = standstill or v_ego < stop_speed_threshold
|
|
return (
|
|
hold_ready and
|
|
(auto_hold_armed or auto_hold_engaged or brake_pressed) and
|
|
not gas_pressed and
|
|
stopped and
|
|
not long_active and
|
|
not regen_braking
|
|
)
|
|
|
|
|
|
def get_friction_brake_bus(CP):
|
|
volt_gateway_alt_brake = (
|
|
CP.carFingerprint == CAR.CHEVROLET_VOLT and
|
|
CP.networkLocation == NetworkLocation.gateway and
|
|
bool(CP.flags & GMFlags.NO_ACCELERATOR_POS_MSG.value)
|
|
)
|
|
if volt_gateway_alt_brake:
|
|
return CanBus.POWERTRAIN
|
|
|
|
if CP.networkLocation == NetworkLocation.fwdCamera:
|
|
if CP.carFingerprint in SDGM_CAR:
|
|
# cam-long: 0x315 goes where the panda whitelist allows it and the EBCM hears it
|
|
safety_cfg = getattr(CP, "safetyConfigs", ())
|
|
safety_param = safety_cfg[0].safetyParam if safety_cfg else 0
|
|
if safety_param & GMSafetyFlags.HW_CAM_LONG.value:
|
|
# SASCM relays 0x315 to the EBCM off its camera-bus (bus2) leg; bare SDGM uses the pt bus
|
|
if CP.flags & GMFlags.SASCM.value:
|
|
return CanBus.CAMERA
|
|
return CanBus.POWERTRAIN
|
|
return CanBus.CAMERA
|
|
return CanBus.POWERTRAIN
|
|
|
|
return CanBus.CHASSIS
|
|
|
|
|
|
def supports_bolt_acc_pedal_friction_experiment(CP) -> bool:
|
|
return (
|
|
CP.carFingerprint == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL and
|
|
CP.openpilotLongitudinalControl and
|
|
CP.enableGasInterceptorDEPRECATED and
|
|
bool(CP.flags & GMFlags.PEDAL_LONG.value)
|
|
)
|
|
|
|
|
|
def get_bolt_acc_pedal_friction_brake(apply_brake, full_brake_accel, v_ego, params) -> int:
|
|
if apply_brake <= 0:
|
|
return 0
|
|
|
|
full_brake_accel = min(full_brake_accel, -0.1)
|
|
legacy_full_scale = max(-params.ACCEL_MIN, 0.1)
|
|
corrected_scale = legacy_full_scale / max(-full_brake_accel, 0.1)
|
|
speed_gain = float(np.interp(v_ego, [0.0, 8.0, 15.0, 25.0], [1.0, 1.08, 1.2, 1.35]))
|
|
onset_gain = float(np.interp(
|
|
apply_brake,
|
|
[0.0, 5.0, 20.0, 60.0, 120.0, 240.0, params.MAX_BRAKE],
|
|
[0.0, 1.8, 1.65, 1.4, 1.22, 1.08, 1.0],
|
|
))
|
|
|
|
shaped_brake = apply_brake * corrected_scale * speed_gain * onset_gain
|
|
minimum_brake = float(np.interp(v_ego, [0.0, 6.0, 8.0, 12.0, 18.0, 25.0], [0.0, 0.0, 4.0, 10.0, 20.0, 28.0]))
|
|
shaped_brake = max(shaped_brake, minimum_brake)
|
|
|
|
return int(round(np.clip(shaped_brake, 0, params.MAX_BRAKE)))
|
|
|
|
|
|
def shape_bolt_acc_pedal_low_speed_friction(apply_brake: int, v_ego: float, stopping: bool, active: bool):
|
|
if apply_brake <= 0:
|
|
return 0, False
|
|
|
|
engage_threshold = float(np.interp(v_ego, [0.0, 1.5, 3.0, 5.0, 8.0], [40.0, 20.0, 12.0, 10.0, 0.0]))
|
|
release_threshold = float(np.interp(v_ego, [0.0, 1.5, 3.0, 5.0, 8.0], [0.0, 8.0, 6.0, 4.0, 0.0]))
|
|
|
|
if not active:
|
|
if apply_brake < engage_threshold:
|
|
return 0, False
|
|
active = True
|
|
elif apply_brake < release_threshold:
|
|
return 0, False
|
|
|
|
if stopping:
|
|
stop_fade = float(np.interp(v_ego, [0.0, 0.6, 0.9, 1.2, 1.8, 2.8], [0.0, 0.0, 0.05, 0.12, 0.32, 0.78]))
|
|
apply_brake = int(round(apply_brake * stop_fade))
|
|
if apply_brake <= 0 or apply_brake < release_threshold:
|
|
return 0, False
|
|
|
|
return apply_brake, active
|
|
|
|
|
|
def get_bolt_pedal_long_accel_limit(v_ego: float) -> float:
|
|
return float(np.interp(v_ego, BOLT_PEDAL_LONG_ACCEL_LIMIT_BP, BOLT_PEDAL_LONG_ACCEL_LIMIT_V))
|
|
|
|
|
|
def get_bolt_acc_pedal_planner_brake_switch(v_ego: float, params, tire_radius: float, mass: float,
|
|
coeff_drag: float, frontal_area: float, air_density: float) -> int:
|
|
planner_accel_limit = get_bolt_pedal_long_accel_limit(v_ego)
|
|
aero_drag_force = 0.5 * coeff_drag * frontal_area * air_density * v_ego ** 2
|
|
planner_torque = tire_radius * ((mass * planner_accel_limit) + aero_drag_force)
|
|
return int(round(planner_torque + params.ZERO_GAS))
|
|
|
|
|
|
def get_bolt_acc_pedal_effective_brake_switch(stock_switch: int, planner_switch: int) -> int:
|
|
return max(stock_switch, planner_switch)
|
|
|
|
|
|
def get_bolt_acc_pedal_friction_command_state(apply_brake: int, cruise_main_on: bool, release_frames: int):
|
|
command_brake = apply_brake if cruise_main_on else 0
|
|
|
|
if command_brake > 0:
|
|
release_frames = BOLT_ACC_PEDAL_FRICTION_RELEASE_FRAMES
|
|
elif release_frames > 0:
|
|
release_frames -= 1
|
|
|
|
should_send = cruise_main_on or release_frames > 0
|
|
return command_brake, release_frames, should_send
|
|
|
|
|
|
def get_interceptor_sng_gas_cmd(CP, interceptor_gas_cmd: float, accel: float, params, maneuver_mode: bool) -> float:
|
|
if maneuver_mode:
|
|
return max(interceptor_gas_cmd, float(np.interp(accel, [0.0, 1.0, 2.0], [params.SNG_INTERCEPTOR_GAS, 0.11, 0.16])))
|
|
|
|
if supports_bolt_acc_pedal_friction_experiment(CP):
|
|
return max(interceptor_gas_cmd, params.SNG_INTERCEPTOR_GAS)
|
|
|
|
return params.SNG_INTERCEPTOR_GAS
|
|
|
|
|
|
def should_use_fixed_stopping_brake(CP, near_stop: bool, stopping: bool, resume: bool) -> bool:
|
|
if not (near_stop and stopping and not resume):
|
|
return False
|
|
|
|
return not supports_bolt_acc_pedal_friction_experiment(CP)
|
|
|
|
|
|
class CarController(CarControllerBase):
|
|
def __init__(self, dbc_names, CP):
|
|
super().__init__(dbc_names, CP)
|
|
self.start_time = 0.
|
|
self.apply_torque_last = 0
|
|
self.apply_gas = 0
|
|
self.apply_brake = 0
|
|
self.last_steer_frame = 0
|
|
self.last_button_frame = 0
|
|
self.cancel_counter = 0
|
|
self.xt4_cc_button_burst_remaining = 0
|
|
self.xt4_cc_button_burst_button = CruiseButtons.INIT
|
|
self.xt4_cc_button_burst_last_counter = -1
|
|
self.xt4_cc_button_observed_counter = -1
|
|
self.xt4_cc_button_counter_frame = 0
|
|
self.gm_cc_last_direction_button = CruiseButtons.INIT
|
|
self.gm_cc_last_direction_frame = 0
|
|
self.gm_cc_pending_reverse_button = CruiseButtons.INIT
|
|
self.gm_cc_pending_reverse_frame = 0
|
|
|
|
self.lka_steering_cmd_counter = 0
|
|
self.lka_icon_status_last = (False, False)
|
|
|
|
self.params = CarControllerParams(self.CP)
|
|
|
|
self.packer_pt = CANPacker(DBC[self.CP.carFingerprint][Bus.pt])
|
|
self.packer_obj = CANPacker(DBC[self.CP.carFingerprint][Bus.radar])
|
|
self.packer_ch = CANPacker(DBC[self.CP.carFingerprint][Bus.chassis])
|
|
|
|
self.apply_speed = 0
|
|
self.prev_op_enabled = False
|
|
self.pedal_steady = 0
|
|
self.regen_paddle_pressed = False
|
|
self.regen_paddle_timer = 0
|
|
self.regen_press_counter = 0
|
|
self.regen_release_counter = 0
|
|
self.regen_min_on_frames = 0
|
|
self.regen_min_off_frames = 0
|
|
self.planner_regen_hold = False
|
|
self.paddle_handoff_frames = 0
|
|
self.pedal_active_last = False
|
|
self.aego = 0.0
|
|
self.maneuver_paddle_mode = "auto"
|
|
self.longitudinal_maneuver_mode = False
|
|
self.params_ = Params()
|
|
|
|
self.is_volt = self.CP.carFingerprint in {
|
|
CAR.CHEVROLET_VOLT,
|
|
CAR.CHEVROLET_VOLT_2019,
|
|
CAR.CHEVROLET_VOLT_ASCM,
|
|
CAR.CHEVROLET_VOLT_CAMERA,
|
|
CAR.CHEVROLET_VOLT_CC,
|
|
}
|
|
self.mass = CP.mass
|
|
self.tireRadius = 0.075 * CP.wheelbase + 0.1453
|
|
self.frontalArea = 1.05 * CP.wheelbase + 0.0679
|
|
self.coeffDrag = 0.30
|
|
self.airDensity = 1.225
|
|
self.malibu_cancel_phase = 0
|
|
self.malibu_button_phase = 0
|
|
self.malibu_last_button_ts_nanos = 0
|
|
self.auto_hold_brake = 0
|
|
self.volt_one_pedal_pid = PIDController(
|
|
(CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
|
|
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
|
|
rate=1 / (DT_CTRL * 4),
|
|
pos_limit=0.0,
|
|
neg_limit=VOLT_ONE_PEDAL_PID_NEG_LIMIT,
|
|
)
|
|
self.volt_one_pedal_decel = 0.0
|
|
self.volt_one_pedal_brake = 0
|
|
self.volt_one_pedal_lift_frames = 0
|
|
self.volt_one_pedal_gas_pressed_last = False
|
|
try:
|
|
self.gm_auto_hold_enabled = self.params_.get_bool("GMAutoHold")
|
|
except UnknownKeyName:
|
|
self.gm_auto_hold_enabled = False
|
|
self.bolt_acc_pedal_friction_release_frames = 0
|
|
self.bolt_acc_pedal_friction_low_speed_active = False
|
|
self.truck_friction_brake_active = False
|
|
self.truck_follow_accel = 0.0
|
|
|
|
def _reset_volt_one_pedal(self):
|
|
self.volt_one_pedal_pid.reset()
|
|
self.volt_one_pedal_decel = min(0.0, float(self.aego))
|
|
self.volt_one_pedal_brake = 0
|
|
self.volt_one_pedal_lift_frames = 0
|
|
|
|
def _update_volt_one_pedal_brake(self, CC, CS):
|
|
pitch_accel = 0.0
|
|
if len(CC.orientationNED) == 3 and CS.out.vEgo > self.CP.vEgoStopping:
|
|
pitch_accel = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
|
|
pitch_factor_values = VOLT_ONE_PEDAL_ACCEL_PITCH_FACTOR_V if pitch_accel <= 0.0 else VOLT_ONE_PEDAL_ACCEL_PITCH_FACTOR_INCLINE_V
|
|
pitch_accel *= float(np.interp(CS.out.vEgo, VOLT_ONE_PEDAL_ACCEL_PITCH_FACTOR_BP, pitch_factor_values))
|
|
|
|
target_decel = get_volt_one_pedal_target_decel(CS.out.vEgo)
|
|
measured_decel = min(0.0, CS.out.aEgo + pitch_accel)
|
|
error_factor = float(np.interp(CS.out.vEgo, VOLT_ONE_PEDAL_SPEED_ERROR_FACTOR_BP, VOLT_ONE_PEDAL_SPEED_ERROR_FACTOR_V))
|
|
error = (target_decel - measured_decel) * error_factor
|
|
|
|
raw_decel = float(self.volt_one_pedal_pid.update(error, speed=CS.out.vEgo, feedforward=target_decel))
|
|
rate_limit_factor = min(
|
|
float(np.interp(CS.out.vEgo, VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_SPEED_FACTOR_BP, VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_SPEED_FACTOR_V)),
|
|
float(np.interp(abs(CS.out.steeringAngleDeg), VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_STEER_FACTOR_BP, VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_STEER_FACTOR_V)),
|
|
)
|
|
lower = min(self.volt_one_pedal_decel, measured_decel) - VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_UP * rate_limit_factor
|
|
upper = max(self.volt_one_pedal_decel, measured_decel) + VOLT_ONE_PEDAL_DECEL_RATE_LIMIT_DOWN + rate_limit_factor
|
|
self.volt_one_pedal_decel = float(np.clip(raw_decel, lower, upper))
|
|
self.volt_one_pedal_decel = max(self.volt_one_pedal_decel, VOLT_ONE_PEDAL_MAX_DECEL)
|
|
self.volt_one_pedal_brake = int(round(np.clip(
|
|
np.interp(self.volt_one_pedal_decel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V),
|
|
0,
|
|
self.params.MAX_BRAKE,
|
|
)))
|
|
if self.volt_one_pedal_lift_frames > 0:
|
|
self.volt_one_pedal_brake = max(self.volt_one_pedal_brake, get_volt_one_pedal_lift_brake(CS.out.vEgo))
|
|
self.volt_one_pedal_lift_frames -= 1
|
|
|
|
def calc_pedal_command(self, accel: float, long_active: bool, v_ego: float):
|
|
if not long_active:
|
|
self.planner_regen_hold = False
|
|
self.regen_paddle_pressed = False
|
|
self.regen_paddle_timer = 0
|
|
self.regen_press_counter = 0
|
|
self.regen_release_counter = 0
|
|
self.regen_min_on_frames = 0
|
|
self.regen_min_off_frames = 0
|
|
self.pedal_active_last = False
|
|
self.pedal_steady = 0.0
|
|
return 0., False
|
|
|
|
supports_regen_paddle = self.CP.carFingerprint in CC_REGEN_PADDLE_CAR
|
|
switched_state = False
|
|
press_regen_paddle = False
|
|
if supports_regen_paddle:
|
|
press_cmd_threshold = np.interp(v_ego, [0.0, 4.0, 12.0, 25.0], [-0.90, -0.82, -0.72, -0.65])
|
|
release_cmd_threshold = np.interp(v_ego, [0.0, 4.0, 12.0, 25.0], [-0.10, -0.17, -0.24, -0.30])
|
|
press_aego_threshold = np.interp(v_ego, [0.0, 4.0, 12.0, 25.0], [-0.95, -0.86, -0.76, -0.70])
|
|
release_aego_threshold = np.interp(v_ego, [0.0, 4.0, 12.0, 25.0], [-0.16, -0.23, -0.30, -0.36])
|
|
|
|
press_confirm_frames = int(round(np.interp(v_ego, [0.0, 4.0, 12.0, 25.0], [8.0, 6.0, 5.0, 4.0])))
|
|
release_confirm_frames = int(round(np.interp(v_ego, [0.0, 4.0, 12.0, 25.0], [18.0, 15.0, 12.0, 10.0])))
|
|
min_on_frames = int(round(np.interp(v_ego, [0.0, 4.0, 12.0, 25.0], [34.0, 27.0, 20.0, 16.0])))
|
|
min_off_frames = int(round(np.interp(v_ego, [0.0, 4.0, 12.0, 25.0], [16.0, 14.0, 12.0, 10.0])))
|
|
confirm_boost = int(round(np.interp(v_ego, [0.0, 6.0, 8.0, 20.0, 25.0], [0.0, 0.0, 3.0, 3.0, 0.0])))
|
|
press_confirm_frames += confirm_boost
|
|
release_confirm_frames += confirm_boost
|
|
|
|
want_press = self.planner_regen_hold or accel <= press_cmd_threshold or self.aego <= press_aego_threshold
|
|
want_release = (not self.planner_regen_hold) and accel >= release_cmd_threshold and self.aego >= release_aego_threshold
|
|
|
|
if want_press:
|
|
self.regen_press_counter += 1
|
|
else:
|
|
self.regen_press_counter = max(self.regen_press_counter - 1, 0)
|
|
|
|
if want_release:
|
|
self.regen_release_counter += 1
|
|
else:
|
|
self.regen_release_counter = max(self.regen_release_counter - 1, 0)
|
|
|
|
if self.planner_regen_hold and accel <= (press_cmd_threshold - 0.30):
|
|
self.regen_press_counter = max(self.regen_press_counter, press_confirm_frames)
|
|
|
|
if self.regen_min_on_frames > 0:
|
|
self.regen_min_on_frames -= 1
|
|
if self.regen_min_off_frames > 0:
|
|
self.regen_min_off_frames -= 1
|
|
|
|
if self.regen_paddle_pressed:
|
|
if self.regen_min_on_frames == 0 and self.regen_release_counter >= release_confirm_frames:
|
|
self.regen_paddle_pressed = False
|
|
self.regen_min_off_frames = min_off_frames
|
|
self.regen_release_counter = 0
|
|
switched_state = True
|
|
else:
|
|
if self.regen_min_off_frames == 0 and self.regen_press_counter >= press_confirm_frames:
|
|
self.regen_paddle_pressed = True
|
|
self.regen_min_on_frames = min_on_frames
|
|
self.regen_press_counter = 0
|
|
switched_state = True
|
|
|
|
self.regen_paddle_timer = self.regen_press_counter
|
|
press_regen_paddle = self.regen_paddle_pressed
|
|
|
|
if self.maneuver_paddle_mode == "off":
|
|
self.regen_paddle_pressed = False
|
|
self.regen_press_counter = 0
|
|
self.regen_release_counter = 0
|
|
self.regen_min_on_frames = 0
|
|
press_regen_paddle = False
|
|
elif self.maneuver_paddle_mode == "force":
|
|
forced_press = accel < -0.02
|
|
self.regen_paddle_pressed = forced_press
|
|
press_regen_paddle = forced_press
|
|
else:
|
|
self.planner_regen_hold = False
|
|
self.regen_paddle_pressed = False
|
|
self.regen_paddle_timer = 0
|
|
self.regen_press_counter = 0
|
|
self.regen_release_counter = 0
|
|
self.regen_min_on_frames = 0
|
|
self.regen_min_off_frames = 0
|
|
|
|
speed_mps = [0.559, 1.678, 2.797, 3.916, 5.035, 6.154, 7.273, 8.392, 9.511, 10.63,
|
|
11.749, 12.868, 13.987, 15.106, 16.225, 17.344, 18.463, 19.582, 20.701, 21.820,
|
|
22.939, 24.058, 25.177, 26.296]
|
|
regen_gain_ratio = [1.01, 1.01, 1.02, 1.05, 1.08, 1.31, 1.33,
|
|
1.34, 1.35, 1.36, 1.37, 1.38, 1.39, 1.39,
|
|
1.40, 1.40, 1.41, 1.42, 1.43, 1.43, 1.44,
|
|
1.44, 1.45, 1.45]
|
|
|
|
gain = np.interp(v_ego, speed_mps, regen_gain_ratio)
|
|
gain *= np.interp(v_ego, [0.0, 2.0, 4.0, 5.5, 8.0, 12.0], [0.92, 0.92, 0.93, 0.94, 0.96, 1.0])
|
|
accel_gain = np.interp(v_ego, [0.0, 3.0, 8.0, 20.0], [0.47, 0.52, 0.57, 0.61])
|
|
pedaloffset = np.interp(v_ego, [0.0, 1.0, 3.0, 6.0, 15.0, 30.0], [0.085, 0.11, 0.17, 0.23, 0.235, 0.23])
|
|
|
|
# Suppress tiny planner sign flaps around zero so creep-speed pedal shaping stays stable.
|
|
pedal_accel = 0.0 if abs(accel) < 0.04 else accel
|
|
|
|
accel_term_scale = (1.0 / max(gain, 1e-3)) if press_regen_paddle else 1.0
|
|
if pedal_accel >= 0.0:
|
|
small_cmd_scale = np.interp(abs(pedal_accel), [0.0, 0.35, 0.8, 1.5, 2.5], [0.58, 0.68, 0.82, 0.93, 1.0])
|
|
else:
|
|
small_cmd_scale = np.interp(abs(pedal_accel), [0.0, 0.35, 0.8, 1.5, 2.5], [0.44, 0.54, 0.70, 0.89, 1.0])
|
|
accel_cmd = pedal_accel * small_cmd_scale
|
|
if pedal_accel < -2.0:
|
|
accel_cmd *= np.interp(abs(pedal_accel), [2.0, 2.5, 3.0], [1.0, 1.03, 1.06])
|
|
raw_pedal_gas = float(np.clip(pedaloffset + accel_cmd * accel_gain * accel_term_scale, 0.0, 1.0))
|
|
|
|
pedal_gas_max = np.interp(v_ego, [0.0, 1.0, 2.5, 4.5, 6.0, 8.0, 12.0], [0.20, 0.235, 0.29, 0.365, 0.52, 0.78, 1.0])
|
|
target_pedal_gas = float(np.clip(raw_pedal_gas, 0.0, pedal_gas_max))
|
|
|
|
if not self.pedal_active_last or (switched_state and v_ego > 1.0):
|
|
pedal_gas = target_pedal_gas
|
|
self.pedal_active_last = True
|
|
else:
|
|
urgency = float(np.clip(abs(accel) / 2.0, 0.0, 1.0))
|
|
rate_up = np.interp(v_ego, [0.0, 3.0, 8.0, 20.0], [0.007, 0.012, 0.022, 0.036]) + 0.011 * urgency
|
|
if accel > 0.0 and v_ego > 6.0:
|
|
comfort_factor = np.interp(abs(accel), [0.0, 0.12, 0.25, 0.45, 0.8], [0.55, 0.58, 0.68, 0.82, 1.0])
|
|
rate_up *= comfort_factor
|
|
if accel > 1.2:
|
|
rate_up += np.interp(v_ego, [0.0, 4.0, 12.0, 25.0], [0.006, 0.005, 0.003, 0.002])
|
|
rate_down = np.interp(v_ego, [0.0, 3.0, 8.0, 20.0], [0.008, 0.014, 0.026, 0.045]) + 0.015 * urgency
|
|
pedal_gas = float(np.clip(target_pedal_gas, self.pedal_steady - rate_down, self.pedal_steady + rate_up))
|
|
|
|
self.pedal_steady = pedal_gas
|
|
return pedal_gas, press_regen_paddle
|
|
|
|
def _update_malibu_button_slot(self, cs):
|
|
button_ts = cs.steering_button_ts_nanos
|
|
if button_ts != 0 and button_ts != self.malibu_last_button_ts_nanos:
|
|
self.malibu_last_button_ts_nanos = button_ts
|
|
return True
|
|
return False
|
|
|
|
def _sync_malibu_phase_from_oem(self, cs):
|
|
phase_map = gmcan.malibu_phase_map_for_acc(cs.cruise_buttons)
|
|
if phase_map and cs.steering_button_checksum in phase_map:
|
|
phase = phase_map[cs.steering_button_checksum]
|
|
self.malibu_cancel_phase = phase
|
|
self.malibu_button_phase = phase
|
|
|
|
def update(self, CC, CS, now_nanos, starpilot_toggles):
|
|
actuators = CC.actuators
|
|
self.aego = CS.out.aEgo
|
|
accel = actuators.accel
|
|
press_regen_paddle = False
|
|
auto_hold_enabled = supports_volt_auto_hold(self.CP, self.gm_auto_hold_enabled)
|
|
volt_one_pedal_supported = supports_volt_one_pedal(
|
|
self.CP, bool(getattr(starpilot_toggles, "volt_one_pedal_mode", False))
|
|
)
|
|
volt_one_pedal_active = should_activate_volt_one_pedal(
|
|
volt_one_pedal_supported,
|
|
CS.out.cruiseState.available,
|
|
CC.longActive,
|
|
CS.out.gasPressed,
|
|
CS.out.brakePressed,
|
|
CS.out.regenBraking,
|
|
bool(getattr(CS, "single_pedal_mode", False)),
|
|
CS.out.gearShifter,
|
|
float(getattr(CS, "one_pedal_drive_time", 0.0)),
|
|
)
|
|
if volt_one_pedal_active and self.volt_one_pedal_gas_pressed_last and not CS.out.gasPressed:
|
|
if CS.out.vEgo < VOLT_ONE_PEDAL_LIFT_BRAKE_BP[-1]:
|
|
self.volt_one_pedal_lift_frames = VOLT_ONE_PEDAL_LIFT_BRAKE_FRAMES
|
|
elif CS.out.gasPressed or not volt_one_pedal_active:
|
|
self.volt_one_pedal_lift_frames = 0
|
|
|
|
if self.frame % 4 == 0:
|
|
if volt_one_pedal_active:
|
|
self._update_volt_one_pedal_brake(CC, CS)
|
|
else:
|
|
self._reset_volt_one_pedal()
|
|
if not self.CP.openpilotLongitudinalControl:
|
|
self.apply_gas = 0
|
|
self.apply_brake = self.volt_one_pedal_brake if volt_one_pedal_active else 0
|
|
self.volt_one_pedal_gas_pressed_last = CS.out.gasPressed
|
|
|
|
stock_hold_apply_brake = max(self.apply_brake if self.CP.openpilotLongitudinalControl else 0, self.volt_one_pedal_brake)
|
|
|
|
hold_ready = (
|
|
auto_hold_enabled and
|
|
CS.out.cruiseState.available and
|
|
CS.out.gearShifter in AUTO_HOLD_DRIVE_GEARS and
|
|
CS.auto_hold_drive_time >= AUTO_HOLD_MIN_DRIVE_TIME_S
|
|
)
|
|
if not hold_ready or CS.out.gasPressed:
|
|
CS.auto_hold_armed = False
|
|
if CS.out.gasPressed:
|
|
CS.auto_hold_engaged = False
|
|
elif CS.regen_release_timer > 0.0:
|
|
CS.auto_hold_armed = False
|
|
elif not CS.auto_hold_armed and (CS.out.vEgo > 0.03 or ((CS.out.standstill or CS.out.vEgo < 0.02) and CS.out.brakePressed)):
|
|
CS.auto_hold_armed = True
|
|
|
|
if CS.out.vEgo > 0.1 or CS.out.gasPressed or CS.out.gearShifter not in AUTO_HOLD_DRIVE_GEARS:
|
|
self.auto_hold_brake = 0
|
|
elif CS.out.brakePressed or stock_hold_apply_brake > 0:
|
|
self.auto_hold_brake = estimate_auto_hold_brake(CS.out.brake, stock_hold_apply_brake, self.CP)
|
|
|
|
if self.frame % 25 == 0:
|
|
try:
|
|
self.gm_auto_hold_enabled = self.params_.get_bool("GMAutoHold")
|
|
except UnknownKeyName:
|
|
self.gm_auto_hold_enabled = False
|
|
try:
|
|
mode = self.params_.get("LongitudinalManeuverPaddleMode")
|
|
except UnknownKeyName:
|
|
mode = "auto"
|
|
if isinstance(mode, bytes):
|
|
mode = mode.decode("utf-8", errors="replace")
|
|
mode = (mode or "auto").strip().lower()
|
|
self.maneuver_paddle_mode = mode if mode in ("auto", "off", "force") else "auto"
|
|
try:
|
|
self.longitudinal_maneuver_mode = self.params_.get_bool("LongitudinalManeuverMode")
|
|
except UnknownKeyName:
|
|
self.longitudinal_maneuver_mode = False
|
|
|
|
kaofui_cars = SDGM_CAR | ASCM_INT | {
|
|
CAR.CHEVROLET_VOLT,
|
|
CAR.CHEVROLET_VOLT_2019,
|
|
CAR.CHEVROLET_VOLT_ASCM,
|
|
CAR.CHEVROLET_VOLT_CAMERA,
|
|
CAR.CHEVROLET_VOLT_CC,
|
|
CAR.CHEVROLET_MALIBU_CC,
|
|
CAR.CHEVROLET_MALIBU_HYBRID_CC,
|
|
}
|
|
|
|
if (self.CP.enableGasInterceptorDEPRECATED and self.CP.carFingerprint in CC_REGEN_PADDLE_CAR and
|
|
self.CP.openpilotLongitudinalControl and CC.longActive):
|
|
if self.maneuver_paddle_mode == "off":
|
|
self.planner_regen_hold = False
|
|
elif self.maneuver_paddle_mode == "force":
|
|
self.planner_regen_hold = accel < -0.02
|
|
else:
|
|
planner_press_threshold = np.interp(CS.out.vEgo, [0.0, 4.0, 12.0, 25.0], [-0.95, -0.82, -0.70, -0.62])
|
|
planner_release_threshold = np.interp(CS.out.vEgo, [0.0, 4.0, 12.0, 25.0], [-0.14, -0.22, -0.30, -0.36])
|
|
if accel <= planner_press_threshold:
|
|
self.planner_regen_hold = True
|
|
elif accel >= planner_release_threshold:
|
|
self.planner_regen_hold = False
|
|
else:
|
|
self.planner_regen_hold = False
|
|
|
|
hud_control = CC.hudControl
|
|
hud_alert = hud_control.visualAlert
|
|
hud_v_cruise = hud_control.setSpeed
|
|
if hud_v_cruise > 70:
|
|
hud_v_cruise = 0
|
|
dash_speed_spoof_active = should_spoof_dash_speed(self.CP, starpilot_toggles)
|
|
|
|
# Send CAN commands.
|
|
can_sends = []
|
|
malibu_cancel_requested = False
|
|
malibu_oem_button_slot = False
|
|
if self.CP.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC:
|
|
malibu_oem_button_slot = self._update_malibu_button_slot(CS)
|
|
if malibu_oem_button_slot:
|
|
self._sync_malibu_phase_from_oem(CS)
|
|
|
|
raw_regen_active = (
|
|
self.CP.carFingerprint in CC_REGEN_PADDLE_CAR and
|
|
self.CP.openpilotLongitudinalControl and
|
|
CC.longActive and
|
|
self.CP.enableGasInterceptorDEPRECATED and
|
|
self.regen_paddle_pressed
|
|
)
|
|
use_panda_paddle_sched = (
|
|
self.CP.enableGasInterceptorDEPRECATED and
|
|
self.CP.carFingerprint in CC_REGEN_PADDLE_CAR and
|
|
self.CP.openpilotLongitudinalControl and
|
|
bool(self.CP.flags & GMFlags.PEDAL_LONG.value)
|
|
)
|
|
if use_panda_paddle_sched:
|
|
if CC.enabled:
|
|
self.paddle_handoff_frames = 2
|
|
paddle_sched_feed_active = CC.enabled or (self.paddle_handoff_frames > 0)
|
|
if not CC.enabled and self.paddle_handoff_frames > 0:
|
|
self.paddle_handoff_frames -= 1
|
|
else:
|
|
self.paddle_handoff_frames = 0
|
|
paddle_sched_feed_active = False
|
|
|
|
paddle_spoof_pressed = raw_regen_active and (CS.out.vEgo > 2.68)
|
|
auto_hold_active = should_activate_auto_hold(
|
|
hold_ready,
|
|
CS.auto_hold_armed,
|
|
CS.auto_hold_engaged,
|
|
CS.out.brakePressed,
|
|
CS.out.gasPressed,
|
|
CS.out.standstill,
|
|
CC.longActive,
|
|
CS.out.regenBraking,
|
|
CS.out.vEgo,
|
|
get_auto_hold_stop_threshold(self.CP, CS.auto_hold_engaged),
|
|
)
|
|
bolt_acc_pedal_friction_experiment = supports_bolt_acc_pedal_friction_experiment(self.CP)
|
|
bolt_acc_pedal_friction_main_on = bolt_acc_pedal_friction_experiment and CS.out.cruiseState.available
|
|
if not bolt_acc_pedal_friction_main_on:
|
|
self.bolt_acc_pedal_friction_low_speed_active = False
|
|
volt_one_pedal_braking = volt_one_pedal_active and self.volt_one_pedal_brake > 0
|
|
volt_one_pedal_hold_active = (
|
|
volt_one_pedal_braking and
|
|
not auto_hold_active and
|
|
CS.one_pedal_drive_time >= AUTO_HOLD_MIN_DRIVE_TIME_S and
|
|
(CS.out.standstill or CS.out.vEgo < 0.02)
|
|
)
|
|
|
|
# Steering (Active: 50Hz, inactive: 10Hz)
|
|
steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP
|
|
|
|
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
|
# Also send at 50Hz:
|
|
# - on startup, first few msgs are blocked
|
|
# - until we're in sync with camera so counters align when relay closes, preventing a fault.
|
|
# openpilot can subtly drift, so this is activated throughout a drive to stay synced
|
|
out_of_sync = self.lka_steering_cmd_counter % 4 != (CS.cam_lka_steering_cmd_counter + 1) % 4
|
|
if CS.loopback_lka_steering_cmd_ts_nanos == 0 or out_of_sync:
|
|
steer_step = self.params.STEER_STEP
|
|
|
|
self.lka_steering_cmd_counter += 1 if CS.loopback_lka_steering_cmd_updated else 0
|
|
|
|
# Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we
|
|
# received the ASCMLKASteeringCmd loopback confirmation too recently
|
|
last_lka_steer_msg_ms = (now_nanos - CS.loopback_lka_steering_cmd_ts_nanos) * 1e-6
|
|
if (self.frame - self.last_steer_frame) >= steer_step and last_lka_steer_msg_ms > MIN_STEER_MSG_INTERVAL_MS:
|
|
# Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus
|
|
if CS.loopback_lka_steering_cmd_ts_nanos == 0:
|
|
self.lka_steering_cmd_counter = CS.pt_lka_steering_cmd_counter + 1
|
|
|
|
if CC.latActive:
|
|
new_torque = int(round(actuators.torque * self.params.STEER_MAX))
|
|
apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.params)
|
|
else:
|
|
apply_torque = 0
|
|
|
|
if (self.CP.flags & GMFlags.CC_LONG.value) and CC.enabled and not CS.out.cruiseState.enabled:
|
|
# Keep steer command neutral while stock CC is not active on CC_LONG.
|
|
apply_torque = 0
|
|
|
|
self.last_steer_frame = self.frame
|
|
self.apply_torque_last = apply_torque
|
|
idx = self.lka_steering_cmd_counter % 4
|
|
can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_torque, idx, CC.latActive))
|
|
|
|
if should_spoof_ecm_cruise_status(self.CP) and self.frame % 4 == 0:
|
|
can_sends.append(gmcan.create_ecm_cruise_control_command(
|
|
self.packer_pt, CanBus.POWERTRAIN, True, hud_v_cruise * CV.MS_TO_KPH))
|
|
|
|
xt4_cc_button_spam = (
|
|
self.CP.carFingerprint == CAR.CADILLAC_XT4_CC and
|
|
should_send_cc_button_spam(self.CP, CC, CS)
|
|
)
|
|
if xt4_cc_button_spam:
|
|
can_sends.extend(gmcan.create_gm_cc_spam_command(self.packer_pt, self, CS, actuators, starpilot_toggles))
|
|
elif self.CP.carFingerprint == CAR.CADILLAC_XT4_CC:
|
|
self.xt4_cc_button_burst_remaining = 0
|
|
self.xt4_cc_button_burst_button = CruiseButtons.INIT
|
|
self.xt4_cc_button_burst_last_counter = -1
|
|
self.xt4_cc_button_observed_counter = -1
|
|
|
|
if self.CP.openpilotLongitudinalControl:
|
|
# Gas/regen, brakes, and UI commands - all at 25Hz
|
|
if self.frame % 4 == 0:
|
|
stopping = actuators.longControlState == LongCtrlState.stopping
|
|
interceptor_gas_cmd = 0
|
|
at_full_stop = CC.longActive and CS.out.standstill
|
|
near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE)
|
|
if not CC.longActive:
|
|
# ASCM sends max regen when not enabled
|
|
self.apply_gas = self.params.INACTIVE_REGEN
|
|
self.apply_brake = 0
|
|
self.bolt_acc_pedal_friction_low_speed_active = False
|
|
self.planner_regen_hold = False
|
|
self.regen_paddle_pressed = False
|
|
self.regen_paddle_timer = 0
|
|
self.regen_press_counter = 0
|
|
self.regen_release_counter = 0
|
|
self.regen_min_on_frames = 0
|
|
self.regen_min_off_frames = 0
|
|
self.truck_follow_accel = 0.0
|
|
elif should_use_fixed_stopping_brake(self.CP, near_stop, stopping, CC.cruiseControl.resume):
|
|
stop_accel = getattr(starpilot_toggles, "stopAccel", self.CP.stopAccel)
|
|
self.apply_gas = self.params.INACTIVE_REGEN
|
|
self.apply_brake = int(min(-100 * stop_accel, self.params.MAX_BRAKE))
|
|
self.truck_follow_accel = 0.0
|
|
else:
|
|
long_pitch_enabled = bool(getattr(starpilot_toggles, "long_pitch", True))
|
|
pedal_long_path = bool(self.CP.enableGasInterceptorDEPRECATED and (self.CP.flags & GMFlags.PEDAL_LONG.value))
|
|
long_pitch_for_powertrain = long_pitch_enabled or pedal_long_path
|
|
|
|
if self.is_volt:
|
|
if long_pitch_for_powertrain and len(CC.orientationNED) == 3 and CS.out.vEgo > self.CP.vEgoStopping:
|
|
volt_pitch_accel = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
|
|
else:
|
|
volt_pitch_accel = 0.0
|
|
|
|
aero_drag_accel = (0.5 * self.coeffDrag * self.frontalArea * self.airDensity * CS.out.vEgo ** 2) / self.mass
|
|
accel_cmd = float(np.clip(accel + aero_drag_accel + volt_pitch_accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX))
|
|
brake_accel = float(np.clip(
|
|
accel + aero_drag_accel + volt_pitch_accel * np.interp(CS.out.vEgo, [5.0, 10.0], [0.0, 1.0]),
|
|
self.params.ACCEL_MIN, self.params.ACCEL_MAX))
|
|
|
|
if self.CP.carFingerprint in EV_CAR:
|
|
self.params.update_ev_gas_brake_threshold(CS.out.vEgo)
|
|
self.apply_gas = int(round(np.interp(accel_cmd, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
|
|
self.apply_brake = int(round(np.interp(brake_accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
|
|
else:
|
|
self.apply_gas = int(round(np.interp(accel_cmd, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
|
|
self.apply_brake = int(round(np.interp(brake_accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
|
|
|
|
self.apply_gas = int(round(np.clip(self.apply_gas, self.params.MAX_ACC_REGEN, self.params.MAX_GAS)))
|
|
self.apply_brake = int(round(np.clip(self.apply_brake, 0, self.params.MAX_BRAKE)))
|
|
if self.apply_brake > 0:
|
|
self.apply_gas = self.params.INACTIVE_REGEN
|
|
else:
|
|
if long_pitch_for_powertrain and len(CC.orientationNED) == 3 and CS.out.vEgo > self.CP.vEgoStopping:
|
|
accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
|
|
else:
|
|
accel_due_to_pitch = 0.0
|
|
|
|
gas_max = self.params.MAX_GAS
|
|
accel_max = self.params.ACCEL_MAX
|
|
if testing_ground.use_1:
|
|
accel_max = min(accel_max, np.interp(CS.out.vEgo, [0.0, 4.0, 12.0], [1.25, 1.6, self.params.ACCEL_MAX]))
|
|
|
|
truck_long_smoothing = (
|
|
getattr(starpilot_toggles, "truck_tuning", False) and
|
|
self.CP.carFingerprint in TRUCK_LONG_SMOOTH_CARS and
|
|
getattr(self.CP, "transmissionType", None) == TransmissionType.automatic and
|
|
not self.CP.enableGasInterceptorDEPRECATED
|
|
)
|
|
accel_due_to_pitch = shape_truck_pitch_accel(accel_due_to_pitch, CS.out.vEgo, truck_long_smoothing)
|
|
accel_input = actuators.accel + accel_due_to_pitch
|
|
if truck_long_smoothing:
|
|
accel_input = shape_truck_positive_accel(
|
|
accel_input,
|
|
CS.out.vEgo,
|
|
True,
|
|
lead_visible=CC.hudControl.leadVisible,
|
|
set_speed_error=max(CC.hudControl.setSpeed - CS.out.vEgo, 0.0),
|
|
)
|
|
|
|
accel_cmd = float(np.clip(accel_input, self.params.ACCEL_MIN, accel_max))
|
|
if truck_long_smoothing:
|
|
accel_cmd = smooth_truck_follow_accel(
|
|
accel_cmd,
|
|
self.truck_follow_accel,
|
|
CS.out.vEgo,
|
|
True,
|
|
CC.hudControl.leadVisible,
|
|
stopping,
|
|
)
|
|
self.truck_follow_accel = accel_cmd
|
|
torque = self.tireRadius * ((self.mass * accel_cmd) + (0.5 * self.coeffDrag * self.frontalArea * self.airDensity * CS.out.vEgo ** 2))
|
|
scaled_torque = torque + self.params.ZERO_GAS
|
|
apply_gas_torque = np.clip(scaled_torque, self.params.MAX_ACC_REGEN, gas_max)
|
|
brake_switch = int(round(np.interp(CS.out.vEgo, self.params.BRAKE_SWITCH_LOOKUP_BP, self.params.BRAKE_SWITCH_LOOKUP_V)))
|
|
if testing_ground.use_1:
|
|
brake_switch_bias = get_testing_ground_1_brake_switch_bias(CS.out.vEgo)
|
|
brake_switch = min(self.params.ZERO_GAS, brake_switch + brake_switch_bias)
|
|
if bolt_acc_pedal_friction_main_on:
|
|
planner_brake_switch = get_bolt_acc_pedal_planner_brake_switch(
|
|
CS.out.vEgo, self.params, self.tireRadius, self.mass, self.coeffDrag, self.frontalArea, self.airDensity,
|
|
)
|
|
brake_switch = get_bolt_acc_pedal_effective_brake_switch(brake_switch, planner_brake_switch)
|
|
brake_accel = min((scaled_torque - brake_switch) / (self.tireRadius * self.mass), 0)
|
|
self.apply_gas = int(round(apply_gas_torque))
|
|
self.apply_brake = int(round(np.interp(brake_accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
|
|
if truck_long_smoothing:
|
|
self.apply_brake, self.truck_friction_brake_active = shape_truck_friction_brake(
|
|
self.apply_brake, accel_cmd, stopping, self.truck_friction_brake_active,
|
|
)
|
|
else:
|
|
self.truck_friction_brake_active = False
|
|
if bolt_acc_pedal_friction_main_on:
|
|
if self.apply_brake > 0:
|
|
full_brake_accel = min(
|
|
self.params.ACCEL_MIN + (0.5 * self.coeffDrag * self.frontalArea * self.airDensity * CS.out.vEgo ** 2) / self.mass +
|
|
(self.params.ZERO_GAS - brake_switch) / (self.tireRadius * self.mass),
|
|
-0.1,
|
|
)
|
|
self.apply_brake = get_bolt_acc_pedal_friction_brake(
|
|
self.apply_brake, full_brake_accel, CS.out.vEgo, self.params,
|
|
)
|
|
self.apply_brake, self.bolt_acc_pedal_friction_low_speed_active = shape_bolt_acc_pedal_low_speed_friction(
|
|
self.apply_brake, CS.out.vEgo, stopping, self.bolt_acc_pedal_friction_low_speed_active,
|
|
)
|
|
if self.apply_brake > 0:
|
|
self.apply_gas = self.params.INACTIVE_REGEN
|
|
|
|
if stopping:
|
|
self.apply_gas = self.params.INACTIVE_REGEN
|
|
|
|
if self.CP.carFingerprint in CC_ONLY_CAR:
|
|
# gas interceptor only used for full long control on cars without ACC
|
|
interceptor_gas_cmd, press_regen_paddle = self.calc_pedal_command(actuators.accel, CC.longActive, CS.out.vEgo)
|
|
|
|
if volt_one_pedal_braking:
|
|
self.apply_gas = self.params.INACTIVE_REGEN
|
|
self.apply_brake = max(self.apply_brake, self.volt_one_pedal_brake)
|
|
|
|
maneuver_sng_launch = self.longitudinal_maneuver_mode and self.is_volt
|
|
if (
|
|
self.CP.enableGasInterceptorDEPRECATED and
|
|
self.apply_gas > self.params.INACTIVE_REGEN and
|
|
use_interceptor_sng_launch(self.CP, CS, maneuver_sng_launch)
|
|
):
|
|
interceptor_gas_cmd = get_interceptor_sng_gas_cmd(
|
|
self.CP, interceptor_gas_cmd, actuators.accel, self.params, maneuver_sng_launch,
|
|
)
|
|
self.apply_brake = 0
|
|
self.apply_gas = self.params.INACTIVE_REGEN
|
|
|
|
idx = (self.frame // 4) % 4
|
|
|
|
self.regen_paddle_pressed = press_regen_paddle
|
|
raw_regen_active = (
|
|
self.CP.carFingerprint in CC_REGEN_PADDLE_CAR and
|
|
self.CP.openpilotLongitudinalControl and
|
|
CC.longActive and
|
|
self.CP.enableGasInterceptorDEPRECATED and
|
|
self.regen_paddle_pressed
|
|
)
|
|
paddle_spoof_pressed = raw_regen_active and (CS.out.vEgo > 2.68)
|
|
if paddle_sched_feed_active:
|
|
can_sends.append(gmcan.create_prndl2_command(self.packer_pt, CanBus.POWERTRAIN, paddle_spoof_pressed, self.CP))
|
|
can_sends.append(gmcan.create_regen_paddle_command(self.packer_pt, CanBus.POWERTRAIN, paddle_spoof_pressed))
|
|
|
|
if self.CP.flags & GMFlags.CC_LONG.value:
|
|
if should_send_cc_button_spam(self.CP, CC, CS):
|
|
if self.CP.carFingerprint != CAR.CADILLAC_XT4_CC:
|
|
# Using extend instead of append since the message is only sent intermittently
|
|
can_sends.extend(gmcan.create_gm_cc_spam_command(self.packer_pt, self, CS, actuators, starpilot_toggles))
|
|
else:
|
|
if (CS.out.cruiseState.enabled and CC.enabled and self.frame % 52 == 0 and
|
|
CS.cruise_buttons == CruiseButtons.UNPRESS and CS.out.gasPressed and CS.out.cruiseState.speed < CS.out.vEgo < hud_v_cruise):
|
|
if self.CP.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC:
|
|
can_sends.append(gmcan.create_buttons_malibu(
|
|
self.packer_pt, CanBus.POWERTRAIN, CruiseButtons.DECEL_SET,
|
|
self.malibu_button_phase, CS.steering_button_prefix))
|
|
self.malibu_button_phase = (self.malibu_button_phase + 1) % 4
|
|
else:
|
|
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, (CS.buttons_counter + 1) % 4, CruiseButtons.DECEL_SET))
|
|
if self.CP.enableGasInterceptorDEPRECATED:
|
|
can_sends.append(create_gas_interceptor_command(self.packer_pt, interceptor_gas_cmd, idx))
|
|
if bolt_acc_pedal_friction_experiment:
|
|
friction_brake_bus = get_friction_brake_bus(self.CP)
|
|
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
|
at_full_stop = at_full_stop and stopping
|
|
|
|
experiment_brake, self.bolt_acc_pedal_friction_release_frames, should_send_bolt_acc_pedal_friction = \
|
|
get_bolt_acc_pedal_friction_command_state(
|
|
self.apply_brake,
|
|
bolt_acc_pedal_friction_main_on,
|
|
self.bolt_acc_pedal_friction_release_frames,
|
|
)
|
|
|
|
# This fingerprint is routed through the CC-only pedal path, so it
|
|
# does not fall through to the normal friction-brake sender below.
|
|
# Never apply stock friction with cruise main off, but do send a short
|
|
# explicit zero-brake unwind so the last nonzero stock-EBCM command
|
|
# cannot linger after a disengage or main-off event.
|
|
if should_send_bolt_acc_pedal_friction:
|
|
can_sends.append(gmcan.create_friction_brake_command(
|
|
self.packer_ch, friction_brake_bus, experiment_brake, idx, bolt_acc_pedal_friction_main_on,
|
|
near_stop, at_full_stop, self.CP))
|
|
if self.CP.carFingerprint not in CC_ONLY_CAR:
|
|
friction_brake_bus = get_friction_brake_bus(self.CP)
|
|
# GM Camera exceptions
|
|
# TODO: can we always check the longControlState?
|
|
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
|
at_full_stop = at_full_stop and stopping
|
|
if should_send_acc_2cd(self.CP):
|
|
can_sends.append(gmcan.create_acc_2cd_command(CanBus.POWERTRAIN, idx))
|
|
|
|
if self.CP.autoResumeSng:
|
|
resume = actuators.longControlState != LongCtrlState.starting or CC.cruiseControl.resume
|
|
at_full_stop = at_full_stop and not resume
|
|
|
|
if CC.cruiseControl.resume and CS.pcm_acc_status == AccState.STANDSTILL and starpilot_toggles.volt_sng:
|
|
acc_engaged = False
|
|
else:
|
|
acc_engaged = CC.enabled
|
|
|
|
if auto_hold_active:
|
|
hold_brake = max(self.volt_one_pedal_brake, self.auto_hold_brake or estimate_auto_hold_brake(CS.out.brake, self.apply_brake, self.CP))
|
|
hold_standstill = CS.pcm_acc_status == AccState.STANDSTILL
|
|
hold_near_stop = CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE
|
|
can_sends.append(gmcan.create_friction_brake_command(
|
|
self.packer_ch, friction_brake_bus, hold_brake, idx, False, hold_near_stop, hold_standstill,
|
|
self.CP, allow_near_stop_mode=True))
|
|
CS.auto_hold_engaged = True
|
|
CS.auto_hold_fault_suppression_timer = 1.0
|
|
elif volt_one_pedal_hold_active:
|
|
hold_brake = max(self.volt_one_pedal_brake, self.auto_hold_brake or estimate_auto_hold_brake(0.0, self.volt_one_pedal_brake, self.CP))
|
|
hold_standstill = CS.pcm_acc_status == AccState.STANDSTILL
|
|
hold_near_stop = CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE
|
|
can_sends.append(gmcan.create_friction_brake_command(
|
|
self.packer_ch, friction_brake_bus, hold_brake, idx, False, hold_near_stop, hold_standstill,
|
|
self.CP, allow_near_stop_mode=True))
|
|
CS.auto_hold_engaged = True
|
|
CS.auto_hold_fault_suppression_timer = 1.0
|
|
else:
|
|
if volt_one_pedal_braking:
|
|
at_full_stop = at_full_stop or CS.pcm_acc_status == AccState.STANDSTILL
|
|
near_stop = near_stop or (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE)
|
|
# GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation
|
|
can_sends.append(gmcan.create_gas_regen_command(
|
|
self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, acc_engaged, at_full_stop,
|
|
include_always_one3=self.CP.carFingerprint in kaofui_cars, use_volt_layout=self.is_volt))
|
|
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake,
|
|
idx, CC.enabled, near_stop, at_full_stop, self.CP,
|
|
allow_near_stop_mode=volt_one_pedal_braking))
|
|
CS.auto_hold_engaged = False
|
|
|
|
if should_send_acc_dashboard_status(self.CP, dash_speed_spoof_active):
|
|
fcw_alert = get_acc_dashboard_fcw_alert(hud_alert, CS)
|
|
acc_dashboard_status_active = get_acc_dashboard_status_active(self.CP, CC)
|
|
acc_dashboard_always_one = get_acc_dashboard_always_one(self.CP)
|
|
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, acc_dashboard_status_active,
|
|
hud_v_cruise * CV.MS_TO_KPH, hud_control, fcw_alert,
|
|
acc_dashboard_always_one))
|
|
|
|
# Radar needs to know current speed and yaw rate (50hz),
|
|
# and that ADAS is alive (10hz)
|
|
if should_send_adas_status(self.CP, self.CP.carFingerprint in kaofui_cars):
|
|
tt = self.frame * DT_CTRL
|
|
if self.CP.carFingerprint in kaofui_cars:
|
|
time_and_headlights_step = 10
|
|
speed_and_accelerometer_step = 2
|
|
if self.frame % time_and_headlights_step == 0:
|
|
idx = (self.frame // time_and_headlights_step) % 4
|
|
can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx))
|
|
can_sends.append(gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE))
|
|
if self.frame % speed_and_accelerometer_step == 0:
|
|
idx = (self.frame // speed_and_accelerometer_step) % 4
|
|
can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
|
|
can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx))
|
|
else:
|
|
time_and_headlights_step = 20
|
|
if self.frame % time_and_headlights_step == 0:
|
|
idx = (self.frame // time_and_headlights_step) % 4
|
|
can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx))
|
|
can_sends.append(gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE))
|
|
can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
|
|
can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx))
|
|
|
|
keepalive_step = get_adas_keepalive_step(self.CP, self.CP.carFingerprint in kaofui_cars)
|
|
if keepalive_step is not None and self.frame % keepalive_step == 0:
|
|
can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
|
|
|
|
pedal_cancel = bool(self.CP.flags & GMFlags.PEDAL_LONG.value) and CS.out.cruiseState.enabled
|
|
cc_long_cancel = ((self.CP.flags & GMFlags.CC_LONG.value) and
|
|
self.prev_op_enabled and not CC.enabled and CS.out.cruiseState.enabled)
|
|
|
|
if self.CP.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC:
|
|
stock_cc_active = get_stock_cc_active_for_cancel(self.CP, CS)
|
|
pedal_cancel = bool(self.CP.flags & GMFlags.PEDAL_LONG.value) and CC.longActive
|
|
cc_long_cancel = bool(self.CP.flags & GMFlags.CC_LONG.value) and not CC.enabled
|
|
|
|
if (pedal_cancel or cc_long_cancel) and (stock_cc_active if self.CP.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC else True):
|
|
if self.CP.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC:
|
|
malibu_cancel_requested = True
|
|
elif (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
|
|
self.last_button_frame = self.frame
|
|
cancel_bus = CanBus.CAMERA if self.CP.carFingerprint == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL else CanBus.POWERTRAIN
|
|
can_sends.append(gmcan.create_buttons(self.packer_pt, cancel_bus, (CS.buttons_counter + 1) % 4, CruiseButtons.CANCEL))
|
|
|
|
else:
|
|
if self.frame % 4 == 0 and auto_hold_active:
|
|
idx = (self.frame // 4) % 4
|
|
hold_brake = max(self.volt_one_pedal_brake, self.auto_hold_brake or estimate_auto_hold_brake(CS.out.brake, stock_hold_apply_brake, self.CP))
|
|
hold_standstill = CS.pcm_acc_status == AccState.STANDSTILL
|
|
hold_near_stop = CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE
|
|
can_sends.append(gmcan.create_friction_brake_command(
|
|
self.packer_ch, get_friction_brake_bus(self.CP), hold_brake, idx, False, hold_near_stop, hold_standstill,
|
|
self.CP, allow_near_stop_mode=True))
|
|
CS.auto_hold_engaged = True
|
|
CS.auto_hold_fault_suppression_timer = 1.0
|
|
elif self.frame % 4 == 0 and volt_one_pedal_hold_active:
|
|
idx = (self.frame // 4) % 4
|
|
hold_brake = max(self.volt_one_pedal_brake, self.auto_hold_brake or estimate_auto_hold_brake(0.0, self.volt_one_pedal_brake, self.CP))
|
|
hold_standstill = CS.pcm_acc_status == AccState.STANDSTILL
|
|
hold_near_stop = CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE
|
|
can_sends.append(gmcan.create_friction_brake_command(
|
|
self.packer_ch, get_friction_brake_bus(self.CP), hold_brake, idx, False, hold_near_stop, hold_standstill,
|
|
self.CP, allow_near_stop_mode=True))
|
|
CS.auto_hold_engaged = True
|
|
CS.auto_hold_fault_suppression_timer = 1.0
|
|
elif self.frame % 4 == 0 and volt_one_pedal_braking:
|
|
idx = (self.frame // 4) % 4
|
|
near_stop = CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE
|
|
can_sends.append(gmcan.create_friction_brake_command(
|
|
self.packer_ch, get_friction_brake_bus(self.CP), self.volt_one_pedal_brake, idx, False, near_stop, False,
|
|
self.CP, allow_near_stop_mode=True))
|
|
CS.auto_hold_engaged = False
|
|
elif self.frame % 4 == 0:
|
|
self.apply_brake = 0
|
|
CS.auto_hold_engaged = False
|
|
|
|
# While car is braking, cancel button causes ECM to enter a soft disable state with a fault status.
|
|
# A delayed cancellation allows camera to cancel and avoids a fault when user depresses brake quickly
|
|
self.cancel_counter = self.cancel_counter + 1 if CC.cruiseControl.cancel else 0
|
|
|
|
# Stock longitudinal, integrated at camera
|
|
if self.CP.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC and self.cancel_counter > CAMERA_CANCEL_DELAY_FRAMES:
|
|
malibu_cancel_requested = True
|
|
elif (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
|
|
if self.cancel_counter > CAMERA_CANCEL_DELAY_FRAMES:
|
|
self.last_button_frame = self.frame
|
|
sdgm_stock_cancel_pt = (
|
|
self.CP.carFingerprint in SDGM_CAR and
|
|
self.CP.carFingerprint not in {
|
|
CAR.CHEVROLET_VOLT,
|
|
CAR.CHEVROLET_VOLT_2019,
|
|
CAR.CHEVROLET_VOLT_ASCM,
|
|
CAR.CHEVROLET_VOLT_CAMERA,
|
|
CAR.CHEVROLET_VOLT_CC,
|
|
CAR.CHEVROLET_BLAZER,
|
|
CAR.CHEVROLET_MALIBU_SDGM,
|
|
CAR.CHEVROLET_TRAVERSE,
|
|
}
|
|
)
|
|
cancel_bus = CanBus.POWERTRAIN if sdgm_stock_cancel_pt else CanBus.CAMERA
|
|
can_sends.append(gmcan.create_buttons(self.packer_pt, cancel_bus, CS.buttons_counter, CruiseButtons.CANCEL))
|
|
|
|
if self.CP.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC:
|
|
if malibu_cancel_requested and malibu_oem_button_slot:
|
|
can_sends.append(gmcan.create_buttons_malibu_cancel(
|
|
CanBus.POWERTRAIN, (self.malibu_cancel_phase + 1) % 4, CS.steering_button_prefix))
|
|
self.malibu_cancel_phase = (self.malibu_cancel_phase + 1) % 4
|
|
|
|
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
|
# Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1
|
|
if self.frame % 10 == 0:
|
|
can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status))
|
|
|
|
new_actuators = actuators.as_builder()
|
|
new_actuators.torque = self.apply_torque_last / self.params.STEER_MAX
|
|
new_actuators.torqueOutputCan = self.apply_torque_last
|
|
new_actuators.gas = self.apply_gas
|
|
new_actuators.brake = self.apply_brake
|
|
|
|
self.prev_op_enabled = CC.enabled
|
|
self.frame += 1
|
|
|
|
new_actuators.speed = self.apply_speed
|
|
|
|
return new_actuators, can_sends
|