713 lines
37 KiB
Python
713 lines
37 KiB
Python
"""
|
|
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
|
"""
|
|
import sys
|
|
import os
|
|
import math
|
|
import numpy as np
|
|
import random
|
|
from iqdbc.can import CANPacker
|
|
from iqdbc.car import Bus, DT_CTRL, make_tester_present_msg, structs
|
|
from iqdbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_simple
|
|
from iqdbc.car.lateral import apply_std_curvature_limits
|
|
from iqdbc.car.common.conversions import Conversions as CV
|
|
from iqdbc.car.common.numpy_fast import clip, interp
|
|
from iqdbc.car.interfaces import CarControllerBase
|
|
from iqdbc.car.volkswagen import mlbcan, mqbcan, pqcan, mebcan
|
|
from iqdbc.car.volkswagen.pq_radar_handler import PQRadarHandler
|
|
from iqdbc.car.volkswagen.values import (
|
|
CanBus, CarControllerParams, MQB_A0_CARS, VolkswagenFlags, VolkswagenFlagsIQ, apply_pq_stopping_accel,
|
|
)
|
|
from iqdbc.car.volkswagen.mebutils import LongControlJerk, LongControlLimit, LatControlCurvature
|
|
from iqdbc.car.vehicle_model import VehicleModel
|
|
|
|
iqpilot_path = os.path.join(os.path.dirname(__file__), '..', '..', '..')
|
|
sys.path.insert(0, iqpilot_path)
|
|
try:
|
|
from iqpilot.common.params import Params
|
|
except ImportError:
|
|
pass
|
|
|
|
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
|
AudibleAlert = structs.CarControl.HUDControl.AudibleAlert
|
|
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
|
|
|
|
|
def dVisual(CCS, CS):
|
|
if CCS == mqbcan:
|
|
decelV = CS.tsk_verzoeg_anf
|
|
elif CCS == pqcan:
|
|
decelV = CS.br8_acc_anf
|
|
else:
|
|
decelV = False
|
|
return decelV
|
|
|
|
class MQBStandstillManager:
|
|
BRAKE_TORQUE_RAMP_RATE = 2000.0 # Nm/s
|
|
ASSUMED_WHEEL_RADIUS = 0.328 # m, typical tire rolling radius
|
|
GRAVITY = 9.81 # m/s^2
|
|
WEGIMPULSE_STILLNESS_FRAMES = 5 # frames of no wheel tick change before assuming standstill
|
|
ESP_OVERRIDE_SPEED = 9.5 * CV.KPH_TO_MS
|
|
MAX_SAFE_STOPPING_SPEED = 10.0 * CV.KPH_TO_MS
|
|
|
|
def __init__(self, vehicle_mass: float = 1540.0, accel_min: float = -3.5):
|
|
self.vehicle_mass = vehicle_mass
|
|
self.accel_min = accel_min
|
|
self.can_stop_forever = False
|
|
self.rollback_detected = False
|
|
self.start_commit_active = False
|
|
self.frames_since_last_wheel_pulse = 0
|
|
self.prev_sum_wegimpulse: int | None = None
|
|
self.prev_accel = 0
|
|
self.hold_recovery_active = False
|
|
|
|
def get_hill_hold_decel_deficit(self, pitch: float, brake_torque: float) -> float:
|
|
if self.vehicle_mass <= 0:
|
|
return 0.0
|
|
uphill_pitch = max(pitch, 0.0)
|
|
hill_hold_decel = self.GRAVITY * math.sin(uphill_pitch)
|
|
brake_decel = max(brake_torque, 0.0) / (self.vehicle_mass * self.ASSUMED_WHEEL_RADIUS)
|
|
return max(hill_hold_decel - brake_decel, 0.0)
|
|
|
|
def get_safe_speed_for_brake_torque(self, pitch: float, brake_torque: float) -> float:
|
|
missing_brake_decel = self.get_hill_hold_decel_deficit(pitch, brake_torque)
|
|
if missing_brake_decel <= 0 or self.vehicle_mass <= 0:
|
|
return 0.0
|
|
brake_decel_build_rate = self.BRAKE_TORQUE_RAMP_RATE / (self.vehicle_mass * self.ASSUMED_WHEEL_RADIUS)
|
|
forward_speed_needed_while_brake_builds = 1.5 * missing_brake_decel ** 2 / brake_decel_build_rate
|
|
return min(forward_speed_needed_while_brake_builds, self.MAX_SAFE_STOPPING_SPEED)
|
|
|
|
def get_blended_brake_accel(self, raw_accel: float, v_ego: float, pitch: float, brake_torque: float) -> float:
|
|
zero_brake_decel_deficit = self.get_hill_hold_decel_deficit(pitch, 0.0)
|
|
current_brake_decel_deficit = self.get_hill_hold_decel_deficit(pitch, brake_torque)
|
|
zero_brake_safe_speed = self.get_safe_speed_for_brake_torque(pitch, 0.0)
|
|
if zero_brake_decel_deficit <= 0 or zero_brake_safe_speed <= 0:
|
|
return raw_accel
|
|
brake_deficit_risk = current_brake_decel_deficit / zero_brake_decel_deficit
|
|
speed_risk = max(zero_brake_safe_speed - v_ego, 0.0) / zero_brake_safe_speed
|
|
rollback_risk = float(np.clip(speed_risk * brake_deficit_risk, 0.0, 1.0))
|
|
blended_accel = raw_accel + rollback_risk * (self.accel_min - raw_accel)
|
|
return min(raw_accel, blended_accel)
|
|
|
|
def update(self, CS, long_active: bool, accel: float, stopping: bool, starting: bool,
|
|
max_planned_speed: float, pitch: float = 0.0,
|
|
tsk_brake_torque: float = 0.0) -> tuple[bool, float, bool, bool, bool | None, bool | None]:
|
|
|
|
safe_stopping_speed = self.get_safe_speed_for_brake_torque(pitch, 0.0)
|
|
below_safe_stop_speed = CS.out.vEgo < safe_stopping_speed
|
|
can_accelerate = max_planned_speed > safe_stopping_speed
|
|
uphill_grade_pct = max(math.tan(pitch) * 100.0, 0.0)
|
|
takeoff_acceleration = max(0.2, 0.1 * uphill_grade_pct)
|
|
|
|
if CS.out.vEgo < self.ESP_OVERRIDE_SPEED:
|
|
esp_starting_override: bool | None = True
|
|
esp_stopping_override: bool | None = False
|
|
else:
|
|
esp_starting_override = None
|
|
esp_stopping_override = None
|
|
|
|
if CS.rolling_backward:
|
|
self.rollback_detected = True
|
|
elif CS.rolling_forward:
|
|
self.rollback_detected = False
|
|
|
|
wheel_did_pulse = CS.sum_wegimpulse != self.prev_sum_wegimpulse
|
|
self.prev_sum_wegimpulse = CS.sum_wegimpulse
|
|
if wheel_did_pulse:
|
|
self.frames_since_last_wheel_pulse = 0
|
|
else:
|
|
self.frames_since_last_wheel_pulse += 1
|
|
near_standstill = self.frames_since_last_wheel_pulse >= self.WEGIMPULSE_STILLNESS_FRAMES
|
|
|
|
# acc type 1 is sensitive to control signals when brake is pressed (when preEnabled)
|
|
if CS.out.brakePressed:
|
|
long_active = False
|
|
|
|
if long_active and not CS.out.gasPressed:
|
|
if CS.esp_hold_confirmation:
|
|
self.start_commit_active = True
|
|
if can_accelerate and below_safe_stop_speed and accel > 0:
|
|
self.start_commit_active = True
|
|
elif self.start_commit_active:
|
|
if CS.out.vEgo > safe_stopping_speed:
|
|
self.start_commit_active = False
|
|
else:
|
|
self.start_commit_active = False
|
|
|
|
if long_active:
|
|
raw_accel = accel
|
|
if self.start_commit_active:
|
|
accel = max(accel, takeoff_acceleration)
|
|
stopping = False
|
|
starting = True
|
|
elif self.rollback_detected:
|
|
accel = self.accel_min
|
|
stopping = True
|
|
starting = False
|
|
elif below_safe_stop_speed:
|
|
accel = self.get_blended_brake_accel(accel, CS.out.vEgo, pitch, tsk_brake_torque)
|
|
if accel < raw_accel:
|
|
stopping = True
|
|
starting = False
|
|
if near_standstill and accel < 0 and tsk_brake_torque == 0:
|
|
accel = self.accel_min
|
|
stopping = True
|
|
starting = False
|
|
if CS.out.standstill and accel < 0:
|
|
accel = min(accel, self.prev_accel)
|
|
|
|
if long_active:
|
|
if CS.out.vEgo > self.ESP_OVERRIDE_SPEED:
|
|
self.can_stop_forever = False
|
|
if CS.esp_hold_confirmation:
|
|
self.can_stop_forever = False
|
|
self.hold_recovery_active = True
|
|
|
|
if self.start_commit_active:
|
|
esp_starting_override = True
|
|
esp_stopping_override = False
|
|
elif CS.esp_stopping:
|
|
self.can_stop_forever = True
|
|
self.hold_recovery_active = False
|
|
esp_starting_override = True
|
|
esp_stopping_override = False
|
|
elif self.can_stop_forever:
|
|
esp_starting_override = True
|
|
esp_stopping_override = False
|
|
elif near_standstill:
|
|
esp_starting_override = False
|
|
esp_stopping_override = True
|
|
# recover from hold confirmations while moving to prevent reconfirming them
|
|
elif self.hold_recovery_active and not CS.out.standstill:
|
|
esp_starting_override = False
|
|
esp_stopping_override = True
|
|
else:
|
|
self.can_stop_forever = False
|
|
self.hold_recovery_active = False
|
|
|
|
self.prev_accel = accel
|
|
return long_active, accel, stopping, starting, esp_starting_override, esp_stopping_override
|
|
|
|
|
|
def accel_during_driver_override(accel: float, gas_pressed: bool, keep_long_active: bool) -> float:
|
|
return 0.0 if gas_pressed and keep_long_active else accel
|
|
|
|
|
|
def ea_send_ready(stock_values, last_counter):
|
|
return bool(stock_values) and stock_values["COUNTER"] != last_counter
|
|
|
|
|
|
EA_BLINKER_STEP = 2
|
|
|
|
|
|
def next_ea_counter(tx_counter, stock_counter):
|
|
return ((stock_counter if tx_counter is None else tx_counter) + 1) % 16
|
|
|
|
|
|
def ea_blinker_command(left_request, right_request, left_active, right_active):
|
|
blinker_active = left_active or right_active
|
|
return left_request and not blinker_active, right_request and not blinker_active
|
|
|
|
|
|
class CarController(CarControllerBase):
|
|
def __init__(self, dbc_names, CP, CP_IQ):
|
|
from iqpilot.system.proprietary_runtime._verified_import import import_verified_module
|
|
self._iq_lvbs_alc = import_verified_module("iqpilot_alc_private", "iqpilot_private.konn3kt.iqlvbs.alc")
|
|
super().__init__(dbc_names, CP, CP_IQ)
|
|
self._params = Params()
|
|
self.CCP = CarControllerParams(CP)
|
|
self.CAN = CanBus(CP)
|
|
self.packer_pt = CANPacker(dbc_names[Bus.pt])
|
|
|
|
self._pt_tx_bus = self.CAN.pt
|
|
if CP.flags & VolkswagenFlags.PQ:
|
|
self.CCS = pqcan
|
|
if CP.flags & VolkswagenFlagsIQ.IQ_PQ_LOWLINE:
|
|
self._pt_tx_bus = self.CAN.aux
|
|
elif CP.flags & VolkswagenFlags.MLB:
|
|
self.CCS = mlbcan
|
|
if CP.flags & VolkswagenFlagsIQ.IQ_MLB_NO_ECAN:
|
|
self._pt_tx_bus = self.CAN.aux
|
|
elif CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
|
|
self.CCS = mebcan
|
|
else:
|
|
self.CCS = mqbcan
|
|
|
|
self.accel = 0
|
|
self.apply_torque_last = 0
|
|
self.apply_curvature_last = 0.
|
|
self.apply_angle_last = 0
|
|
self.ALC_entryCounter = 0
|
|
self.ALC_driverExit = False
|
|
self.ALC_reentry_blocked = False
|
|
self.ALC_override_last = False
|
|
self.ALC_override_counter = 0
|
|
self.entering = False
|
|
self.active = False
|
|
self.CSLH3_SignLast = 0
|
|
self.CSsteeringAngleDegLast = 0
|
|
self.steering_power_last = 0
|
|
self.long_jerk_control = LongControlJerk(dt=(DT_CTRL * self.CCP.ACC_CONTROL_STEP)) if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO) else None
|
|
self.long_limit_control = LongControlLimit(dt=(DT_CTRL * self.CCP.ACC_CONTROL_STEP)) if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO) else None
|
|
self.gra_acc_counter_last = None
|
|
self.gra_cancel_ticks = 0
|
|
self.motor3_frame_last = None
|
|
self.motor3_was_stopping = False
|
|
self.motor3_resuming = False
|
|
self.sng_handoff_active = False
|
|
self.acc_counter_seeded = False
|
|
self.klr_counter_last = None
|
|
self.ea_counter_last = None
|
|
self.ea_tx_counter = None
|
|
self.eps_timer_soft_disable_alert = False
|
|
self.hca_frame_timer_running = 0
|
|
self.hca_frame_same_torque = 0
|
|
self.accel_last = 0
|
|
self.long_deviation = 0
|
|
self.long_jerklimit = 0
|
|
self.HCA_Status = 3
|
|
self.leadDistanceBars = 0
|
|
self.lead_distance_bars_last = None
|
|
self.distance_bar_frame = 0
|
|
self.mlb_hud_text = 0
|
|
self.mlb_hud_text_frame = 0
|
|
self.mlb_set_speed_last = 0
|
|
self.mlb_lead_distance_bars_last = None
|
|
self.speed_limit_last = 0
|
|
self.speed_limit_changed_timer = 0
|
|
self.blinkerActive = None
|
|
self.hide_ea_error = False
|
|
self.radar_disabled_warning_timer = 0
|
|
# Check once at init whether the DBC includes MEB_AWV_01 (AEB HUD for radar-disabled camera harness cars)
|
|
self._has_aeb_hud_msg = "MEB_AWV_01" in self.packer_pt.dbc.name_to_msg
|
|
self.eps_timer_workaround = bool(CP.flags & (VolkswagenFlags.MLB | VolkswagenFlagsIQ.IQ_PQ_TIMEBOMB))
|
|
self._pq_patch_checked = not bool(CP.flags & VolkswagenFlags.PQ) or self.eps_timer_workaround
|
|
self.hca_frame_timer_resetting = 0
|
|
self.hca_frame_low_torque = 0
|
|
self.acc_hold_type_last = mebcan.ACC_HMS_NO_REQUEST
|
|
self.acc_hold_ramp_counter = 0
|
|
self.standstill_manager = MQBStandstillManager(CP.mass, self.CCP.ACCEL_MIN) if self.CCS == mqbcan else None
|
|
self.radar_handler = PQRadarHandler(self.CAN) if self.CCS is pqcan else None
|
|
self.blend_stock_radar = False
|
|
self.unavailable = False
|
|
self.unavailable_hold = 0
|
|
self.VM = VehicleModel(CP)
|
|
self.is_mqb_a0 = self._is_mqb_a0_car(CP.carFingerprint)
|
|
self.LateralController = (
|
|
LatControlCurvature(self.CCP.CURVATURE_PID, self.CCP.CURVATURE_LIMITS.CURVATURE_MAX, 1 / (DT_CTRL * self.CCP.STEER_STEP))
|
|
if (CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO))
|
|
else None
|
|
)
|
|
|
|
@staticmethod
|
|
def _is_mqb_a0_car(candidate) -> bool:
|
|
return candidate in MQB_A0_CARS
|
|
|
|
def _get_mqb_steering_torque_scale(self, v_ego: float, enabled: bool) -> float:
|
|
if enabled and self.CCS == mqbcan:
|
|
return float(np.interp(v_ego, [0.4, 3.5, 4.0], [0.8, 0.95, 1.0]))
|
|
return 1.0
|
|
|
|
def _mlb_acc_hud_text(self, hud_control, set_speed: float) -> int:
|
|
# ACC_02 primary display text, briefly surfaced on a follow distance or set speed change
|
|
if hud_control.leadDistanceBars != self.mlb_lead_distance_bars_last:
|
|
self.mlb_hud_text_frame = self.frame
|
|
self.mlb_hud_text = self.CCP.ACC_HUD_TEXT_DISTANCE.get(hud_control.leadDistanceBars, self.CCP.ACC_HUD_TEXTS["none"])
|
|
elif set_speed != self.mlb_set_speed_last and hud_control.speedVisible:
|
|
self.mlb_hud_text_frame = self.frame
|
|
self.mlb_hud_text = self.CCP.ACC_HUD_TEXTS["setSpeed"]
|
|
elif self.frame - self.mlb_hud_text_frame >= self.CCP.ACC_HUD_TEXT_STEP:
|
|
self.mlb_hud_text = self.CCP.ACC_HUD_TEXTS["none"]
|
|
self.mlb_lead_distance_bars_last = hud_control.leadDistanceBars
|
|
self.mlb_set_speed_last = set_speed
|
|
return self.mlb_hud_text
|
|
|
|
def _tap_gra_cancel(self, cancel_req: bool, gra_send_ready: bool) -> bool:
|
|
if not cancel_req:
|
|
self.gra_cancel_ticks = 0
|
|
return False
|
|
|
|
period = self.CCP.GRA_CANCEL_TAP_ON + self.CCP.GRA_CANCEL_TAP_OFF
|
|
if self.gra_cancel_ticks >= period * self.CCP.GRA_CANCEL_MAX_TAPS:
|
|
return False
|
|
|
|
pressed = self.gra_cancel_ticks % period < self.CCP.GRA_CANCEL_TAP_ON
|
|
if gra_send_ready:
|
|
self.gra_cancel_ticks += 1
|
|
return pressed
|
|
|
|
def _should_spam_mqb_a0_resume(self, CS, enabled: bool) -> bool:
|
|
return bool(
|
|
enabled and
|
|
self.is_mqb_a0 and
|
|
self.CCS == mqbcan and
|
|
CS.out.standstill and
|
|
self.frame % 50 < 15
|
|
)
|
|
|
|
def update(self, CC, CC_IQ, CS, now_nanos):
|
|
actuators = CC.actuators
|
|
hud_control = CC.hudControl
|
|
can_sends = []
|
|
output_torque = 0
|
|
apply_torque = 0
|
|
pqhca5or7Toggle = self._params.get_bool("pqhca5or7Toggle")
|
|
eBrakeActive = self._params.get_bool("eBrakeActive")
|
|
iq_mqb_steering_lockout = self._params.get_bool("iqMqbSteeringLockout")
|
|
iq_mqb_acc_resume = self._params.get_bool("iqMqbAccResume")
|
|
self.blend_stock_radar = self._params.get_bool("IQDynamicBlendStockRadar")
|
|
if not self._pq_patch_checked:
|
|
self._pq_patch_checked = True
|
|
self.eps_timer_workaround = not self._params.get_bool("VwPqEpsPatched")
|
|
blend_active = bool(self.blend_stock_radar and self.radar_handler is not None and self.CP.openpilotLongitudinalControl)
|
|
AngleLateralControl = self._iq_lvbs_alc.angle_lateral_control_enabled(self, CS)
|
|
self.entering = CS.vw_iq_lvbs_alc_entering
|
|
self.active = CS.vw_iq_lvbs_alc_active
|
|
|
|
if hud_control.audibleAlert == AudibleAlert.refuse:
|
|
self.unavailable_hold = self.CCP.IQ_PQ_UNAVAILABLE_HUD_FRAMES
|
|
else:
|
|
self.unavailable_hold = max(0, self.unavailable_hold - 1)
|
|
self.unavailable = self.unavailable_hold > 0
|
|
|
|
CS.force_rhd_for_bsm = getattr(CC, "forceRHDForBSM", False)
|
|
CS.enable_predicative_speed_limit = getattr(CC.cruiseControl, "speedLimitPredicative", False)
|
|
CS.enable_pred_react_to_speed_limits = getattr(CC.cruiseControl, "speedLimitPredReactToSL", False)
|
|
CS.enable_pred_react_to_curves = getattr(CC.cruiseControl, "speedLimitPredReactToCurves", False)
|
|
|
|
if self.frame % self.CCP.STEER_STEP == 0:
|
|
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
|
|
if CC.latActive:
|
|
hca_enabled = True
|
|
if CC.curvatureControllerActive:
|
|
apply_curvature = self.LateralController.update(CS.out, CC, actuators.curvature)
|
|
apply_curvature = apply_curvature + (CS.out.steeringCurvature - (CC.currentCurvature - CC.rollCompensation))
|
|
else:
|
|
apply_curvature = actuators.curvature + (CS.out.steeringCurvature - CC.currentCurvature)
|
|
apply_curvature = apply_std_curvature_limits(apply_curvature, self.apply_curvature_last, CS.out.vEgoRaw, CS.out.steeringCurvature,
|
|
CS.out.steeringPressed, self.CCP.STEER_STEP, CC.latActive, self.CCP.CURVATURE_LIMITS)
|
|
|
|
min_power = max(self.steering_power_last - self.CCP.STEERING_POWER_STEP, self.CCP.STEERING_POWER_MIN)
|
|
max_power = min(self.steering_power_last + self.CCP.STEERING_POWER_STEP, self.CCP.STEERING_POWER_MAX)
|
|
target_power_driver = int(np.interp(abs(CS.out.steeringTorque), [self.CCP.STEER_DRIVER_ALLOWANCE, self.CCP.STEER_DRIVER_MAX],
|
|
[self.CCP.STEERING_POWER_MAX, self.CCP.STEERING_POWER_MIN]))
|
|
target_power = int(np.interp(CS.out.vEgo, [0., 0.5], [self.CCP.STEERING_POWER_MIN, target_power_driver]))
|
|
steering_power = min(max(target_power, min_power), max_power)
|
|
else:
|
|
if self.LateralController is not None:
|
|
self.LateralController.reset()
|
|
if self.steering_power_last > 0:
|
|
hca_enabled = True
|
|
handoff_curvature = self.apply_curvature_last + (CS.out.steeringCurvature - self.apply_curvature_last) * self.CCP.CURVATURE_HANDOFF_RATE
|
|
apply_curvature = apply_std_curvature_limits(handoff_curvature, self.apply_curvature_last, CS.out.vEgoRaw, CS.out.steeringCurvature,
|
|
CS.out.steeringPressed, self.CCP.STEER_STEP, True, self.CCP.CURVATURE_LIMITS)
|
|
steering_power = max(self.steering_power_last - self.CCP.STEERING_POWER_STEP, 0)
|
|
else:
|
|
hca_enabled = False
|
|
apply_curvature = 0.
|
|
steering_power = 0
|
|
|
|
can_sends.append(self.CCS.create_steering_control(self.packer_pt, self.CAN.pt, apply_curvature, hca_enabled, steering_power))
|
|
self.apply_curvature_last = apply_curvature
|
|
self.steering_power_last = steering_power
|
|
else:
|
|
if CC.latActive and not AngleLateralControl:
|
|
torque_scale = self._get_mqb_steering_torque_scale(CS.out.vEgo, iq_mqb_steering_lockout)
|
|
new_torque = int(round(actuators.torque * self.CCP.STEER_MAX * torque_scale))
|
|
apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.CCP)
|
|
self.hca_frame_timer_running += self.CCP.STEER_STEP
|
|
if self.apply_torque_last == apply_torque:
|
|
self.hca_frame_same_torque += self.CCP.STEER_STEP
|
|
if self.hca_frame_same_torque > self.CCP.STEER_TIME_STUCK_TORQUE / DT_CTRL:
|
|
apply_torque -= (1, -1)[apply_torque < 0]
|
|
self.hca_frame_same_torque = 0
|
|
else:
|
|
self.hca_frame_same_torque = 0
|
|
hca_enabled = abs(apply_torque) > 0
|
|
if self.eps_timer_workaround and self.hca_frame_timer_running >= self.CCP.STEER_TIME_BM / DT_CTRL:
|
|
if abs(apply_torque) <= self.CCP.STEER_LOW_TORQUE:
|
|
self.hca_frame_low_torque += self.CCP.STEER_STEP
|
|
if self.hca_frame_low_torque >= self.CCP.STEER_TIME_LOW_TORQUE / DT_CTRL:
|
|
hca_enabled = False
|
|
else:
|
|
self.hca_frame_low_torque = 0
|
|
if self.hca_frame_timer_resetting > 0:
|
|
apply_torque = 0
|
|
else:
|
|
self.hca_frame_low_torque = 0
|
|
hca_enabled = False
|
|
apply_torque = 0
|
|
|
|
if hca_enabled:
|
|
output_torque = apply_torque
|
|
self.hca_frame_timer_resetting = 0
|
|
else:
|
|
output_torque = 0
|
|
self.hca_frame_timer_resetting += self.CCP.STEER_STEP
|
|
if self.hca_frame_timer_resetting >= self.CCP.STEER_TIME_RESET / DT_CTRL or not self.eps_timer_workaround:
|
|
self.hca_frame_timer_running = 0
|
|
apply_torque = 0
|
|
|
|
if hca_enabled and abs(apply_torque) > 0:
|
|
if pqhca5or7Toggle and (self.CP.flags & (VolkswagenFlags.PQ | VolkswagenFlags.MLB)):
|
|
self.HCA_Status = 7
|
|
else:
|
|
self.HCA_Status = 5
|
|
else:
|
|
self.HCA_Status = 3
|
|
|
|
self.eps_timer_soft_disable_alert = self.hca_frame_timer_running > self.CCP.STEER_TIME_ALERT / DT_CTRL
|
|
self.apply_torque_last = apply_torque
|
|
if not (AngleLateralControl and self.CCS in (mqbcan, pqcan, mlbcan)):
|
|
can_sends.append(self.CCS.create_hca_steering_control(self.packer_pt, self._pt_tx_bus, output_torque, self.HCA_Status))
|
|
|
|
if self.CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT and self.CCS == mqbcan:
|
|
ea_simulated_torque = float(np.clip(apply_torque * 2, -self.CCP.STEER_MAX, self.CCP.STEER_MAX))
|
|
if abs(CS.out.steeringTorque) > abs(ea_simulated_torque):
|
|
ea_simulated_torque = CS.out.steeringTorque
|
|
can_sends.append(self.CCS.create_eps_update(self.packer_pt, self.CAN.cam, CS.eps_stock_values, ea_simulated_torque))
|
|
|
|
self._iq_lvbs_alc.update_vw_alc(self, CC, CS, actuators, can_sends, apply_torque)
|
|
if self.frame % self.CCP.STEER_STEP == 0:
|
|
self._iq_lvbs_alc.append_private_apd(self, CC_IQ, can_sends)
|
|
|
|
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO) and self.CP.flags & VolkswagenFlags.STOCK_KLR_PRESENT:
|
|
if CS.klr_stock_values:
|
|
klr_send_ready = CS.klr_stock_values["COUNTER"] != self.klr_counter_last
|
|
if klr_send_ready:
|
|
can_sends.append(mebcan.create_capacitive_wheel_touch(self.packer_pt, self.CAN.cam, CC.latActive, CS.klr_stock_values))
|
|
can_sends.append(mebcan.create_capacitive_wheel_touch(self.packer_pt, self.CAN.pt, CC.latActive, CS.klr_stock_values))
|
|
self.klr_counter_last = CS.klr_stock_values["COUNTER"]
|
|
|
|
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
|
|
if CS.ea_hud_stock_values and self.frame % EA_BLINKER_STEP == 0:
|
|
self.ea_tx_counter = next_ea_counter(self.ea_tx_counter, CS.ea_hud_stock_values["COUNTER"])
|
|
left_blinker, right_blinker = ea_blinker_command(
|
|
CC.leftBlinker, CC.rightBlinker, CS.left_blinker_active, CS.right_blinker_active,
|
|
)
|
|
can_sends.append(self.CCS.create_blinker_control(self.packer_pt, self.CAN.pt, CS.ea_hud_stock_values, CS.ea_control_stock_values,
|
|
left_blinker, right_blinker, self.hide_ea_error, self.ea_tx_counter))
|
|
self.ea_counter_last = CS.ea_hud_stock_values["COUNTER"]
|
|
|
|
if self.CP.openpilotLongitudinalControl and self.CCS in (mqbcan, mlbcan) and not self.acc_counter_seeded and CS.acc_stock_counters:
|
|
seed_msgs = ("ACC_01", "ACC_02") if self.CCS is mlbcan else ("ACC_02", "ACC_06", "ACC_07", "ACC_10")
|
|
for name in seed_msgs:
|
|
addr = self.packer_pt.dbc.name_to_msg[name].address
|
|
self.packer_pt.counters[addr] = (CS.acc_stock_counters[name] + 1) % 16
|
|
self.acc_counter_seeded = True
|
|
|
|
if self.frame % self.CCP.ACC_CONTROL_STEP == 0 and self.CP.openpilotLongitudinalControl and not CS.out.radarDisableFailed:
|
|
stopping = actuators.longControlState == LongCtrlState.stopping
|
|
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
|
|
starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < 0.25)
|
|
accel = float(np.clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.enabled else 0)
|
|
|
|
long_override = CC.cruiseControl.override or CS.out.gasPressed
|
|
|
|
|
|
critical_state = hud_control.visualAlert == VisualAlert.fcw
|
|
if CC.longComfortMode and self.long_jerk_control is not None and self.long_limit_control is not None:
|
|
self.long_jerk_control.update(CC.enabled, long_override, hud_control.leadDistance, hud_control.leadVisible, accel, critical_state)
|
|
self.long_limit_control.update(CC.enabled, CS.out.vEgoRaw, hud_control.setSpeed, hud_control.leadDistance, hud_control.leadVisible, critical_state)
|
|
|
|
acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.enabled, long_override)
|
|
acc_hold_type, self.acc_hold_ramp_counter = self.CCS.acc_hold_type(
|
|
CS.out.cruiseState.available, CS.out.accFaulted, CC.enabled, starting, stopping,
|
|
CS.esp_hold_confirmation, CS.out.vEgo, self.acc_hold_type_last, self.acc_hold_ramp_counter)
|
|
self.acc_hold_type_last = acc_hold_type
|
|
can_sends.extend(self.CCS.create_acc_accel_control(
|
|
self.packer_pt, self.CAN.pt, self.CP, CS.acc_type, CC.enabled,
|
|
self.long_jerk_control.get_jerk_up() if CC.longComfortMode and self.long_jerk_control is not None else 4.0,
|
|
self.long_jerk_control.get_jerk_down() if CC.longComfortMode and self.long_jerk_control is not None else 4.0,
|
|
self.long_limit_control.get_upper_limit() if CC.longComfortMode and self.long_limit_control is not None else 0.,
|
|
self.long_limit_control.get_lower_limit() if CC.longComfortMode and self.long_limit_control is not None else 0.,
|
|
accel, acc_control, acc_hold_type, stopping, starting, CS.esp_hold_confirmation,
|
|
CS.out.vEgoRaw * CV.MS_TO_KPH, long_override, CS.travel_assist_available,
|
|
))
|
|
self.accel_last = accel
|
|
else:
|
|
stopping = actuators.longControlState == LongCtrlState.stopping
|
|
starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < 0.25)
|
|
long_active = CC.longActive
|
|
accel = accel_during_driver_override(actuators.accel, CS.out.gasPressed, self.CP_IQ.longActiveWithGasOverride)
|
|
esp_starting_override = None
|
|
esp_stopping_override = None
|
|
|
|
if self.CCS == mqbcan and CS.acc_type == 1 and self.standstill_manager is not None:
|
|
pitch = CC.orientationNED[1] if len(CC.orientationNED) == 3 else 0.0
|
|
long_active, accel, stopping, starting, esp_starting_override, esp_stopping_override = self.standstill_manager.update(
|
|
CS, long_active, accel, stopping, starting, float(getattr(actuators, "speed", 0.0)),
|
|
pitch, CS.tsk_brake_torque,
|
|
)
|
|
|
|
acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, long_active, CC.cruiseControl.override, CS.out.accFaulted)
|
|
accel = float(np.clip(accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if (long_active or CC.cruiseControl.override) else 0)
|
|
self.long_jerklimit = float(interp(accel, [-1.5, -0.3, 0.05], [2.0, 0.52, 0.30]))
|
|
self.long_deviation = float(interp(accel, [-0.3, 0.1], [0.0, 0.20]))
|
|
self.accel_last = accel
|
|
|
|
if blend_active and getattr(CC_IQ, "useRadarAccel", False) and CS.acc_radar_sta_adr == 1 and not self.radar_handler.failed:
|
|
accel = float(np.clip(CS.acc_radar_sollbeschl, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX))
|
|
self.long_jerklimit = CS.acc_radar_aendgrad
|
|
self.long_deviation = CS.acc_radar_regelabw
|
|
self.accel_last = accel
|
|
|
|
if self.CCS == mqbcan:
|
|
can_sends.extend(self.CCS.create_acc_accel_control(
|
|
self.packer_pt, self.CAN.pt, CS.acc_type, accel, acc_control, stopping, starting, CS.esp_hold_confirmation,
|
|
self.long_deviation, self.long_jerklimit, eBrakeActive,
|
|
esp_starting_override=esp_starting_override, esp_stopping_override=esp_stopping_override,
|
|
))
|
|
elif self.CCS == mlbcan:
|
|
can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, self.CAN.pt, accel, acc_control, stopping))
|
|
else:
|
|
accel = apply_pq_stopping_accel(self.CP.carFingerprint, accel, stopping)
|
|
|
|
sng_ecd_enabled = bool(self.CP.flags & VolkswagenFlagsIQ.IQ_PQ_SNG_ECD)
|
|
low_speed = CS.out.vEgo <= self.CCP.SNG_HANDOFF_SPEED
|
|
if sng_ecd_enabled and long_active and low_speed and accel <= 0.05 and not starting:
|
|
self.sng_handoff_active = True
|
|
else:
|
|
self.sng_handoff_active = False
|
|
sng_decel_req = float(np.clip(accel, self.CCP.ACCEL_MIN, self.CCP.SNG_HOLD_DECEL_MAX)) if self.sng_handoff_active else 0.0
|
|
|
|
can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, self.CAN.pt, CS.acc_type, accel, acc_control, stopping and not self.motor3_resuming, starting, CS.esp_hold_confirmation, self.long_deviation, self.long_jerklimit, eBrakeActive, sng_active=self.sng_handoff_active))
|
|
if sng_ecd_enabled:
|
|
can_sends.append(self.CCS.create_sng_handoff_control(self.packer_pt, self.CAN.aux, self.sng_handoff_active, sng_decel_req))
|
|
|
|
if (self.CP.flags & VolkswagenFlags.DISABLE_RADAR) and self.CP.openpilotLongitudinalControl and not CS.out.radarDisableFailed:
|
|
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
|
|
if self.radar_disabled_warning_timer < 600:
|
|
self.radar_disabled_warning_timer += 1
|
|
else:
|
|
self.hide_ea_error = True
|
|
|
|
if self.frame % self.CCP.AEB_CONTROL_STEP == 0:
|
|
can_sends.append(make_tester_present_msg(0x700, self.CAN.pt, suppress_response=True))
|
|
can_sends.append(self.CCS.create_aeb_control(self.packer_pt, self.CAN.pt, self.CP))
|
|
|
|
if self.frame % self.CCP.AEB_HUD_STEP == 0 and self._has_aeb_hud_msg:
|
|
can_sends.append(self.CCS.create_aeb_hud(self.packer_pt, self.CAN.pt, self.radar_disabled_warning_timer < 600))
|
|
|
|
if self.frame % 4 == 0:
|
|
can_sends.append(self.CCS.create_radar_objects(self.packer_pt, self.CAN.pt))
|
|
|
|
if self.frame % self.CCP.LDW_STEP == 0:
|
|
hud_alert = 0
|
|
if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) or CS.out.steerFaultTemporary:
|
|
hud_alert = self.CCP.LDW_MESSAGES["laneAssistTakeOver"]
|
|
steering_pressed_hud = (self.frame // 2) % 2 == 0 if self.entering else CS.out.steeringPressed
|
|
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
|
|
disable_alerts = getattr(CC, "disableCarSteerAlerts", False)
|
|
sound_alert = self.CCP.LDW_SOUNDS["Chime"] if hud_alert != 0 and not disable_alerts else self.CCP.LDW_SOUNDS["None"]
|
|
can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, self.CAN.pt, CS.ldw_stock_values, CC.latActive, CS.out.steeringPressed,
|
|
hud_alert, hud_control, sound_alert))
|
|
else:
|
|
can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, self._pt_tx_bus, CS.ldw_stock_values, CC.latActive, steering_pressed_hud,
|
|
hud_alert, hud_control, self.entering, self.CCS is pqcan and AngleLateralControl, self.active))
|
|
|
|
if hud_control.leadDistanceBars != self.lead_distance_bars_last:
|
|
self.distance_bar_frame = self.frame
|
|
|
|
if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl:
|
|
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
|
|
d_unresponsive = hud_control.driverUnresponsive
|
|
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
|
|
show_distance_bars = self.frame - self.distance_bar_frame < 400
|
|
gap = max(8, CS.out.vEgo * hud_control.leadFollowTime)
|
|
distance = max(8, hud_control.leadDistance) if hud_control.leadDistance != 0 else 0
|
|
acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.enabled,
|
|
CC.cruiseControl.override or CS.out.gasPressed)
|
|
|
|
sl_predicative_active = CC.cruiseControl.speedLimitPredicative and CS.out.cruiseState.speedLimitPredicative != 0
|
|
if CC.cruiseControl.speedLimit and CS.out.cruiseState.speedLimit != 0 and self.speed_limit_last != CS.out.cruiseState.speedLimit:
|
|
self.speed_limit_changed_timer = self.frame
|
|
self.speed_limit_last = CS.out.cruiseState.speedLimit
|
|
sl_active = self.frame - self.speed_limit_changed_timer < 400
|
|
speed_limit = CS.out.cruiseState.speedLimitPredicative if sl_predicative_active else (CS.out.cruiseState.speedLimit if sl_active else 0)
|
|
|
|
acc_hud_event = self.CCS.acc_hud_event(acc_hud_status, CS.esp_hold_confirmation, sl_predicative_active,
|
|
CS.speed_limit_predicative_type, sl_active)
|
|
|
|
can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, self.CAN.pt, acc_hud_status, hud_control.setSpeed * CV.MS_TO_KPH,
|
|
hud_control.leadVisible, hud_control.leadDistanceBars + 1, show_distance_bars,
|
|
CS.esp_hold_confirmation, distance, gap, fcw_alert, acc_hud_event, speed_limit))
|
|
else:
|
|
# MLB scales the raw lead distance against the set follow gap in the packer, the others clamp to a bar count
|
|
leadDistance = hud_control.leadDistance if self.CCS is mlbcan else \
|
|
(min(8, hud_control.leadDistance) if hud_control.leadDistance != 0 else 0)
|
|
self.leadDistanceBars = min(3, hud_control.leadDistanceBars)
|
|
acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive,
|
|
CC.cruiseControl.override or CS.out.gasPressed)
|
|
set_speed = hud_control.setSpeed * CV.MS_TO_KPH
|
|
decel = dVisual(self.CCS, CS)
|
|
hud_kwargs = {"hud_text": self._mlb_acc_hud_text(hud_control, set_speed),
|
|
"desired_distance": max(8.0, CS.out.vEgo * hud_control.leadFollowTime)} if self.CCS is mlbcan else {}
|
|
can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, self.CAN.pt, acc_hud_status, set_speed, leadDistance,
|
|
self.leadDistanceBars, fcw_alert, hud_control.leadVisible, self.unavailable,
|
|
decel, d_unresponsive, **hud_kwargs))
|
|
|
|
if self.CP.flags & VolkswagenFlags.PQ:
|
|
self._iq_lvbs_alc.update_turn_signals(self, CC, CS, can_sends)
|
|
|
|
if self.CP.openpilotLongitudinalControl and (self.CP.flags & VolkswagenFlags.PQ):
|
|
if blend_active:
|
|
can_sends.extend(self.radar_handler.update(
|
|
self.packer_pt, self.frame, CS,
|
|
blend_active=True,
|
|
engage_req=getattr(CC_IQ, "radarEngageReq", False),
|
|
cancel_req=getattr(CC_IQ, "radarCancelReq", False),
|
|
set_speed_kph=getattr(CC_IQ, "radarSetSpeedKph", 0.0),
|
|
gap_bars=getattr(CC_IQ, "radarGapBars", 0),
|
|
v_ego=CS.out.vEgo,
|
|
))
|
|
elif self.frame % 2 == 0:
|
|
can_sends.append(self.CCS.filter_motor2(self.packer_pt, self.CAN.ext, CS.motor2_stock))
|
|
can_sends.append(self.CCS.filter_motor5(self.packer_pt, self.CAN.ext, CS.motor5_stock))
|
|
|
|
gra_send_ready = self.CP.pcmCruise and CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last
|
|
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
|
|
main_cruise_latching = not bool(CS.gra_stock_values["GRA_Typ_Hauptschalter"])
|
|
stock_cancel_pressed = bool(CS.gra_stock_values["GRA_Abbrechen"] if main_cruise_latching else CS.gra_stock_values["GRA_Hauptschalter"])
|
|
elif self.CP.flags & VolkswagenFlags.MLB:
|
|
stock_cancel_pressed = bool(CS.gra_stock_values["LS_Abbrechen"])
|
|
else:
|
|
stock_cancel_pressed = bool(CS.gra_stock_values["GRA_Abbrechen"])
|
|
|
|
cancel_cmd = stock_cancel_pressed or self._tap_gra_cancel(CC.cruiseControl.cancel, gra_send_ready)
|
|
resume_cmd = CC.cruiseControl.resume or self._should_spam_mqb_a0_resume(CS, iq_mqb_acc_resume)
|
|
if gra_send_ready and (cancel_cmd or resume_cmd):
|
|
stalk_on_powertrain = self.CP.flags & VolkswagenFlags.PQ or self.CP.flags & VolkswagenFlagsIQ.IQ_MLB_NO_ECAN
|
|
bus_send = self.CAN.aux if stalk_on_powertrain else self.CAN.ext
|
|
can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, bus_send, CS.gra_stock_values,
|
|
cancel=cancel_cmd, resume=resume_cmd))
|
|
|
|
if self.CP.openpilotLongitudinalControl and self.CCS == pqcan and not blend_active:
|
|
if self.frame % 3:
|
|
can_sends.append(self.CCS.create_gra_neu(self.packer_pt, self.CAN.ext, CS.gra_stock_values, CC.longActive))
|
|
|
|
if self.CP.flags & VolkswagenFlagsIQ.IQ_PQ_ACC_FTS_EPB:
|
|
is_stopping = actuators.longControlState == LongCtrlState.stopping
|
|
if CS.out.vEgo > 0.5 or not CC.longActive:
|
|
self.motor3_resuming = False
|
|
elif (self.motor3_was_stopping and not is_stopping and self.CP.openpilotLongitudinalControl and CC.longActive):
|
|
self.motor3_resuming = True
|
|
if self.motor3_resuming and CS.motor3_stock:
|
|
can_sends.append(self.CCS.create_motor3_resume(self.packer_pt, self.CAN.aux, CS.motor1_stock, CS.motor3_stock, resume=True))
|
|
self.motor3_was_stopping = is_stopping
|
|
|
|
new_actuators = actuators.as_builder()
|
|
new_actuators.torque = self.apply_torque_last / self.CCP.STEER_MAX
|
|
new_actuators.torqueOutputCan = self.apply_torque_last
|
|
if self.CP.steerControlType == structs.CarParams.SteerControlType.angle:
|
|
new_actuators.steeringAngleDeg = float(self.apply_angle_last)
|
|
new_actuators.curvature = float(self.apply_curvature_last)
|
|
new_actuators.accel = self.accel_last
|
|
new_actuators.speed = float(getattr(actuators, "speed", 0.0))
|
|
|
|
self.lead_distance_bars_last = hud_control.leadDistanceBars
|
|
self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"]
|
|
self.motor3_frame_last = CS.motor3_frame
|
|
self.frame += 1
|
|
return new_actuators, can_sends
|