class TiciFanController(BaseFanController):
def __init__(self) -> None:
super().__init__()
cloudlog.info("Setting up TICI fan handler")
self.last_ignition = False
self.controller = PIDController(k_p=0,
k_i=4e-3,
k_f=1,
neg_limit=-80,
pos_limit=0,
rate=(1 / DT_TRML))
def update(self, max_cpu_temp: float, ignition: bool) -> int:
self.controller.neg_limit = -(80 if ignition else 30)
self.controller.pos_limit = -(30 if ignition else 0)
if ignition != self.last_ignition:
self.controller.reset()
error = 70 - max_cpu_temp
fan_pwr_out = -int(
self.controller.update(error=error,
feedforward=interp(
max_cpu_temp, [60.0, 100.0], [0, -80])))
self.last_ignition = ignition
return fan_pwr_out
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.pid = PIDController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=self.steer_max,
neg_limit=-self.steer_max)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
def __init__(self, CP):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = PIDController(
(CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
k_f=CP.longitudinalTuning.kf,
rate=1 / DT_CTRL)
self.v_pid = 0.0
self.last_output_accel = 0.0
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.pid = PIDController(CP.lateralTuning.torque.kp,
CP.lateralTuning.torque.ki,
k_f=CP.lateralTuning.torque.kf,
pos_limit=self.steer_max,
neg_limit=-self.steer_max)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle
self.friction = CP.lateralTuning.torque.friction
self.kf = CP.lateralTuning.torque.kf
def __init__(self) -> None:
super().__init__()
cloudlog.info("Setting up TICI fan handler")
self.last_ignition = False
self.controller = PIDController(k_p=0,
k_i=4e-3,
k_f=1,
neg_limit=-80,
pos_limit=0,
rate=(1 / DT_TRML))
class LatControlPID(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf,
k_d=(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
pos_limit=self.steer_max, neg_limit=-self.steer_max)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
self.errors = []
def reset(self):
super().reset()
self.pid.reset()
self.errors = []
def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
error = angle_steers_des - CS.steeringAngleDeg
pid_log.steeringAngleDesiredDeg = angle_steers_des
pid_log.angleError = error
if CS.vEgo < MIN_STEER_SPEED or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
# offset does not contribute to resistive torque
steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
error_rate = 0
if len(self.errors) >= ERROR_RATE_FRAME:
error_rate = (error - self.errors[-ERROR_RATE_FRAME]) / ERROR_RATE_FRAME
self.errors.append(float(error))
while len(self.errors) > ERROR_RATE_FRAME:
self.errors.pop(0)
output_steer = self.pid.update(error, error_rate, override=CS.steeringPressed,
feedforward=steer_feedforward, speed=CS.vEgo)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS)
return output_steer, angle_steers_des, pid_log
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.pid = PIDController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
neg_limit=-1.0,
sat_limit=CP.steerLimitTimer,
derivative_period=0.1)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 16., 35.]
kdV = [0.08, 0.215, 0.51]
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(CP.longitudinalTuning.kfBP, CP.longitudinalTuning.kfV),
(kdBP, kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 33, 55., 78]
kdBP = [i * CV.MPH_TO_MS for i in kdBP]
kdV = [0.05, 0.4, 0.8, 1.2]
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(kdBP, kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.lastdecelForTurn = False
#self.had_lead = False
self.last_output_gb = 0.0
def __init__(self, CP, compute_gb, candidate):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 16., 35.]
kdV = [0.05, 0.935, 1.65]
self.pid = PIDController(
(CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(kdBP, kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
self.op_params = opParams()
self.enable_dg = self.op_params.get('dynamic_gas', True)
self.dynamic_gas = DynamicGas(CP, candidate)
def __init__(self, dbc_name, CP, VM):
self.frame = 0
self.packer = CANPacker(dbc_name)
# Speed, balance and turn PIDs
self.speed_pid = PIDController(0.115, k_i=0.23, rate=1 / DT_CTRL)
self.balance_pid = PIDController(1300,
k_i=0,
k_d=280,
rate=1 / DT_CTRL)
self.turn_pid = PIDController(110, k_i=11.5, rate=1 / DT_CTRL)
self.torque_r_filtered = 0.
self.torque_l_filtered = 0.
def thermald_thread(end_event, hw_queue):
pm = messaging.PubMaster(['deviceState'])
sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "controlsState", "pandaStates"], poll=["pandaStates"])
fan_speed = 0
count = 0
onroad_conditions: Dict[str, bool] = {
"ignition": False,
}
startup_conditions: Dict[str, bool] = {}
startup_conditions_prev: Dict[str, bool] = {}
off_ts = None
started_ts = None
started_seen = False
thermal_status = ThermalStatus.green
usb_power = True
last_hw_state = HardwareState(
network_type=NetworkType.none,
network_strength=NetworkStrength.unknown,
network_info=None,
nvme_temps=[],
modem_temps=[],
)
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML)
should_start_prev = False
in_car = False
handle_fan = None
is_uno = False
engaged_prev = False
params = Params()
power_monitor = PowerMonitoring()
HARDWARE.initialize_hardware()
thermal_config = HARDWARE.get_thermal_config()
# TODO: use PI controller for UNO
controller = PIDController(k_p=0, k_i=2e-3, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML))
while not end_event.is_set():
sm.update(PANDA_STATES_TIMEOUT)
pandaStates = sm['pandaStates']
peripheralState = sm['peripheralState']
msg = read_thermal(thermal_config)
if sm.updated['pandaStates'] and len(pandaStates) > 0:
# Set ignition based on any panda connected
onroad_conditions["ignition"] = any(ps.ignitionLine or ps.ignitionCan for ps in pandaStates if ps.pandaType != log.PandaState.PandaType.unknown)
pandaState = pandaStates[0]
in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected
usb_power = peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client
# Setup fan handler on first connect to panda
if handle_fan is None and peripheralState.pandaType != log.PandaState.PandaType.unknown:
is_uno = peripheralState.pandaType == log.PandaState.PandaType.uno
if TICI:
cloudlog.info("Setting up TICI fan handler")
handle_fan = handle_fan_tici
elif is_uno or PC:
cloudlog.info("Setting up UNO fan handler")
handle_fan = handle_fan_uno
else:
cloudlog.info("Setting up EON fan handler")
setup_eon_fan()
handle_fan = handle_fan_eon
try:
last_hw_state = hw_queue.get_nowait()
except queue.Empty:
pass
msg.deviceState.freeSpacePercent = get_available_percent(default=100.0)
msg.deviceState.memoryUsagePercent = int(round(psutil.virtual_memory().percent))
msg.deviceState.cpuUsagePercent = [int(round(n)) for n in psutil.cpu_percent(percpu=True)]
msg.deviceState.gpuUsagePercent = int(round(HARDWARE.get_gpu_usage_percent()))
msg.deviceState.networkType = last_hw_state.network_type
msg.deviceState.networkStrength = last_hw_state.network_strength
if last_hw_state.network_info is not None:
msg.deviceState.networkInfo = last_hw_state.network_info
msg.deviceState.nvmeTempC = last_hw_state.nvme_temps
msg.deviceState.modemTempC = last_hw_state.modem_temps
msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness()
msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity()
msg.deviceState.batteryCurrent = HARDWARE.get_battery_current()
msg.deviceState.usbOnline = HARDWARE.get_usb_present()
current_filter.update(msg.deviceState.batteryCurrent / 1e6)
max_comp_temp = temp_filter.update(
max(max(msg.deviceState.cpuTempC), msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC))
)
if handle_fan is not None:
fan_speed = handle_fan(controller, max_comp_temp, fan_speed, onroad_conditions["ignition"])
msg.deviceState.fanSpeedPercentDesired = fan_speed
is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5))
if is_offroad_for_5_min and max_comp_temp > OFFROAD_DANGER_TEMP:
# If device is offroad we want to cool down before going onroad
# since going onroad increases load and can make temps go over 107
thermal_status = ThermalStatus.danger
else:
current_band = THERMAL_BANDS[thermal_status]
band_idx = list(THERMAL_BANDS.keys()).index(thermal_status)
if current_band.min_temp is not None and max_comp_temp < current_band.min_temp:
thermal_status = list(THERMAL_BANDS.keys())[band_idx - 1]
elif current_band.max_temp is not None and max_comp_temp > current_band.max_temp:
thermal_status = list(THERMAL_BANDS.keys())[band_idx + 1]
# **** starting logic ****
# Ensure date/time are valid
now = datetime.datetime.utcnow()
startup_conditions["time_valid"] = (now.year > 2020) or (now.year == 2020 and now.month >= 10)
set_offroad_alert_if_changed("Offroad_InvalidTime", (not startup_conditions["time_valid"]))
startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get_bool("DisableUpdates") or params.get_bool("SnoozeUpdate")
startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall")
startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version
# with 2% left, we killall, otherwise the phone will take a long time to boot
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \
params.get_bool("Passive")
startup_conditions["not_driver_view"] = not params.get_bool("IsDriverViewEnabled")
startup_conditions["not_taking_snapshot"] = not params.get_bool("IsTakingSnapshot")
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger
set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not onroad_conditions["device_temp_good"]))
if TICI:
missing = (not Path("/data/media").is_mount()) and (not os.path.isfile("/persist/comma/living-in-the-moment"))
set_offroad_alert_if_changed("Offroad_StorageMissing", missing)
# Handle offroad/onroad transition
should_start = all(onroad_conditions.values())
if started_ts is None:
should_start = should_start and all(startup_conditions.values())
if should_start != should_start_prev or (count == 0):
params.put_bool("IsOnroad", should_start)
params.put_bool("IsOffroad", not should_start)
params.put_bool("IsEngaged", False)
engaged_prev = False
HARDWARE.set_power_save(not should_start)
if sm.updated['controlsState']:
engaged = sm['controlsState'].enabled
if engaged != engaged_prev:
params.put_bool("IsEngaged", engaged)
engaged_prev = engaged
try:
with open('/dev/kmsg', 'w') as kmsg:
kmsg.write(f"<3>[thermald] engaged: {engaged}\n")
except Exception:
pass
if should_start:
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
else:
if onroad_conditions["ignition"] and (startup_conditions != startup_conditions_prev):
cloudlog.event("Startup blocked", startup_conditions=startup_conditions, onroad_conditions=onroad_conditions)
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
# Offroad power monitoring
power_monitor.calculate(peripheralState, onroad_conditions["ignition"])
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity())
current_power_draw = HARDWARE.get_current_power_draw() # pylint: disable=assignment-from-none
msg.deviceState.powerDrawW = current_power_draw if current_power_draw is not None else 0
# Check if we need to disable charging (handled by boardd)
msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(onroad_conditions["ignition"], in_car, off_ts)
# Check if we need to shut down
if power_monitor.should_shutdown(peripheralState, onroad_conditions["ignition"], in_car, off_ts, started_seen):
cloudlog.warning(f"shutting device down, offroad since {off_ts}")
params.put_bool("DoShutdown", True)
msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.deviceState.started = started_ts is not None
msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0))
last_ping = params.get("LastAthenaPingTime")
if last_ping is not None:
msg.deviceState.lastAthenaPingTime = int(last_ping)
msg.deviceState.thermalStatus = thermal_status
pm.send("deviceState", msg)
if EON and not is_uno:
set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power))
should_start_prev = should_start
startup_conditions_prev = startup_conditions.copy()
# Log to statsd
statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent)
statlog.gauge("gpu_usage_percent", msg.deviceState.gpuUsagePercent)
statlog.gauge("memory_usage_percent", msg.deviceState.memoryUsagePercent)
for i, usage in enumerate(msg.deviceState.cpuUsagePercent):
statlog.gauge(f"cpu{i}_usage_percent", usage)
for i, temp in enumerate(msg.deviceState.cpuTempC):
statlog.gauge(f"cpu{i}_temperature", temp)
for i, temp in enumerate(msg.deviceState.gpuTempC):
statlog.gauge(f"gpu{i}_temperature", temp)
statlog.gauge("memory_temperature", msg.deviceState.memoryTempC)
statlog.gauge("ambient_temperature", msg.deviceState.ambientTempC)
for i, temp in enumerate(msg.deviceState.pmicTempC):
statlog.gauge(f"pmic{i}_temperature", temp)
for i, temp in enumerate(last_hw_state.nvme_temps):
statlog.gauge(f"nvme_temperature{i}", temp)
for i, temp in enumerate(last_hw_state.modem_temps):
statlog.gauge(f"modem_temperature{i}", temp)
statlog.gauge("fan_speed_percent_desired", msg.deviceState.fanSpeedPercentDesired)
statlog.gauge("screen_brightness_percent", msg.deviceState.screenBrightnessPercent)
# report to server once every 10 minutes
if (count % int(600. / DT_TRML)) == 0:
if EON and started_ts is None and msg.deviceState.memoryUsagePercent > 40:
cloudlog.event("High offroad memory usage", mem=msg.deviceState.memoryUsagePercent)
cloudlog.event("STATUS_PACKET",
count=count,
pandaStates=[strip_deprecated_keys(p.to_dict()) for p in pandaStates],
peripheralState=strip_deprecated_keys(peripheralState.to_dict()),
location=(strip_deprecated_keys(sm["gpsLocationExternal"].to_dict()) if sm.alive["gpsLocationExternal"] else None),
deviceState=strip_deprecated_keys(msg.to_dict()))
count += 1
class LongControl():
def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 33, 55., 78]
kdBP = [i * CV.MPH_TO_MS for i in kdBP]
kdV = [0.05, 0.4, 0.8, 1.2]
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(kdBP, kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.lastdecelForTurn = False
#self.had_lead = False
self.last_output_gb = 0.0
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def dynamic_gas(self, v_ego, gas_interceptor, gas_button_status):
dynamic = False
if gas_interceptor:
if gas_button_status == 0:
dynamic = True
x = [0.0, 1.4082, 2.8031, 4.2266, 5.3827, 6.1656, 7.2478, 8.2831, 10.2447, 12.964, 15.423, 18.119, 20.117, 24.4661, 29.0581, 32.7101, 35.7633]
y = [0.218, 0.222, 0.233, 0.25, 0.273, 0.294, 0.337, 0.362, 0.38, 0.389, 0.398, 0.41, 0.421, 0.459, 0.512, 0.564, 0.621]
elif gas_button_status == 1:
y = [0.3, 0.9, 0.9]
elif gas_button_status == 2:
y = [0.2, 0.5, 0.7]
else:
if gas_button_status == 0:
dynamic = True
x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326]
y = [0.35, 0.47, 0.43, 0.35, 0.3, 0.3, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7]
elif gas_button_status == 1:
y = [0.5, 0.95, 0.99]
elif gas_button_status == 2:
y = [0.25, 0.2, 0.2]
if not dynamic:
x = [0., 9., 55.] # default BP values
accel = interp(v_ego, x, y)
#if dynamic and hasLead: # dynamic gas profile specific operations, and if lead
# if v_ego < 6.7056: # if under 15 mph
# x = [1.61479, 1.99067, 2.28537, 2.49888, 2.6312, 2.68224]
# y = [-accel, -(accel / 1.06), -(accel / 1.2), -(accel / 1.8), -(accel / 4.4), 0] # array that matches current chosen accel value
# accel += interp(vRel, x, y)
# else:
# x = [-0.89408, 0, 0.89408, 4.4704]
# y = [-.15, -.05, .005, .05]
# accel += interp(vRel, x, y)
min_return = 0.0
max_return = 1.0
return round(max(min(accel, max_return), min_return), 5) # ensure we return a value between range
def update(self, active, CS, v_target, v_target_future, a_target, CP, hasLead, radarState, decelForTurn, longitudinalPlanSource, gas_button_status):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
try:
gas_interceptor = CP.enableGasInterceptor
except AttributeError:
gas_interceptor = False
# Actuation limits
#gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
gas_max = self.dynamic_gas(CS.vEgo, gas_interceptor, gas_button_status)
brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
# Update state machine
output_gb = self.last_output_gb
if radarState is None:
dRel = 200
else:
dRel = radarState.leadOne.dRel
if hasLead:
stop = True if (dRel < 4.0 and radarState.leadOne.status) else False
else:
stop = False
self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
v_target_future, self.v_pid, output_gb,
CS.brakePressed, CS.cruiseState.standstill, stop)
v_ego_pid = max(CS.vEgo, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or (CS.brakePressed or CS.gasPressed and not travis):
self.v_pid = v_ego_pid
self.pid.reset()
output_gb = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = - brake_max
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
#if not self.had_lead and has_lead:
# if enableGasInterceptor:
# self.pid._k_p = ([0., 5., 35.], [1.2, 0.8, 0.5])
# self.pid._k_i = ([0., 35.], [0.18, 0.12])
# else:
# self.pid._k_p = ([0., 5., 35.], [3.6, 2.4, 1.5])
# self.pid._k_i = ([0., 35.], [0.54, 0.36])
#elif self.had_lead and not has_lead:
# self.pid._k_p = (CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV)
# self.pid._k_i = (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV)
#self.had_lead = has_lead
if longitudinalPlanSource == 'cruise':
if decelForTurn and not self.lastdecelForTurn:
self.lastdecelForTurn = True
self.pid._k_p = (CP.longitudinalTuning.kpBP, [x * 0 for x in CP.longitudinalTuning.kpV])
self.pid._k_i = (CP.longitudinalTuning.kiBP, [x * 0 for x in CP.longitudinalTuning.kiV])
self.pid.i = 0.0
self.pid.k_f=1.0
self.v_pid = CS.vEgo
self.pid.reset()
if self.lastdecelForTurn and not decelForTurn:
self.lastdecelForTurn = False
self.pid._k_p = (CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV)
self.pid._k_i = (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV)
self.pid.k_f=1.0
self.v_pid = CS.vEgo
self.pid.reset()
else:
if self.lastdecelForTurn:
self.v_pid = CS.vEgo
self.pid.reset()
self.lastdecelForTurn = False
self.pid._k_p = (CP.longitudinalTuning.kpBP, [x * 1 for x in CP.longitudinalTuning.kpV])
self.pid._k_i = (CP.longitudinalTuning.kiBP, [x * 1 for x in CP.longitudinalTuning.kiV])
self.pid.k_f=1.0
output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
if prevent_overshoot:
output_gb = min(output_gb, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
factor = 1
if hasLead:
factor = interp(dRel,[2.0,3.0,4.0,5.0,6.0,7.0,8.0], [3.0,2.1,1.5,1.0,0.6,0.29,0.0])
if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= STOPPING_BRAKE_RATE / RATE * factor
output_gb = clip(output_gb, -brake_max, gas_max)
self.v_pid = CS.vEgo
self.pid.reset()
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
factor = 1
if hasLead:
factor = interp(dRel,[0.0,2.0,4.0,6.0], [0.0,0.5,1.0,2.0])
if output_gb < -0.2:
output_gb += STARTING_BRAKE_RATE / RATE * factor
self.v_pid = CS.vEgo
self.pid.reset()
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
return final_gas, final_brake
class LatControlPID(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.pid = PIDController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
(CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
neg_limit=-1.0,
sat_limit=CP.steerLimitTimer,
derivative_period=0.1)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
def reset(self):
super().reset()
self.pid.reset()
def update(self, active, CS, CP, VM, params, desired_curvature,
desired_curvature_rate):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
angle_steers_des_no_offset = math.degrees(
VM.get_steer_from_curvature(-desired_curvature, CS.vEgo,
params.roll))
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
pid_log.steeringAngleDesiredDeg = angle_steers_des
pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
if CS.vEgo < MIN_STEER_SPEED or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
# offset does not contribute to resistive torque
steer_feedforward = self.get_steer_feedforward(
angle_steers_des_no_offset, CS.vEgo)
# torque for steer rate. ~0 angle, steer rate ~= steer command.
steer_rate_actual = CS.steeringRateDeg
steer_rate_desired = math.degrees(
VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo,
0))
speed_mph = CS.vEgo * CV.MS_TO_MPH
steer_rate_max = 0.0389837 * speed_mph**2 - 5.34858 * speed_mph + 223.831
steer_feedforward += ((steer_rate_desired - steer_rate_actual) /
steer_rate_max)
deadzone = 0.0
check_saturation = (
CS.vEgo >
10) and not CS.steeringRateLimited and not CS.steeringPressed
output_steer = self.pid.update(angle_steers_des,
CS.steeringAngleDeg,
check_saturation=check_saturation,
override=CS.steeringPressed,
feedforward=steer_feedforward,
speed=CS.vEgo,
deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = self._check_saturation(
steers_max - abs(output_steer) < 1e-3, CS)
return output_steer, angle_steers_des, pid_log
class LongControl():
def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 16., 35.]
kdV = [0.08, 0.215, 0.51]
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(CP.longitudinalTuning.kfBP, CP.longitudinalTuning.kfV),
(kdBP, kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
self.stop = False
self.stop_timer = 0
def update(self, active, CS, v_target, v_target_future, a_target, CP, hasLead, radarState):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits
gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
# Update state machine
output_gb = self.last_output_gb
if radarState is None:
dRel = 200
vLead = 0
else:
dRel = radarState.leadOne.dRel
vLead = radarState.leadOne.vLead
if hasLead and dRel < 5. and radarState.leadOne.status:
self.stop = True
if self.stop:
self.stop_timer = 100
elif self.stop_timer > 0:
self.stop_timer -= 1
else:
self.stop = False
self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo, v_target,
output_gb, CS.standstill, self.stop, vLead)
v_ego_pid = max(CS.vEgo, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.v_pid = v_ego_pid
self.pid.reset()
output_gb = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = - brake_max
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target,
freeze_integrator=prevent_overshoot, leadvisible=hasLead, leaddistance=dRel, leadvel=vLead)
if prevent_overshoot:
output_gb = min(output_gb, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
factor = 1.
if hasLead:
factor = interp(dRel, [2., 3., 4., 5., 6., 7., 8.], [5., 2.5, 1., .5, .25, .05, .005])
if output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= (STOPPING_BRAKE_RATE * factor) / RATE
if output_gb < -.5 and CS.standstill:
output_gb += .033
output_gb = clip(output_gb, -brake_max, gas_max)
self.v_pid = CS.vEgo
self.pid.reset()
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
factor = 1.
if hasLead:
factor = interp(dRel, [0., 2., 4., 6.], [2., 2., 2., 3.])
if output_gb < 2.:
output_gb += (STARTING_BRAKE_RATE * factor) / RATE
self.v_pid = CS.vEgo
self.pid.reset()
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
return final_gas, final_brake
class LatControlTorque(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.pid = PIDController(CP.lateralTuning.torque.kp,
CP.lateralTuning.torque.ki,
k_f=CP.lateralTuning.torque.kf,
pos_limit=self.steer_max,
neg_limit=-self.steer_max)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle
self.friction = CP.lateralTuning.torque.friction
self.kf = CP.lateralTuning.torque.kf
self.steering_angle_deadzone_deg = CP.lateralTuning.torque.steeringAngleDeadzoneDeg
def update(self, active, CS, VM, params, last_actuators, desired_curvature,
desired_curvature_rate, llk):
pid_log = log.ControlsState.LateralTorqueState.new_message()
if CS.vEgo < MIN_STEER_SPEED or not active:
output_torque = 0.0
pid_log.active = False
else:
if self.use_steering_angle:
actual_curvature = -VM.calc_curvature(
math.radians(CS.steeringAngleDeg - params.angleOffsetDeg),
CS.vEgo, params.roll)
curvature_deadzone = abs(
VM.calc_curvature(
math.radians(self.steering_angle_deadzone_deg),
CS.vEgo, 0.0))
else:
actual_curvature_vm = -VM.calc_curvature(
math.radians(CS.steeringAngleDeg - params.angleOffsetDeg),
CS.vEgo, params.roll)
actual_curvature_llk = llk.angularVelocityCalibrated.value[
2] / CS.vEgo
actual_curvature = interp(
CS.vEgo, [2.0, 5.0],
[actual_curvature_vm, actual_curvature_llk])
curvature_deadzone = 0.0
desired_lateral_accel = desired_curvature * CS.vEgo**2
# desired rate is the desired rate of change in the setpoint, not the absolute desired curvature
#desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2
actual_lateral_accel = actual_curvature * CS.vEgo**2
lateral_accel_deadzone = curvature_deadzone * CS.vEgo**2
low_speed_factor = interp(CS.vEgo, [0, 15], [500, 0])
setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
measurement = actual_lateral_accel + low_speed_factor * actual_curvature
error = apply_deadzone(setpoint - measurement,
lateral_accel_deadzone)
pid_log.error = error
ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
# convert friction into lateral accel units for feedforward
friction_compensation = interp(
error, [-FRICTION_THRESHOLD, FRICTION_THRESHOLD],
[-self.friction, self.friction])
ff += friction_compensation / self.kf
freeze_integrator = CS.steeringRateLimited or CS.steeringPressed or CS.vEgo < 5
output_torque = self.pid.update(
error,
feedforward=ff,
speed=CS.vEgo,
freeze_integrator=freeze_integrator)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.d = self.pid.d
pid_log.f = self.pid.f
pid_log.output = -output_torque
pid_log.saturated = self._check_saturation(
self.steer_max - abs(output_torque) < 1e-3, CS)
pid_log.actualLateralAccel = actual_lateral_accel
pid_log.desiredLateralAccel = desired_lateral_accel
# TODO left is positive in this convention
return -output_torque, 0.0, pid_log
class LongControl():
def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
kdBP = [0., 16., 35.]
kdV = [0.08, 0.215, 0.51]
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
(CP.longitudinalTuning.kfBP, CP.longitudinalTuning.kfV),
(kdBP, kdV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
self.v_pid = 0.0
self.last_output_gb = 0.0
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def update(self, active, CS, v_target, v_target_future, a_target, CP):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits
gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
# Update state machine
output_gb = self.last_output_gb
self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
v_target_future, self.v_pid, output_gb,
CS.brakePressed, CS.cruiseState.standstill)
v_ego_pid = max(CS.vEgo, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.reset(v_ego_pid)
output_gb = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
self.pid.pos_limit = gas_max
self.pid.neg_limit = - brake_max
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
if prevent_overshoot:
output_gb = min(output_gb, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= STOPPING_BRAKE_RATE / RATE
output_gb = clip(output_gb, -brake_max, gas_max)
self.reset(CS.vEgo)
# Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting:
if output_gb < -0.2:
output_gb += STARTING_BRAKE_RATE / RATE
self.reset(CS.vEgo)
self.last_output_gb = output_gb
final_gas = clip(output_gb, 0., gas_max)
final_brake = -clip(output_gb, -brake_max, 0.)
return final_gas, final_brake
class CarController():
def __init__(self, dbc_name, CP, VM):
self.frame = 0
self.packer = CANPacker(dbc_name)
# Speed, balance and turn PIDs
self.speed_pid = PIDController(0.115, k_i=0.23, rate=1 / DT_CTRL)
self.balance_pid = PIDController(1300,
k_i=0,
k_d=280,
rate=1 / DT_CTRL)
self.turn_pid = PIDController(110, k_i=11.5, rate=1 / DT_CTRL)
self.torque_r_filtered = 0.
self.torque_l_filtered = 0.
@staticmethod
def deadband_filter(torque, deadband):
if torque > 0:
torque += deadband
else:
torque -= deadband
return torque
def update(self, CC, CS):
torque_l = 0
torque_r = 0
llk_valid = len(CC.orientationNED) > 0 and len(CC.angularVelocity) > 0
if CC.enabled and llk_valid:
# Read these from the joystick
# TODO: this isn't acceleration, okay?
speed_desired = CC.actuators.accel / 5.
speed_diff_desired = -CC.actuators.steer
speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl +
CS.out.wheelSpeeds.fr) / 2.
speed_error = speed_desired - speed_measured
freeze_integrator = (
(speed_error < 0
and self.speed_pid.error_integral <= -MAX_POS_INTEGRATOR)
or (speed_error > 0
and self.speed_pid.error_integral >= MAX_POS_INTEGRATOR))
angle_setpoint = self.speed_pid.update(
speed_error, freeze_integrator=freeze_integrator)
# Clip angle error, this is enough to get up from stands
angle_error = np.clip((-CC.orientationNED[1]) - angle_setpoint,
-MAX_ANGLE_ERROR, MAX_ANGLE_ERROR)
angle_error_rate = np.clip(-CC.angularVelocity[1], -1., 1.)
torque = self.balance_pid.update(angle_error,
error_rate=angle_error_rate)
speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl -
CS.out.wheelSpeeds.fr)
turn_error = speed_diff_measured - speed_diff_desired
freeze_integrator = (
(turn_error < 0
and self.turn_pid.error_integral <= -MAX_POS_INTEGRATOR)
or (turn_error > 0
and self.turn_pid.error_integral >= MAX_POS_INTEGRATOR))
torque_diff = self.turn_pid.update(
turn_error, freeze_integrator=freeze_integrator)
# Combine 2 PIDs outputs
torque_r = torque + torque_diff
torque_l = torque - torque_diff
# Torque rate limits
self.torque_r_filtered = np.clip(
self.deadband_filter(torque_r, 10),
self.torque_r_filtered - MAX_TORQUE_RATE,
self.torque_r_filtered + MAX_TORQUE_RATE)
self.torque_l_filtered = np.clip(
self.deadband_filter(torque_l, 10),
self.torque_l_filtered - MAX_TORQUE_RATE,
self.torque_l_filtered + MAX_TORQUE_RATE)
torque_r = int(
np.clip(self.torque_r_filtered, -MAX_TORQUE, MAX_TORQUE))
torque_l = int(
np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE))
can_sends = []
can_sends.append(
bodycan.create_control(self.packer, torque_l, torque_r,
self.frame // 2))
new_actuators = CC.actuators.copy()
new_actuators.accel = torque_l
new_actuators.steer = torque_r
self.frame += 1
return new_actuators, can_sends
class LatControlTorque(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.pid = PIDController(CP.lateralTuning.torque.kp,
CP.lateralTuning.torque.ki,
k_d=CP.lateralTuning.torque.kd,
k_f=CP.lateralTuning.torque.kf,
pos_limit=self.steer_max,
neg_limit=-self.steer_max)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle
self.friction = CP.lateralTuning.torque.friction
self.kf = CP.lateralTuning.torque.kf
self.deadzone = CP.lateralTuning.torque.deadzone
self.errors = []
self.tune = nTune(CP, self)
def reset(self):
super().reset()
#self.pid.reset()
self.errors = []
def update(self, active, CS, VM, params, last_actuators, desired_curvature,
desired_curvature_rate, llk):
self.tune.check()
pid_log = log.ControlsState.LateralTorqueState.new_message()
if CS.vEgo < MIN_STEER_SPEED or not active:
output_torque = 0.0
pid_log.active = False
angle_steers_des = 0.0
self.errors = []
else:
if self.use_steering_angle:
actual_curvature = -VM.calc_curvature(
math.radians(CS.steeringAngleDeg - params.angleOffsetDeg),
CS.vEgo, params.roll)
else:
actual_curvature_vm = -VM.calc_curvature(
math.radians(CS.steeringAngleDeg - params.angleOffsetDeg),
CS.vEgo, params.roll)
actual_curvature_llk = llk.angularVelocityCalibrated.value[
2] / CS.vEgo
actual_curvature = interp(
CS.vEgo, [2.0, 5.0],
[actual_curvature_vm, actual_curvature_llk])
desired_lateral_accel = desired_curvature * CS.vEgo**2
#desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2
actual_lateral_accel = actual_curvature * CS.vEgo**2
setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature
measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature
error = apply_deadzone(setpoint - measurement, self.deadzone)
error_rate = 0
if len(self.errors) >= ERROR_RATE_FRAME:
error_rate = (
error - self.errors[-ERROR_RATE_FRAME]) / ERROR_RATE_FRAME
self.errors.append(float(error))
while len(self.errors) > ERROR_RATE_FRAME:
self.errors.pop(0)
pid_log.error = error
ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
# convert friction into lateral accel units for feedforward
friction_compensation = interp(error,
[-JERK_THRESHOLD, JERK_THRESHOLD],
[-self.friction, self.friction])
ff += friction_compensation / self.kf
freeze_integrator = CS.steeringRateLimited or CS.steeringPressed or CS.vEgo < 5
output_torque = self.pid.update(
error,
error_rate,
feedforward=ff,
speed=CS.vEgo,
freeze_integrator=freeze_integrator)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.d = self.pid.d
pid_log.f = self.pid.f
pid_log.output = -output_torque
pid_log.saturated = self._check_saturation(
self.steer_max - abs(output_torque) < 1e-3, CS)
pid_log.actualLateralAccel = actual_lateral_accel
pid_log.desiredLateralAccel = desired_lateral_accel
angle_steers_des = math.degrees(
VM.get_steer_from_curvature(
-desired_curvature, CS.vEgo,
params.roll)) + params.angleOffsetDeg
# TODO left is positive in this convention
return -output_torque, angle_steers_des, pid_log
class LatControlTorque(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.pid = PIDController(CP.lateralTuning.torque.kp,
CP.lateralTuning.torque.ki,
k_f=CP.lateralTuning.torque.kf,
pos_limit=self.steer_max,
neg_limit=-self.steer_max)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle
self.friction = CP.lateralTuning.torque.friction
self.kf = CP.lateralTuning.torque.kf
def reset(self):
super().reset()
self.pid.reset()
def update(self, active, CS, VM, params, last_actuators, desired_curvature,
desired_curvature_rate, llk):
pid_log = log.ControlsState.LateralTorqueState.new_message()
if CS.vEgo < MIN_STEER_SPEED or not active:
output_torque = 0.0
pid_log.active = False
if not active:
self.pid.reset()
else:
if self.use_steering_angle:
actual_curvature = -VM.calc_curvature(
math.radians(CS.steeringAngleDeg - params.angleOffsetDeg),
CS.vEgo, params.roll)
else:
actual_curvature = llk.angularVelocityCalibrated.value[
2] / CS.vEgo
desired_lateral_accel = desired_curvature * CS.vEgo**2
desired_lateral_jerk = desired_curvature_rate * CS.vEgo**2
actual_lateral_accel = actual_curvature * CS.vEgo**2
setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature
measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature
error = setpoint - measurement
pid_log.error = error
ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
# convert friction into lateral accel units for feedforward
friction_compensation = interp(desired_lateral_jerk,
[-JERK_THRESHOLD, JERK_THRESHOLD],
[-self.friction, self.friction])
ff += friction_compensation / self.kf
output_torque = self.pid.update(
error,
override=CS.steeringPressed,
feedforward=ff,
speed=CS.vEgo,
freeze_integrator=CS.steeringRateLimited)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.d = self.pid.d
pid_log.f = self.pid.f
pid_log.output = -output_torque
pid_log.saturated = self._check_saturation(
self.steer_max - abs(output_torque) < 1e-3, CS)
pid_log.actualLateralAccel = actual_lateral_accel
pid_log.desiredLateralAccel = desired_lateral_accel
# TODO left is positive in this convention
return -output_torque, 0.0, pid_log
class LongControl:
def __init__(self, CP):
self.CP = CP
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = PIDController(
(CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
k_f=CP.longitudinalTuning.kf,
rate=1 / DT_CTRL)
self.v_pid = 0.0
self.last_output_accel = 0.0
def reset(self, v_pid):
"""Reset PID controller and change setpoint"""
self.pid.reset()
self.v_pid = v_pid
def update(self, active, CS, long_plan, accel_limits, t_since_plan,
radar_state):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Interp control trajectory
speeds = long_plan.speeds
if len(speeds) == CONTROL_N:
v_target = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds)
a_target = interp(t_since_plan, T_IDXS[:CONTROL_N],
long_plan.accels)
v_target_lower = interp(
self.CP.longitudinalActuatorDelayLowerBound + t_since_plan,
T_IDXS[:CONTROL_N], speeds)
a_target_lower = 2 * (
v_target_lower - v_target
) / self.CP.longitudinalActuatorDelayLowerBound - a_target
v_target_upper = interp(
self.CP.longitudinalActuatorDelayUpperBound + t_since_plan,
T_IDXS[:CONTROL_N], speeds)
a_target_upper = 2 * (
v_target_upper - v_target
) / self.CP.longitudinalActuatorDelayUpperBound - a_target
a_target = min(a_target_lower, a_target_upper)
v_target_future = speeds[-1]
else:
v_target = 0.0
v_target_future = 0.0
a_target = 0.0
# TODO: This check is not complete and needs to be enforced by MPC
a_target = clip(a_target, ACCEL_MIN_ISO, ACCEL_MAX_ISO)
self.pid.neg_limit = accel_limits[0]
self.pid.pos_limit = accel_limits[1]
# Update state machine
output_accel = self.last_output_accel
self.long_control_state = long_control_state_trans(
self.CP, active, self.long_control_state, CS.vEgo, v_target,
v_target_future, CS.brakePressed, CS.cruiseState.standstill,
radar_state)
if self.long_control_state == LongCtrlState.off:
self.reset(CS.vEgo)
output_accel = 0.
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
self.v_pid = v_target
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid
deadzone = interp(CS.vEgo, self.CP.longitudinalTuning.deadzoneBP,
self.CP.longitudinalTuning.deadzoneV)
freeze_integrator = prevent_overshoot
error = self.v_pid - CS.vEgo
error_deadzone = apply_deadzone(error, deadzone)
output_accel = self.pid.update(error_deadzone,
speed=CS.vEgo,
feedforward=a_target,
freeze_integrator=freeze_integrator)
if prevent_overshoot:
output_accel = min(output_accel, 0.0)
# Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped
if not CS.standstill or output_accel > self.CP.stopAccel:
output_accel -= self.CP.stoppingDecelRate * DT_CTRL * \
interp(output_accel, [self.CP.stopAccel, self.CP.stopAccel/2., self.CP.stopAccel/4., 0.], [0.3, 0.65, 1., 2.])
output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
self.reset(CS.vEgo)
self.last_output_accel = output_accel
final_accel = clip(output_accel, accel_limits[0], accel_limits[1])
return final_accel
