def __init__(self, CP, init_v=0.0, init_a=0.0):
self.CP = CP
self.mpc = LongitudinalMpc()
self.fcw = False
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)
# dp
self.dp_accel_profile_ctrl = False
self.dp_accel_profile = DP_ACCEL_ECO
self.dp_following_profile_ctrl = False
self.dp_following_profile = 2
self.dp_following_dist = 1.8 # default val
self.cruise_source = 'cruise'
self.vision_turn_controller = VisionTurnController(CP)
self.speed_limit_controller = SpeedLimitController()
self.events = Events()
self.turn_speed_controller = TurnSpeedController()
def __init__(self, CP, init_v=0.0, init_a=0.0):
self.CP = CP
self.mpc = LongitudinalMpc()
self.fcw = False
self.a_desired = init_a
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL)
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, CP, init_v=0.0, init_a=0.0):
self.CP = CP
self.mpc = LongitudinalMpc()
self.fcw = False
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, CP, init_v=0.0, init_a=0.0):
self.CP = CP
self.mpc = LongitudinalMpc()
self.fcw = False
self.a_desired = init_a
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL)
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.j_desired_trajectory = np.zeros(CONTROL_N)
self.use_cluster_speed = Params().get_bool('UseClusterSpeed')
self.long_control_enabled = Params().get_bool('LongControlEnabled')
def __init__(self, CP, init_v=0.0, init_a=0.0):
self.CP = CP
self.mpc = LongitudinalMpc()
self.fcw = False
self.v_desired = init_v
self.a_desired = init_a
self.alpha = np.exp(-DT_MDL / 2.0)
self.lead_0 = log.ModelDataV2.LeadDataV3.new_message()
self.lead_1 = log.ModelDataV2.LeadDataV3.new_message()
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, CP):
self.CP = CP
self.mpcs = {}
self.mpcs['lead0'] = LeadMpc(0)
self.mpcs['lead1'] = LeadMpc(1)
self.mpcs['cruise'] = LongitudinalMpc()
self.fcw = False
self.fcw_checker = FCWChecker()
self.v_desired = 0.0
self.a_desired = 0.0
self.longitudinalPlanSource = 'cruise'
self.alpha = np.exp(-DT_MDL / 2.0)
self.lead_0 = log.ModelDataV2.LeadDataV3.new_message()
self.lead_1 = log.ModelDataV2.LeadDataV3.new_message()
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.j_desired_trajectory = np.zeros(CONTROL_N)
class Planner:
def __init__(self, CP, init_v=0.0, init_a=0.0):
self.CP = CP
self.mpc = LongitudinalMpc()
self.fcw = False
self.a_desired = init_a
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL)
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.j_desired_trajectory = np.zeros(CONTROL_N)
self.solverExecutionTime = 0.0
def update(self, sm):
v_ego = sm['carState'].vEgo
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
# Reset current state when not engaged, or user is controlling the speed
reset_state = long_control_state == LongCtrlState.off
# No change cost when user is controlling the speed, or when standstill
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
if reset_state:
self.v_desired_filter.x = v_ego
self.a_desired = 0.0
# Prevent divergence, smooth in current v_ego
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
accel_limits_turns = 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_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0],
accel_limits_turns[1])
# clip limits, cannot init MPC outside of bounds
accel_limits_turns[0] = min(accel_limits_turns[0],
self.a_desired + 0.05)
accel_limits_turns[1] = max(accel_limits_turns[1],
self.a_desired - 0.05)
self.mpc.set_weights(prev_accel_constraint)
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
self.mpc.update(sm['carState'], sm['radarState'], v_cruise)
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_filter.x = self.v_desired_filter.x + DT_MDL * (
self.a_desired + a_prev) / 2.0
def publish(self, sm, pm):
plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_checks(
service_list=['carState', 'controlsState'])
longitudinalPlan = plan_send.longitudinalPlan
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
longitudinalPlan.processingDelay = (plan_send.logMonoTime /
1e9) - sm.logMonoTime['modelV2']
longitudinalPlan.speeds = self.v_desired_trajectory.tolist()
longitudinalPlan.accels = self.a_desired_trajectory.tolist()
longitudinalPlan.jerks = self.j_desired_trajectory.tolist()
longitudinalPlan.hasLead = sm['radarState'].leadOne.status
longitudinalPlan.longitudinalPlanSource = self.mpc.source
longitudinalPlan.fcw = self.fcw
longitudinalPlan.solverExecutionTime = self.mpc.solve_time
pm.send('longitudinalPlan', plan_send)
class Planner():
def __init__(self, CP, init_v=0.0, init_a=0.0):
self.CP = CP
self.mpc = LongitudinalMpc()
self.fcw = False
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, CP):
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
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
if not enabled or sm['carState'].gasPressed:
self.v_desired = v_ego
self.a_desired = a_ego
# 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)]
accel_limits_turns = 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_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
# clip limits, cannot init MPC outside of bounds
accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired, self.a_desired)
self.mpc.update(sm['carState'], sm['radarState'], v_cruise)
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
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)
class Planner:
def __init__(self, CP, init_v=0.0, init_a=0.0):
self.CP = CP
self.mpc = LongitudinalMpc()
self.fcw = False
self.a_desired = init_a
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL)
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.j_desired_trajectory = np.zeros(CONTROL_N)
self.use_cluster_speed = Params().get_bool('UseClusterSpeed')
self.long_control_enabled = Params().get_bool('LongControlEnabled')
def update(self, sm):
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
# neokii
if not self.use_cluster_speed:
vCluRatio = sm['carState'].vCluRatio
if vCluRatio > 0.5:
v_cruise *= vCluRatio
v_cruise = int(v_cruise * CV.MS_TO_KPH + 0.25) * 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_filter.x = v_ego
self.a_desired = 0.0
# 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_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
accel_limits_turns = 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_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
# clip limits, cannot init MPC outside of bounds
accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired_filter.x, 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_filter.x = self.v_desired_filter.x + DT_MDL * (self.a_desired + a_prev) / 2.0
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 = self.v_desired_trajectory.tolist()
longitudinalPlan.accels = self.a_desired_trajectory.tolist()
longitudinalPlan.jerks = self.j_desired_trajectory.tolist()
longitudinalPlan.hasLead = sm['radarState'].leadOne.status
longitudinalPlan.longitudinalPlanSource = self.mpc.source
longitudinalPlan.fcw = self.fcw
longitudinalPlan.solverExecutionTime = self.mpc.solve_time
pm.send('longitudinalPlan', plan_send)
class Planner():
def __init__(self, CP, init_v=0.0, init_a=0.0):
self.CP = CP
self.mpc = LongitudinalMpc()
self.fcw = False
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)
# dp
self.dp_accel_profile_ctrl = False
self.dp_accel_profile = DP_ACCEL_ECO
self.dp_following_profile_ctrl = False
self.dp_following_profile = 2
self.dp_following_dist = 1.8 # default val
self.cruise_source = 'cruise'
self.vision_turn_controller = VisionTurnController(CP)
self.speed_limit_controller = SpeedLimitController()
self.events = Events()
self.turn_speed_controller = TurnSpeedController()
def update(self, sm, CP):
# dp
self.dp_accel_profile_ctrl = sm['dragonConf'].dpAccelProfileCtrl
self.dp_accel_profile = sm['dragonConf'].dpAccelProfile
self.dp_following_profile_ctrl = sm[
'dragonConf'].dpFollowingProfileCtrl
self.dp_following_profile = sm['dragonConf'].dpFollowingProfile
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
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
if not enabled or sm['carState'].gasPressed:
self.v_desired = v_ego
self.a_desired = a_ego
# 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)
# Get acceleration and active solutions for custom long mpc.
self.cruise_source, a_min_sol, v_cruise_sol = self.cruise_solutions(
enabled, self.v_desired, self.a_desired, v_cruise, sm)
if not self.dp_accel_profile_ctrl:
accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
else:
accel_limits = dp_calc_cruise_accel_limits(v_cruise,
self.dp_accel_profile)
accel_limits_turns = 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_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0],
accel_limits_turns[1])
# clip limits, cannot init MPC outside of bounds
accel_limits_turns[0] = min(accel_limits_turns[0],
self.a_desired + 0.05, a_min_sol)
accel_limits_turns[1] = max(accel_limits_turns[1],
self.a_desired - 0.05)
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired, self.a_desired)
self.mpc.update(sm['carState'], sm['radarState'], v_cruise_sol)
self.mpc.set_desired_TR(1.8 if not self.dp_following_profile_ctrl else
self.dp_following_profile)
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
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 if self.mpc.source != 'cruise' else self.cruise_source
longitudinalPlan.fcw = self.fcw
longitudinalPlan.visionTurnControllerState = self.vision_turn_controller.state
longitudinalPlan.visionTurnSpeed = float(
self.vision_turn_controller.v_turn)
longitudinalPlan.speedLimitControlState = self.speed_limit_controller.state
longitudinalPlan.speedLimit = float(
self.speed_limit_controller.speed_limit)
longitudinalPlan.speedLimitOffset = float(
self.speed_limit_controller.speed_limit_offset)
longitudinalPlan.distToSpeedLimit = float(
self.speed_limit_controller.distance)
longitudinalPlan.isMapSpeedLimit = bool(
self.speed_limit_controller.source ==
SpeedLimitResolver.Source.map_data)
longitudinalPlan.eventsDEPRECATED = self.events.to_msg()
longitudinalPlan.turnSpeedControlState = self.turn_speed_controller.state
longitudinalPlan.turnSpeed = float(
self.turn_speed_controller.speed_limit)
longitudinalPlan.distToTurn = float(
self.turn_speed_controller.distance)
longitudinalPlan.turnSign = int(self.turn_speed_controller.turn_sign)
pm.send('longitudinalPlan', plan_send)
def cruise_solutions(self, enabled, v_ego, a_ego, v_cruise, sm):
# Update controllers
self.vision_turn_controller.update(enabled, v_ego, a_ego, v_cruise, sm)
self.events = Events()
self.speed_limit_controller.update(enabled, v_ego, a_ego, sm, v_cruise,
self.events)
self.turn_speed_controller.update(enabled, v_ego, a_ego, sm)
# Pick solution with lowest acceleration target.
a_solutions = {'cruise': float("inf")}
v_solutions = {'cruise': v_cruise}
if self.vision_turn_controller.is_active:
a_solutions['turn'] = self.vision_turn_controller.a_target
v_solutions['turn'] = self.vision_turn_controller.v_turn
if self.speed_limit_controller.is_active:
a_solutions['limit'] = self.speed_limit_controller.a_target
v_solutions[
'limit'] = self.speed_limit_controller.speed_limit_offseted
if self.turn_speed_controller.is_active:
a_solutions['turnlimit'] = self.turn_speed_controller.a_target
v_solutions['turnlimit'] = self.turn_speed_controller.speed_limit
source = min(v_solutions, key=v_solutions.get)
return source, a_solutions[source], v_solutions[source]
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)