def __init__(self, dbc_name, CP, VM):
self.apply_steer_last = 0
self.prev_frame = -1
self.lkas_frame = -1
self.prev_lkas_counter = -1
self.hud_count = 0
self.car_fingerprint = CP.carFingerprint
self.torq_enabled = False
self.steer_rate_limited = False
self.last_button_counter = -1
self.button_frame = -1
self.packer = CANPacker(dbc_name)
self.params = Params()
self.cachedParams = CachedParams()
self.opParams = opParams()
self.auto_resume = self.params.get_bool('jvePilot.settings.autoResume')
self.minAccSetting = V_CRUISE_MIN_MS if self.params.get_bool(
"IsMetric") else V_CRUISE_MIN_IMPERIAL_MS
self.round_to_unit = CV.MS_TO_KPH if self.params.get_bool(
"IsMetric") else CV.MS_TO_MPH
self.autoFollowDistanceLock = None
self.moving_fast = False
self.min_steer_check = self.opParams.get("steer.checkMinimum")
def __init__(self, CP, init_v=0.0, init_a=0.0):
self.CP = CP
self.mpc = LongitudinalMpc()
self.fcw = False
self.cachedParams = CachedParams()
self.v_desired = init_v
self.a_desired = init_a
self.alpha = np.exp(-DT_MDL / 2.0)
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.j_desired_trajectory = np.zeros(CONTROL_N)
def __init__(self, wide_camera=False):
self.ll_t = np.zeros((TRAJECTORY_SIZE, ))
self.ll_x = np.zeros((TRAJECTORY_SIZE, ))
self.lll_y = np.zeros((TRAJECTORY_SIZE, ))
self.rll_y = np.zeros((TRAJECTORY_SIZE, ))
self.lane_width_estimate = FirstOrderFilter(3.7, 9.95, DT_MDL)
self.lane_width_certainty = FirstOrderFilter(1.0, 0.95, DT_MDL)
self.lane_width = 3.7
self.lll_prob = 0.
self.rll_prob = 0.
self.d_prob = 0.
self.lll_std = 0.
self.rll_std = 0.
self.l_lane_change_prob = 0.
self.r_lane_change_prob = 0.
self.camera_offset = -CAMERA_OFFSET if wide_camera else CAMERA_OFFSET
self.path_offset = -PATH_OFFSET if wide_camera else PATH_OFFSET
self.cachedParams = CachedParams()
#!/usr/bin/env python3
from cereal import car
from selfdrive.car.chrysler.values import CAR
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from common.cached_params import CachedParams
from common.op_params import opParams
ButtonType = car.CarState.ButtonEvent.Type
GAS_RESUME_SPEED = 2.
cachedParams = CachedParams()
opParams = opParams()
class CarInterface(CarInterfaceBase):
@staticmethod
def get_pid_accel_limits(CP, current_speed, cruise_speed):
return 10., 10. # high limits
@staticmethod
def get_params(candidate,
fingerprint=gen_empty_fingerprint(),
car_fw=None):
min_steer_check = opParams.get('steer.checkMinimum')
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
ret.carName = "chrysler"
ret.safetyConfigs = [
get_safety_config(car.CarParams.SafetyModel.chrysler)
]
class LanePlanner:
def __init__(self, wide_camera=False):
self.ll_t = np.zeros((TRAJECTORY_SIZE, ))
self.ll_x = np.zeros((TRAJECTORY_SIZE, ))
self.lll_y = np.zeros((TRAJECTORY_SIZE, ))
self.rll_y = np.zeros((TRAJECTORY_SIZE, ))
self.lane_width_estimate = FirstOrderFilter(3.7, 9.95, DT_MDL)
self.lane_width_certainty = FirstOrderFilter(1.0, 0.95, DT_MDL)
self.lane_width = 3.7
self.lll_prob = 0.
self.rll_prob = 0.
self.d_prob = 0.
self.lll_std = 0.
self.rll_std = 0.
self.l_lane_change_prob = 0.
self.r_lane_change_prob = 0.
self.camera_offset = -CAMERA_OFFSET if wide_camera else CAMERA_OFFSET
self.path_offset = -PATH_OFFSET if wide_camera else PATH_OFFSET
self.cachedParams = CachedParams()
def parse_model(self, md):
if len(md.laneLines) == 4 and len(
md.laneLines[0].t) == TRAJECTORY_SIZE:
self.ll_t = (np.array(md.laneLines[1].t) +
np.array(md.laneLines[2].t)) / 2
# left and right ll x is the same
self.ll_x = md.laneLines[1].x
# only offset left and right lane lines; offsetting path does not make sense
device_offset = self.cachedParams.get_float(
'jvePilot.settings.deviceOffset', 5000)
self.lll_y = np.array(
md.laneLines[1].y) - (self.camera_offset + device_offset)
self.rll_y = np.array(
md.laneLines[2].y) - (self.camera_offset + device_offset)
self.lll_prob = md.laneLineProbs[1]
self.rll_prob = md.laneLineProbs[2]
self.lll_std = md.laneLineStds[1]
self.rll_std = md.laneLineStds[2]
if len(md.meta.desireState):
self.l_lane_change_prob = md.meta.desireState[
log.LateralPlan.Desire.laneChangeLeft]
self.r_lane_change_prob = md.meta.desireState[
log.LateralPlan.Desire.laneChangeRight]
def get_d_path(self, v_ego, path_t, path_xyz):
# Reduce reliance on lanelines that are too far apart or
# will be in a few seconds
path_xyz[:, 1] -= self.path_offset
l_prob, r_prob = self.lll_prob, self.rll_prob
width_pts = self.rll_y - self.lll_y
prob_mods = []
for t_check in [0.0, 1.5, 3.0]:
width_at_t = interp(t_check * (v_ego + 7), self.ll_x, width_pts)
prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0]))
mod = min(prob_mods)
l_prob *= mod
r_prob *= mod
# Reduce reliance on uncertain lanelines
l_std_mod = interp(self.lll_std, [.15, .3], [1.0, 0.0])
r_std_mod = interp(self.rll_std, [.15, .3], [1.0, 0.0])
l_prob *= l_std_mod
r_prob *= r_std_mod
# Find current lanewidth
self.lane_width_certainty.update(l_prob * r_prob)
current_lane_width = abs(self.rll_y[0] - self.lll_y[0])
self.lane_width_estimate.update(current_lane_width)
speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
self.lane_width = self.lane_width_certainty.x * self.lane_width_estimate.x + \
(1 - self.lane_width_certainty.x) * speed_lane_width
clipped_lane_width = min(4.0, self.lane_width)
path_from_left_lane = self.lll_y + clipped_lane_width / 2.0
path_from_right_lane = self.rll_y - clipped_lane_width / 2.0
self.d_prob = l_prob + r_prob - l_prob * r_prob
lane_path_y = (l_prob * path_from_left_lane + r_prob *
path_from_right_lane) / (l_prob + r_prob + 0.0001)
safe_idxs = np.isfinite(self.ll_t)
if safe_idxs[0]:
lane_path_y_interp = np.interp(path_t, self.ll_t[safe_idxs],
lane_path_y[safe_idxs])
path_xyz[:, 1] = self.d_prob * lane_path_y_interp + (
1.0 - self.d_prob) * path_xyz[:, 1]
else:
cloudlog.warning("Lateral mpc - NaNs in laneline times, ignoring")
return path_xyz
class CarController():
def __init__(self, dbc_name, CP, VM):
self.apply_steer_last = 0
self.prev_frame = -1
self.lkas_frame = -1
self.prev_lkas_counter = -1
self.hud_count = 0
self.car_fingerprint = CP.carFingerprint
self.torq_enabled = False
self.steer_rate_limited = False
self.last_button_counter = -1
self.button_frame = -1
self.packer = CANPacker(dbc_name)
self.params = Params()
self.cachedParams = CachedParams()
self.opParams = opParams()
self.auto_resume = self.params.get_bool('jvePilot.settings.autoResume')
self.minAccSetting = V_CRUISE_MIN_MS if self.params.get_bool(
"IsMetric") else V_CRUISE_MIN_IMPERIAL_MS
self.round_to_unit = CV.MS_TO_KPH if self.params.get_bool(
"IsMetric") else CV.MS_TO_MPH
self.autoFollowDistanceLock = None
self.moving_fast = False
self.min_steer_check = self.opParams.get("steer.checkMinimum")
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert,
gas_resume_speed, c):
if CS.button_pressed(ButtonType.lkasToggle, False):
c.jvePilotState.carControl.useLaneLines = not c.jvePilotState.carControl.useLaneLines
self.params.put(
"EndToEndToggle",
"0" if c.jvePilotState.carControl.useLaneLines else "1")
c.jvePilotState.notifyUi = True
#*** control msgs ***
can_sends = []
self.lkas_control(CS, actuators, can_sends, enabled, hud_alert,
c.jvePilotState)
self.wheel_button_control(CS, can_sends, enabled, gas_resume_speed,
c.jvePilotState, pcm_cancel_cmd)
return can_sends
def lkas_control(self, CS, actuators, can_sends, enabled, hud_alert,
jvepilot_state):
if self.prev_frame == CS.frame:
return
self.prev_frame = CS.frame
self.lkas_frame += 1
lkas_counter = CS.lkas_counter
if self.prev_lkas_counter == lkas_counter:
lkas_counter = (self.prev_lkas_counter +
1) % 16 # Predict the next frame
self.prev_lkas_counter = lkas_counter
# *** compute control surfaces ***
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
low_steer_models = self.car_fingerprint in (CAR.JEEP_CHEROKEE,
CAR.PACIFICA_2017_HYBRID,
CAR.PACIFICA_2018,
CAR.PACIFICA_2018_HYBRID)
if not self.min_steer_check:
self.moving_fast = True
self.torq_enabled = enabled or low_steer_models
elif low_steer_models:
self.moving_fast = not CS.out.steerError and CS.lkas_active
self.torq_enabled = self.torq_enabled or CS.torq_status > 1
else:
self.moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
if CS.out.vEgo > (CS.CP.minSteerSpeed -
0.5): # for command high bit
self.torq_enabled = True
elif CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
self.torq_enabled = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = self.moving_fast and enabled
if not lkas_active:
apply_steer = 0
self.apply_steer_last = apply_steer
if self.lkas_frame % 10 == 0: # 0.1s period
new_msg = create_lkas_heartbit(
self.packer,
0 if jvepilot_state.carControl.useLaneLines else 1,
CS.lkasHeartbit)
can_sends.append(new_msg)
if self.lkas_frame % 25 == 0: # 0.25s period
if CS.lkas_car_model != -1:
new_msg = create_lkas_hud(self.packer, CS.out.gearShifter,
lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer),
self.torq_enabled, lkas_counter)
can_sends.append(new_msg)
def wheel_button_control(self, CS, can_sends, enabled, gas_resume_speed,
jvepilot_state, pcm_cancel_cmd):
button_counter = jvepilot_state.carState.buttonCounter
if button_counter == self.last_button_counter:
return
self.last_button_counter = button_counter
self.button_frame += 1
button_counter_offset = 1
buttons_to_press = []
if pcm_cancel_cmd:
buttons_to_press = ['ACC_CANCEL']
elif not CS.button_pressed(ButtonType.cancel):
follow_inc_button = CS.button_pressed(ButtonType.followInc)
follow_dec_button = CS.button_pressed(ButtonType.followDec)
if jvepilot_state.carControl.autoFollow:
follow_inc_button = CS.button_pressed(ButtonType.followInc,
False)
follow_dec_button = CS.button_pressed(ButtonType.followDec,
False)
if (follow_inc_button and follow_inc_button.pressedFrames < 50) or \
(follow_dec_button and follow_dec_button.pressedFrames < 50):
jvepilot_state.carControl.autoFollow = False
jvepilot_state.notifyUi = True
elif (follow_inc_button and follow_inc_button.pressedFrames >= 50) or \
(follow_dec_button and follow_dec_button.pressedFrames >= 50):
jvepilot_state.carControl.autoFollow = True
jvepilot_state.notifyUi = True
if enabled and not CS.out.brakePressed:
button_counter_offset = [1, 1, 0, None][self.button_frame % 4]
if button_counter_offset is not None:
if (
not CS.out.cruiseState.enabled
) or CS.out.standstill: # Stopped and waiting to resume
buttons_to_press = [
self.auto_resume_button(CS, gas_resume_speed)
]
elif CS.out.cruiseState.enabled: # Control ACC
buttons_to_press = [
self.auto_follow_button(CS, jvepilot_state),
self.hybrid_acc_button(CS, jvepilot_state)
]
buttons_to_press = list(filter(None, buttons_to_press))
if buttons_to_press is not None and len(buttons_to_press) > 0:
new_msg = create_wheel_buttons_command(
self.packer, button_counter + button_counter_offset,
buttons_to_press)
can_sends.append(new_msg)
def auto_resume_button(self, CS, gas_resume_speed):
if self.auto_resume and CS.out.vEgo <= gas_resume_speed: # Keep trying while under gas_resume_speed
return 'ACC_RESUME'
def hybrid_acc_button(self, CS, jvepilot_state):
target = jvepilot_state.carControl.vTargetFuture + 2 * CV.MPH_TO_MS # add extra speed so ACC does the limiting
# Move the adaptive curse control to the target speed
eco_limit = None
if jvepilot_state.carControl.accEco == 1: # if eco mode
eco_limit = self.cachedParams.get_float(
'jvePilot.settings.accEco.speedAheadLevel1', 1000)
elif jvepilot_state.carControl.accEco == 2: # if eco mode
eco_limit = self.cachedParams.get_float(
'jvePilot.settings.accEco.speedAheadLevel2', 1000)
if eco_limit:
target = min(target, CS.out.vEgo + (eco_limit * CV.MPH_TO_MS))
# ACC Braking
diff = CS.out.vEgo - target
if diff > ACC_BRAKE_THRESHOLD and abs(
target - jvepilot_state.carControl.vMaxCruise
) > ACC_BRAKE_THRESHOLD: # ignore change in max cruise speed
target -= diff
# round to nearest unit
target = round(
min(jvepilot_state.carControl.vMaxCruise, target) *
self.round_to_unit)
current = round(CS.out.cruiseState.speed * self.round_to_unit)
if target < current and current > self.minAccSetting:
return 'ACC_SPEED_DEC'
elif target > current:
return 'ACC_SPEED_INC'
def auto_follow_button(self, CS, jvepilot_state):
if jvepilot_state.carControl.autoFollow:
crossover = [
0,
self.cachedParams.get_float(
'jvePilot.settings.autoFollow.speed1-2Bars', 1000) *
CV.MPH_TO_MS,
self.cachedParams.get_float(
'jvePilot.settings.autoFollow.speed2-3Bars', 1000) *
CV.MPH_TO_MS,
self.cachedParams.get_float(
'jvePilot.settings.autoFollow.speed3-4Bars', 1000) *
CV.MPH_TO_MS
]
if CS.out.vEgo < crossover[1]:
target_follow = 0
elif CS.out.vEgo < crossover[2]:
target_follow = 1
elif CS.out.vEgo < crossover[3]:
target_follow = 2
else:
target_follow = 3
if self.autoFollowDistanceLock is not None and abs(
crossover[self.autoFollowDistanceLock] -
CS.out.vEgo) > AUTO_FOLLOW_LOCK_MS:
self.autoFollowDistanceLock = None # unlock
if jvepilot_state.carState.accFollowDistance != target_follow and (
self.autoFollowDistanceLock
or target_follow) == target_follow:
self.autoFollowDistanceLock = target_follow # going from close to far, use upperbound
if jvepilot_state.carState.accFollowDistance > target_follow:
return 'ACC_FOLLOW_DEC'
else:
return 'ACC_FOLLOW_INC'
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster(['jvePilotState', 'sendcan', 'controlsState', 'carState',
'carControl', 'carEvents', 'carParams'])
self.camera_packets = ["roadCameraState", "driverCameraState"]
if TICI:
self.camera_packets.append("wideRoadCameraState")
params = Params()
self.joystick_mode = params.get_bool("JoystickDebugMode")
joystick_packet = ['testJoystick'] if self.joystick_mode else []
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'] if SIMULATION else None
self.sm = messaging.SubMaster(['jvePilotUIState', 'deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet,
ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan'])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
if TICI:
self.log_sock = messaging.sub_sock('androidLog')
# wait for one pandaState and one CAN packet
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
# read params
self.is_metric = params.get_bool("IsMetric")
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
passive = params.get_bool("Passive") or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature = self.CP.communityFeature or \
self.CP.fingerprintSource == car.CarParams.FingerprintSource.can
community_feature_disallowed = community_feature and (not community_feature_toggle)
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
safety_config = car.CarParams.SafetyConfig.new_message()
safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
self.CP.safetyConfigs = [safety_config]
# Write CarParams for radard
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP)
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.initialized = False
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.cruise_mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
self.v_target = 0.
self.buttonPressTimes = {}
self.cachedParams = CachedParams()
self.reverse_acc_button_change = self.cachedParams.get('jvePilot.settings.reverseAccSpeedChange', 0) == "1"
self.jvePilotState = car.JvePilotState.new_message()
self.jvePilotState.carControl.autoFollow = params.get_bool('jvePilot.settings.autoFollow')
self.jvePilotState.carControl.useLaneLines = not params.get_bool('EndToEndToggle')
self.jvePilotState.carControl.accEco = int(params.get('jvePilot.carState.accEco', encoding='utf8') or "1")
self.ui_notify()
# TODO: no longer necessary, aside from process replay
self.sm['liveParameters'].valid = True
self.startup_event = get_startup_event(car_recognized, controller_available, len(self.CP.carFw) > 0)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed and car_recognized and not self.CP.dashcamOnly:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
if len(self.CP.carFw) > 0:
set_offroad_alert("Offroad_CarUnrecognized", True)
else:
set_offroad_alert("Offroad_NoFirmware", True)
elif self.read_only:
self.events.add(EventName.dashcamMode, static=True)
elif self.joystick_mode:
self.events.add(EventName.joystickDebug, static=True)
self.startup_event = None
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
class Controls:
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster(['jvePilotState', 'sendcan', 'controlsState', 'carState',
'carControl', 'carEvents', 'carParams'])
self.camera_packets = ["roadCameraState", "driverCameraState"]
if TICI:
self.camera_packets.append("wideRoadCameraState")
params = Params()
self.joystick_mode = params.get_bool("JoystickDebugMode")
joystick_packet = ['testJoystick'] if self.joystick_mode else []
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'] if SIMULATION else None
self.sm = messaging.SubMaster(['jvePilotUIState', 'deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet,
ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan'])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
if TICI:
self.log_sock = messaging.sub_sock('androidLog')
# wait for one pandaState and one CAN packet
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
# read params
self.is_metric = params.get_bool("IsMetric")
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
passive = params.get_bool("Passive") or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature = self.CP.communityFeature or \
self.CP.fingerprintSource == car.CarParams.FingerprintSource.can
community_feature_disallowed = community_feature and (not community_feature_toggle)
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
safety_config = car.CarParams.SafetyConfig.new_message()
safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
self.CP.safetyConfigs = [safety_config]
# Write CarParams for radard
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP)
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.initialized = False
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.cruise_mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
self.v_target = 0.
self.buttonPressTimes = {}
self.cachedParams = CachedParams()
self.reverse_acc_button_change = self.cachedParams.get('jvePilot.settings.reverseAccSpeedChange', 0) == "1"
self.jvePilotState = car.JvePilotState.new_message()
self.jvePilotState.carControl.autoFollow = params.get_bool('jvePilot.settings.autoFollow')
self.jvePilotState.carControl.useLaneLines = not params.get_bool('EndToEndToggle')
self.jvePilotState.carControl.accEco = int(params.get('jvePilot.carState.accEco', encoding='utf8') or "1")
self.ui_notify()
# TODO: no longer necessary, aside from process replay
self.sm['liveParameters'].valid = True
self.startup_event = get_startup_event(car_recognized, controller_available, len(self.CP.carFw) > 0)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed and car_recognized and not self.CP.dashcamOnly:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
if len(self.CP.carFw) > 0:
set_offroad_alert("Offroad_CarUnrecognized", True)
else:
set_offroad_alert("Offroad_NoFirmware", True)
elif self.read_only:
self.events.add(EventName.dashcamMode, static=True)
elif self.joystick_mode:
self.events.add(EventName.joystickDebug, static=True)
self.startup_event = None
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
def update_events(self, CS):
"""Compute carEvents from carState"""
self.events.clear()
self.events.add_from_msg(CS.events)
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
# Handle startup event
if self.startup_event is not None:
self.events.add(self.startup_event)
self.startup_event = None
# Don't add any more events if not initialized
if not self.initialized:
self.events.add(EventName.controlsInitializing)
return
# Create events for battery, temperature, disk space, and memory
if EON and (self.sm['peripheralState'].pandaType != PandaType.uno) and \
self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError:
# at zero percent battery, while discharging, OP should not allowed
self.events.add(EventName.lowBattery)
if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
self.events.add(EventName.overheat)
if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
# under 7% of space free no enable allowed
self.events.add(EventName.outOfSpace)
# TODO: make tici threshold the same
if self.sm['deviceState'].memoryUsagePercent > (90 if TICI else 65) and not SIMULATION:
self.events.add(EventName.lowMemory)
# TODO: enable this once loggerd CPU usage is more reasonable
#cpus = list(self.sm['deviceState'].cpuUsagePercent)[:(-1 if EON else None)]
#if max(cpus, default=0) > 95 and not SIMULATION:
# self.events.add(EventName.highCpuUsage)
# Alert if fan isn't spinning for 5 seconds
if self.sm['peripheralState'].pandaType in [PandaType.uno, PandaType.dos]:
if self.sm['peripheralState'].fanSpeedRpm == 0 and self.sm['deviceState'].fanSpeedPercentDesired > 50:
if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0:
self.events.add(EventName.fanMalfunction)
else:
self.last_functional_fan_frame = self.sm.frame
# Handle calibration status
cal_status = self.sm['liveCalibration'].calStatus
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
self.events.add(EventName.calibrationIncomplete)
else:
self.events.add(EventName.calibrationInvalid)
# Handle lane change
if self.sm['lateralPlan'].laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['lateralPlan'].laneChangeDirection
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)
else:
self.events.add(EventName.preLaneChangeRight)
elif self.sm['lateralPlan'].laneChangeState in [LaneChangeState.laneChangeStarting,
LaneChangeState.laneChangeFinishing]:
self.events.add(EventName.laneChange)
if self.can_rcv_error or not CS.canValid:
self.events.add(EventName.canError)
for i, pandaState in enumerate(self.sm['pandaStates']):
# All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
if i < len(self.CP.safetyConfigs):
safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam
else:
safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
if safety_mismatch or self.mismatch_counter >= 200:
self.events.add(EventName.controlsMismatch)
if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
self.events.add(EventName.relayMalfunction)
# Check for HW or system issues
if len(self.sm['radarState'].radarErrors):
self.events.add(EventName.radarFault)
elif not self.sm.valid["pandaStates"]:
self.events.add(EventName.usbError)
elif not self.sm.all_alive_and_valid():
self.events.add(EventName.commIssue)
if not self.logged_comm_issue:
invalid = [s for s, valid in self.sm.valid.items() if not valid]
not_alive = [s for s, alive in self.sm.alive.items() if not alive]
cloudlog.event("commIssue", invalid=invalid, not_alive=not_alive)
self.logged_comm_issue = True
else:
self.logged_comm_issue = False
if not self.sm['liveParameters'].valid:
self.events.add(EventName.vehicleModelInvalid)
if not self.sm['lateralPlan'].mpcSolutionValid:
self.events.add(EventName.plannerError)
if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
if self.sm.frame > 5 / DT_CTRL: # Give locationd some time to receive all the inputs
self.events.add(EventName.sensorDataInvalid)
if not self.sm['liveLocationKalman'].posenetOK:
self.events.add(EventName.posenetInvalid)
if not self.sm['liveLocationKalman'].deviceStable:
self.events.add(EventName.deviceFalling)
for pandaState in self.sm['pandaStates']:
if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
self.events.add(EventName.relayMalfunction)
if not REPLAY:
# Check for mismatch between openpilot and car's PCM
cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
if self.cruise_mismatch_counter > int(3. / DT_CTRL):
self.events.add(EventName.cruiseMismatch)
# Check for FCW
stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.5
model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking
planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
if planner_fcw or model_fcw:
self.events.add(EventName.fcw)
if TICI:
logs = messaging.drain_sock(self.log_sock, wait_for_one=False)
messages = []
for m in logs:
try:
messages.append(m.androidLog.message)
except UnicodeDecodeError:
pass
for err in ["ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED"]:
for m in messages:
if err not in m:
continue
csid = m.split("CSID:")[-1].split(" ")[0]
evt = {"0": EventName.roadCameraError, "1": EventName.wideRoadCameraError,
"2": EventName.driverCameraError}.get(csid, None)
if evt is not None:
self.events.add(evt)
# TODO: fix simulator
if not SIMULATION:
if not NOSENSOR:
if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000):
# Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
self.events.add(EventName.noGps)
if not self.sm.all_alive(self.camera_packets):
self.events.add(EventName.cameraMalfunction)
if self.sm['modelV2'].frameDropPerc > 20:
self.events.add(EventName.modeldLagging)
if self.sm['liveLocationKalman'].excessiveResets:
self.events.add(EventName.localizerMalfunction)
# Check if all manager processes are running
not_running = set(p.name for p in self.sm['managerState'].processes if not p.running)
if self.sm.rcv_frame['managerState'] and (not_running - IGNORE_PROCESSES):
self.events.add(EventName.processNotRunning)
# Only allow engagement with brake pressed when stopped behind another stopped car
# speeds = self.sm['longitudinalPlan'].speeds
# if len(speeds) > 1:
# v_future = speeds[-1]
# else:
# v_future = 100.0
# if CS.brakePressed and v_future >= self.CP.vEgoStarting \
# and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
# self.events.add(EventName.noTarget)
def data_sample(self):
"""Receive data from sockets and update carState"""
# Update carState from CAN
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
CS = self.CI.update(self.CC, can_strs)
self.sm.update(0)
all_valid = CS.canValid and self.sm.all_alive_and_valid()
if not self.initialized and (all_valid or self.sm.frame * DT_CTRL > 3.5 or SIMULATION):
if not self.read_only:
self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
self.initialized = True
Params().put_bool("ControlsReady", True)
# Check for CAN timeout
if not can_strs:
self.can_error_counter += 1
self.can_rcv_error = True
else:
self.can_rcv_error = False
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not self.enabled:
self.mismatch_counter = 0
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
if any(not ps.controlsAllowed and self.enabled for ps in self.sm['pandaStates']
if ps.safetyModel not in IGNORED_SAFETY_MODES):
self.mismatch_counter += 1
self.distance_traveled += CS.vEgo * DT_CTRL
if self.jvePilotState.notifyUi:
self.ui_notify()
elif self.sm.updated['jvePilotUIState']:
self.jvePilotState.carControl.autoFollow = self.sm['jvePilotUIState'].autoFollow
self.jvePilotState.carControl.accEco = self.sm['jvePilotUIState'].accEco
put_nonblocking("jvePilot.carState.accEco", str(self.sm['jvePilotUIState'].accEco))
return CS
def ui_notify(self):
self.jvePilotState.notifyUi = False
msg = messaging.new_message('jvePilotUIState')
msg.jvePilotUIState = self.sm['jvePilotUIState']
msg.jvePilotUIState.autoFollow = self.jvePilotState.carControl.autoFollow
msg.jvePilotUIState.accEco = self.jvePilotState.carControl.accEco
msg.jvePilotUIState.useLaneLines = self.jvePilotState.carControl.useLaneLines
self.pm.send('jvePilotState', msg)
def state_transition(self, CS):
"""Compute conditional state transitions and execute actions on state transitions"""
self.v_cruise_kph_last = self.v_cruise_kph
# if stock cruise is completely disabled, then we can use our own set speed logic
if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed:
self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.buttonEvents, self.enabled, self.reverse_acc_button_change, self.is_metric)
elif self.CP.pcmCruise and CS.cruiseState.enabled:
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# decrement the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
self.current_alert_types = [ET.PERMANENT]
# ENABLED, PRE ENABLING, SOFT DISABLING
if self.state != State.disabled:
# user and immediate disable always have priority in a non-disabled state
if self.events.any(ET.USER_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.USER_DISABLE)
elif self.events.any(ET.IMMEDIATE_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
else:
# ENABLED
if self.state == State.enabled:
if self.events.any(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
self.current_alert_types.append(ET.SOFT_DISABLE)
# SOFT DISABLING
elif self.state == State.softDisabling:
if not self.events.any(ET.SOFT_DISABLE):
# no more soft disabling condition, so go back to ENABLED
self.state = State.enabled
elif self.events.any(ET.SOFT_DISABLE) and self.soft_disable_timer > 0:
self.current_alert_types.append(ET.SOFT_DISABLE)
elif self.soft_disable_timer <= 0:
self.state = State.disabled
# PRE ENABLING
elif self.state == State.preEnabled:
if not self.events.any(ET.PRE_ENABLE):
self.state = State.enabled
else:
self.current_alert_types.append(ET.PRE_ENABLE)
# DISABLED
elif self.state == State.disabled:
if self.events.any(ET.ENABLE):
if self.events.any(ET.NO_ENTRY):
self.current_alert_types.append(ET.NO_ENTRY)
else:
if self.events.any(ET.PRE_ENABLE):
self.state = State.preEnabled
else:
self.state = State.enabled
self.current_alert_types.append(ET.ENABLE)
self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last, self.is_metric)
# Check if actuators are enabled
self.active = self.state == State.enabled or self.state == State.softDisabling
if self.active:
self.current_alert_types.append(ET.WARNING)
# Check if openpilot is engaged
self.enabled = self.active or self.state == State.preEnabled
def state_control(self, CS):
"""Given the state, this function returns an actuators packet"""
# Update VehicleModel
params = self.sm['liveParameters']
x = max(params.stiffnessFactor, 0.1)
sr = max(params.steerRatio, 0.1)
self.VM.update_params(x, sr)
lat_plan = self.sm['lateralPlan']
long_plan = self.sm['longitudinalPlan']
actuators = car.CarControl.Actuators.new_message()
actuators.longControlState = self.LoC.long_control_state
if CS.leftBlinker or CS.rightBlinker:
self.last_blinker_frame = self.sm.frame
# State specific actions
if not self.active:
self.LaC.reset()
self.LoC.reset(v_pid=CS.vEgo)
if not self.joystick_mode:
# accel PID loop
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS)
actuators.accel, self.v_target = self.LoC.update(self.active, CS, self.CP, long_plan, pid_accel_limits)
# Steering PID loop and lateral MPC
lat_active = self.active and not CS.steerWarning and not CS.steerError and CS.vEgo > self.CP.minSteerSpeed
desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
lat_plan.psis,
lat_plan.curvatures,
lat_plan.curvatureRates)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(lat_active, CS, self.CP, self.VM, params,
desired_curvature, desired_curvature_rate)
else:
lac_log = log.ControlsState.LateralDebugState.new_message()
if self.sm.rcv_frame['testJoystick'] > 0 and self.active:
actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1)
steer = clip(self.sm['testJoystick'].axes[1], -1, 1)
# max angle is 45 for angle-based cars
actuators.steer, actuators.steeringAngleDeg = steer, steer * 45.
lac_log.active = True
lac_log.steeringAngleDeg = CS.steeringAngleDeg
lac_log.output = steer
lac_log.saturated = abs(steer) >= 0.9
# Check for difference between desired angle and angle for angle based control
angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
if angle_control_saturated and not CS.steeringPressed and self.active:
self.saturated_count += 1
else:
self.saturated_count = 0
# Send a "steering required alert" if saturation count has reached the limit
if (lac_log.saturated and not CS.steeringPressed) or \
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
if len(lat_plan.dPathPoints):
# Check if we deviated from the path
# TODO use desired vs actual curvature
left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[0] < -0.20
right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[0] > 0.20
if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
# Ensure no NaNs/Infs
for p in ACTUATOR_FIELDS:
attr = getattr(actuators, p)
if not isinstance(attr, Number):
continue
if not math.isfinite(attr):
cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
setattr(actuators, p, 0.0)
return actuators, lac_log
def publish_logs(self, CS, start_time, actuators, lac_log):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CC = car.CarControl.new_message()
CC.jvePilotState.carState = CS.jvePilotCarState
CC.jvePilotState.carControl = self.jvePilotState.carControl
CC.enabled = self.enabled
CC.active = self.active
CC.actuators = actuators
if len(self.sm['liveLocationKalman'].orientationNED.value) > 2:
CC.roll = self.sm['liveLocationKalman'].orientationNED.value[0]
CC.pitch = self.sm['liveLocationKalman'].orientationNED.value[1]
CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
CC.cruiseControl.cancel = True
# target the future speed
v_max_speed = float(self.v_cruise_kph * CV.KPH_TO_MS)
CC.jvePilotState.carControl.vMaxCruise = v_max_speed
v_target_future = self.v_target
speeds = self.sm['longitudinalPlan'].speeds
if len(speeds) > 0:
v_target_future = min(speeds) if actuators.accel < 0 else max(speeds)
CC.jvePilotState.carControl.vTargetFuture = min(v_max_speed, v_target_future)
CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = self.enabled
CC.hudControl.lanesVisible = self.enabled
CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
CC.hudControl.rightLaneVisible = True
CC.hudControl.leftLaneVisible = True
recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
meta = self.sm['modelV2'].meta
if len(meta.desirePrediction) and ldw_allowed:
right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
left_lane_visible = self.sm['lateralPlan'].lProb > 0.5
l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1]
r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1]
device_offset = self.cachedParams.get_float('jvePilot.settings.deviceOffset', 5000)
l_lane_close = left_lane_visible and (self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + (CAMERA_OFFSET + device_offset)))
r_lane_close = right_lane_visible and (self.sm['modelV2'].laneLines[2].y[0] < (1.08 - (CAMERA_OFFSET + device_offset)))
CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart:
self.events.add(EventName.ldw)
clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric, self.soft_disable_timer])
self.AM.add_many(self.sm.frame, alerts)
self.AM.process_alerts(self.sm.frame, clear_event)
CC.hudControl.visualAlert = self.AM.visual_alert
if not self.read_only and self.initialized:
# send car controls over can
can_sends = self.CI.apply(CC)
self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
self.jvePilotState.carControl = CC.jvePilotState.carControl
self.jvePilotState.notifyUi = CC.jvePilotState.notifyUi
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
# Curvature & Steering angle
params = self.sm['liveParameters']
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetAverageDeg)
curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo)
# controlsState
dat = messaging.new_message('controlsState')
dat.valid = CS.canValid
controlsState = dat.controlsState
controlsState.alertText1 = self.AM.alert_text_1
controlsState.alertText2 = self.AM.alert_text_2
controlsState.alertSize = self.AM.alert_size
controlsState.alertStatus = self.AM.alert_status
controlsState.alertBlinkingRate = self.AM.alert_rate
controlsState.alertType = self.AM.alert_type
controlsState.alertSound = self.AM.audible_alert
controlsState.canMonoTimes = list(CS.canMonoTimes)
controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan']
controlsState.enabled = self.enabled
controlsState.active = self.active
controlsState.curvature = curvature
controlsState.state = self.state
controlsState.engageable = not self.events.any(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
controlsState.vPid = float(self.LoC.v_pid)
controlsState.vCruise = float(self.v_cruise_kph)
controlsState.upAccelCmd = float(self.LoC.pid.p)
controlsState.uiAccelCmd = float(self.LoC.pid.i)
controlsState.ufAccelCmd = float(self.LoC.pid.f)
controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9)
controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_error_counter
if self.joystick_mode:
controlsState.lateralControlState.debugState = lac_log
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
controlsState.lateralControlState.angleState = lac_log
elif self.CP.lateralTuning.which() == 'pid':
controlsState.lateralControlState.pidState = lac_log
elif self.CP.lateralTuning.which() == 'lqr':
controlsState.lateralControlState.lqrState = lac_log
elif self.CP.lateralTuning.which() == 'indi':
controlsState.lateralControlState.indiState = lac_log
self.pm.send('controlsState', dat)
# carState
car_events = self.events.to_msg()
cs_send = messaging.new_message('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = car_events
self.pm.send('carState', cs_send)
# carEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
ce_send = messaging.new_message('carEvents', len(self.events))
ce_send.carEvents = car_events
self.pm.send('carEvents', ce_send)
self.events_prev = self.events.names.copy()
# carParams - logged every 50 seconds (> 1 per segment)
if (self.sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message('carParams')
cp_send.carParams = self.CP
self.pm.send('carParams', cp_send)
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
# copy CarControl to pass to CarInterface on the next iteration
self.CC = CC
def step(self):
start_time = sec_since_boot()
self.prof.checkpoint("Ratekeeper", ignore=True)
# Sample data from sockets and get a carState
CS = self.data_sample()
self.prof.checkpoint("Sample")
self.update_events(CS)
if not self.read_only and self.initialized:
# Update control state
self.state_transition(CS)
self.prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, lac_log = self.state_control(CS)
self.prof.checkpoint("State Control")
# Publish data
self.publish_logs(CS, start_time, actuators, lac_log)
self.prof.checkpoint("Sent")
def controlsd_thread(self):
while True:
self.step()
self.rk.monitor_time()
self.prof.display()
class Planner:
def __init__(self, CP, init_v=0.0, init_a=0.0):
self.CP = CP
self.mpc = LongitudinalMpc()
self.fcw = False
self.cachedParams = CachedParams()
self.v_desired = init_v
self.a_desired = init_a
self.alpha = np.exp(-DT_MDL / 2.0)
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.j_desired_trajectory = np.zeros(CONTROL_N)
def update(self, sm, lateral_planner):
v_ego = sm['carState'].vEgo
a_ego = sm['carState'].aEgo
v_cruise_kph = sm['controlsState'].vCruise
v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
v_cruise = v_cruise_kph * CV.KPH_TO_MS
long_control_state = sm['controlsState'].longControlState
force_slow_decel = sm['controlsState'].forceDecel
prev_accel_constraint = True
if long_control_state == LongCtrlState.off or sm['carState'].gasPressed:
self.v_desired = v_ego
self.a_desired = a_ego
# Smoothly changing between accel trajectory is only relevant when OP is driving
prev_accel_constraint = False
# Prevent divergence, smooth in current v_ego
self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * v_ego
self.v_desired = max(0.0, self.v_desired)
accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
if not self.cachedParams.get('jvePilot.settings.slowInCurves', 5000) == "1":
accel_limits = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
accel_limits[0] = min(accel_limits[0], accel_limits[1])
# clip limits, cannot init MPC outside of bounds
accel_limits[0] = min(accel_limits[0], self.a_desired + 0.05)
accel_limits[1] = max(accel_limits[1], self.a_desired - 0.05)
self.mpc.set_accel_limits(accel_limits[0], accel_limits[1])
self.mpc.set_cur_state(self.v_desired, self.a_desired)
self.mpc.update(sm['carState'], sm['radarState'], v_cruise, prev_accel_constraint=prev_accel_constraint)
self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)
self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
# TODO counter is only needed because radar is glitchy, remove once radar is gone
self.fcw = self.mpc.crash_cnt > 5
if self.fcw:
cloudlog.info("FCW triggered")
# Interpolate 0.05 seconds and save as starting point for next iteration
a_prev = self.a_desired
self.a_desired = float(interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory))
self.v_desired = self.v_desired + DT_MDL * (self.a_desired + a_prev) / 2.0
if lateral_planner.lateralPlan and self.cachedParams.get('jvePilot.settings.slowInCurves', 5000) == "1":
curvs = list(lateral_planner.lateralPlan.curvatures)
if len(curvs):
# find the largest curvature in the solution and use that.
curv = abs(curvs[-1])
if curv != 0:
self.v_desired = float(min(self.v_desired, self.limit_speed_in_curv(sm, curv)))
def publish(self, sm, pm):
plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState'])
longitudinalPlan = plan_send.longitudinalPlan
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
longitudinalPlan.speeds = [float(x) for x in self.v_desired_trajectory]
longitudinalPlan.accels = [float(x) for x in self.a_desired_trajectory]
longitudinalPlan.jerks = [float(x) for x in self.j_desired_trajectory]
longitudinalPlan.hasLead = sm['radarState'].leadOne.status
longitudinalPlan.longitudinalPlanSource = self.mpc.source
longitudinalPlan.fcw = self.fcw
pm.send('longitudinalPlan', plan_send)
def limit_speed_in_curv(self, sm, curv):
v_ego = sm['carState'].vEgo
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
# drop off
drop_off = self.cachedParams.get_float('jvePilot.settings.slowInCurves.speedDropOff', 5000)
if drop_off != 2 and a_y_max > 0:
a_y_max = np.sqrt(a_y_max) ** drop_off
v_curvature = np.sqrt(a_y_max / np.clip(curv, 1e-4, None))
model_speed = np.min(v_curvature)
return model_speed * self.cachedParams.get_float('jvePilot.settings.slowInCurves.speedRatio', 5000)