def update(self, CS, CP, VM, PP, live20, live100, md, live_map_data):
"""Gets called when new live20 is available"""
cur_time = live20.logMonoTime / 1e9
v_ego = CS.carState.vEgo
long_control_state = live100.live100.longControlState
v_cruise_kph = live100.live100.vCruise
force_slow_decel = live100.live100.forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
self.last_md_ts = md.logMonoTime
self.radar_errors = list(live20.live20.radarErrors)
self.lead_1 = live20.live20.leadOne
self.lead_2 = live20.live20.leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0
self.v_speedlimit = NO_CURVATURE_SPEED
self.v_curvature = NO_CURVATURE_SPEED
self.map_valid = live_map_data.liveMapData.mapValid
# Speed limit and curvature
set_speed_limit_active = self.params.get(
"LimitSetSpeed") == "1" and self.params.get(
"SpeedLimitOffset") is not None
if set_speed_limit_active:
if live_map_data.liveMapData.speedLimitValid:
speed_limit = live_map_data.liveMapData.speedLimit
offset = float(self.params.get("SpeedLimitOffset"))
self.v_speedlimit = speed_limit + offset
if live_map_data.liveMapData.curvatureValid:
curvature = abs(live_map_data.liveMapData.curvature)
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = math.sqrt(a_y_max / max(1e-4, curvature))
self.v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
self.decel_for_turn = bool(self.v_curvature < min(
[v_cruise_setpoint, self.v_speedlimit, v_ego + 1.]))
v_cruise_setpoint = min(
[v_cruise_setpoint, self.v_curvature, self.v_speedlimit])
# Calculate speed for normal cruise control
if enabled:
accel_limits = map(float,
calc_cruise_accel_limits(v_ego, following))
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
accel_limits = limit_accel_in_turns(v_ego,
CS.carState.steeringAngle,
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])
# Change accel limits based on time remaining to turn
if self.decel_for_turn:
time_to_turn = max(
1.0, live_map_data.liveMapData.distToTurn /
max(self.v_cruise, 1.))
required_decel = min(0, (self.v_curvature - self.v_cruise) /
time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits[1], accel_limits[0], jerk_limits[1],
jerk_limits[0], _DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(CS.carState.aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.carState.leftBlinker or CS.carState.rightBlinker
self.fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo,
self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
self.lead_1.yRel, self.lead_1.vLat, self.lead_1.fcw,
blinkers) and not CS.carState.brakePressed
if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
model_dead = cur_time - (md.logMonoTime / 1e9) > 0.5
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
# TODO: Move all these events to controlsd. This has nothing to do with planning
events = []
if model_dead:
events.append(
create_event('modelCommIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if 'fault' in self.radar_errors:
events.append(
create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
plan_send.plan.events = events
plan_send.plan.mdMonoTime = md.logMonoTime
plan_send.plan.l20MonoTime = live20.logMonoTime
# longitudal plan
plan_send.plan.vCruise = self.v_cruise
plan_send.plan.aCruise = self.a_cruise
plan_send.plan.vStart = self.v_acc_start
plan_send.plan.aStart = self.a_acc_start
plan_send.plan.vTarget = self.v_acc
plan_send.plan.aTarget = self.a_acc
plan_send.plan.vTargetFuture = self.v_acc_future
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
plan_send.plan.vCurvature = self.v_curvature
plan_send.plan.decelForTurn = self.decel_for_turn
plan_send.plan.mapValid = self.map_valid
# Send out fcw
fcw = self.fcw and (self.fcw_enabled
or long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
# Interpolate 0.05 seconds and save as starting point for next iteration
dt = 0.05 # s
a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc -
self.a_acc_start)
v_acc_sol = self.v_acc_start + dt * (a_acc_sol +
self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
def update(self, sm, CP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
self.v_acc_start = self.v_acc_next
self.a_acc_start = self.a_acc_next
# Calculate speed for normal cruise control
if enabled and not self.first_loop and not sm['carState'].gasPressed:
accel_limits = [
float(x) for x in calc_cruise_accel_limits(v_ego, following)
]
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
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])
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = self.CP.minSpeedCan if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(sm['carState'], lead_1)
self.mpc2.update(sm['carState'], lead_2)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
self.fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, sm['controlsState'].active,
v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead,
lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw,
blinkers) and not sm['carState'].brakePressed
if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (
self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (
a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_next = v_acc_sol
self.a_acc_next = a_acc_sol
self.first_loop = False
def update(self, CS, LaC, LoC, v_cruise_kph, force_slow_decel):
cur_time = sec_since_boot()
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
md = None
l20 = None
gps_planner_plan = None
for socket, event in self.poller.poll(0):
if socket is self.model:
md = messaging.recv_one(socket)
elif socket is self.live20:
l20 = messaging.recv_one(socket)
elif socket is self.gps_planner_plan:
gps_planner_plan = messaging.recv_one(socket)
if gps_planner_plan is not None:
self.last_gps_planner_plan = gps_planner_plan
if md is not None:
self.last_md_ts = md.logMonoTime
self.last_model = cur_time
self.model_dead = False
self.PP.update(CS.vEgo, md)
if self.last_gps_planner_plan is not None:
plan = self.last_gps_planner_plan.gpsPlannerPlan
self.gps_planner_active = plan.valid
if plan.valid:
self.PP.d_poly = plan.poly
self.PP.p_poly = plan.poly
self.PP.c_poly = plan.poly
self.PP.l_prob = 0.0
self.PP.r_prob = 0.0
self.PP.c_prob = 1.0
if l20 is not None:
self.last_l20_ts = l20.logMonoTime
self.last_l20 = cur_time
self.radar_dead = False
self.radar_errors = list(l20.live20.radarErrors)
self.v_acc_start = self.v_acc_sol
self.a_acc_start = self.a_acc_sol
self.acc_start_time = cur_time
self.lead_1 = l20.live20.leadOne
self.lead_2 = l20.live20.leadTwo
enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping)
following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0
# Calculate speed for normal cruise control
if enabled:
accel_limits = map(float, calc_cruise_accel_limits(CS.vEgo, following))
# TODO: make a separate lookup for jerk tuning
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, 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])
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint,
accel_limits[1], accel_limits[0],
jerk_limits[1], jerk_limits[0],
_DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else CS.vEgo
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.v_acc_sol = reset_speed
self.a_acc_sol = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.leftBlinker or CS.rightBlinker
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, CS.aEgo,
self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
self.lead_1.yRel, self.lead_1.vLat,
self.lead_1.fcw, blinkers) \
and not CS.brakePressed
if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
if cur_time - self.last_model > 0.5:
self.model_dead = True
if cur_time - self.last_l20 > 0.5:
self.radar_dead = True
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
events = []
if self.model_dead:
events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# if self.radar_dead or 'commIssue' in self.radar_errors:
# events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if 'fault' in self.radar_errors:
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if LaC.mpc_solution[0].cost > 10000. or LaC.mpc_nans: # TODO: find a better way to detect when MPC did not converge
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# Interpolation of trajectory
dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start)
self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0
plan_send.plan.events = events
plan_send.plan.mdMonoTime = self.last_md_ts
plan_send.plan.l20MonoTime = self.last_l20_ts
# lateral plan
plan_send.plan.lateralValid = not self.model_dead
plan_send.plan.dPoly = map(float, self.PP.d_poly)
plan_send.plan.laneWidth = float(self.PP.lane_width)
# longitudal plan
plan_send.plan.longitudinalValid = not self.radar_dead
plan_send.plan.vCruise = self.v_cruise
plan_send.plan.aCruise = self.a_cruise
plan_send.plan.vTarget = self.v_acc_sol
plan_send.plan.aTarget = self.a_acc_sol
plan_send.plan.vTargetFuture = self.v_acc_future
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
plan_send.plan.gpsPlannerActive = self.gps_planner_active
# Send out fcw
fcw = self.fcw and (self.fcw_enabled or LoC.long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
return plan_send
def update(self, CS, LoC, v_cruise_kph, user_distracted):
cur_time = sec_since_boot()
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
md = messaging.recv_sock(self.model)
if md is not None:
self.last_md_ts = md.logMonoTime
self.last_model = cur_time
self.model_dead = False
self.PP.update(CS.vEgo, md)
l20 = messaging.recv_sock(self.live20) if md is None else None
if l20 is not None:
self.last_l20_ts = l20.logMonoTime
self.last_l20 = cur_time
self.radar_dead = False
self.radar_errors = list(l20.live20.radarErrors)
self.v_acc_start = self.v_acc_sol
self.a_acc_start = self.a_acc_sol
self.acc_start_time = cur_time
self.lead_1 = l20.live20.leadOne
self.lead_2 = l20.live20.leadTwo
enabled = (LoC.long_control_state
== LongCtrlState.pid) or (LoC.long_control_state
== LongCtrlState.stopping)
following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0
# Calculate speed for normal cruise control
if enabled:
accel_limits = map(
float, calc_cruise_accel_limits(CS.vEgo, following))
# TODO: make a separate lookup for jerk tuning
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
]
accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle,
accel_limits, self.CP)
if user_distracted:
# if user is not responsive to awareness alerts, then start a smooth deceleration
accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
accel_limits[0] = min(accel_limits[0], accel_limits[1])
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits[1], accel_limits[0], jerk_limits[1],
jerk_limits[0], _DT_MPC)
else:
starting = LoC.long_control_state == LongCtrlState.starting
self.v_cruise = CS.vEgo
self.a_cruise = self.CP.startAccel if starting else CS.aEgo
self.v_acc_start = CS.vEgo
self.a_acc_start = self.CP.startAccel if starting else CS.aEgo
self.v_acc = CS.vEgo
self.a_acc = self.CP.startAccel if starting else CS.aEgo
self.v_acc_sol = CS.vEgo
self.a_acc_sol = self.CP.startAccel if starting else CS.aEgo
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.leftBlinker or CS.rightBlinker
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo,
self.lead_1.vLead, self.lead_1.yRel, self.lead_1.vLat,
self.lead_1.fcw, blinkers) \
and not CS.brakePressed
if self.fcw:
cloudlog.info("FCW triggered")
if cur_time - self.last_model > 0.5:
self.model_dead = True
if cur_time - self.last_l20 > 0.5:
self.radar_dead = True
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
events = []
if self.model_dead:
events.append(
create_event('modelCommIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.radar_dead or 'commIssue' in self.radar_errors:
events.append(
create_event('radarCommIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if 'fault' in self.radar_errors:
events.append(
create_event('radarFault',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# Interpolation of trajectory
dt = min(
cur_time - self.acc_start_time, _DT_MPC + _DT
) + _DT # no greater than dt mpc + dt, to prevent too high extraps
self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc -
self.a_acc_start)
self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol +
self.a_acc_start) / 2.0
plan_send.plan.events = events
plan_send.plan.mdMonoTime = self.last_md_ts
plan_send.plan.l20MonoTime = self.last_l20_ts
# lateral plan
plan_send.plan.lateralValid = not self.model_dead
plan_send.plan.dPoly = map(float, self.PP.d_poly)
plan_send.plan.laneWidth = float(self.PP.lane_width)
# longitudal plan
plan_send.plan.longitudinalValid = not self.radar_dead
plan_send.plan.vCruise = self.v_cruise
plan_send.plan.aCruise = self.a_cruise
plan_send.plan.vTarget = self.v_acc_sol
plan_send.plan.aTarget = self.a_acc_sol
plan_send.plan.vTargetFuture = self.v_acc_future
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
# Send out fcw
fcw = self.fcw and (self.fcw_enabled
or LoC.long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
return plan_send
def update_pdl(self, enabled, CS, frame, actuators, pcm_speed):
cur_time = sec_since_boot()
idx = self.pedal_idx
self.pedal_idx = (self.pedal_idx + 1) % 16
if not CS.pedal_interceptor_available or not enabled:
return 0., 0, idx
# Alternative speed decision logic that uses the lead car's distance
# and speed more directly.
# Bring in the lead car distance from the Live20 feed
l20 = None
if enabled:
for socket, _ in self.poller.poll(0):
if socket is self.live20:
l20 = messaging.recv_one(socket)
break
if l20 is not None:
self.lead_1 = l20.live20.leadOne
if _is_present(self.lead_1):
self.lead_last_seen_time_ms = _current_time_millis()
self.continuous_lead_sightings += 1
else:
self.continuous_lead_sightings = 0
self.md_ts = l20.live20.mdMonoTime
self.l100_ts = l20.live20.l100MonoTime
brake_max, accel_max = calc_cruise_accel_limits(CS, self.lead_1)
output_gb = 0
####################################################################
# this mode (Follow) uses the Follow logic created by JJ for ACC
#
# once the speed is detected we have to use our own PID to determine
# how much accel and break we have to do
####################################################################
if PCCModes.is_selected(FollowMode(), CS.cstm_btns):
self.v_pid = self.calc_follow_speed_ms(CS)
# cruise speed can't be negative even is user is distracted
self.v_pid = max(self.v_pid, 0.)
enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [
LongCtrlState.pid, LongCtrlState.stopping
]
if self.enable_pedal_cruise:
jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1,
self.pedal_speed_kph,
self.lead_last_seen_time_ms)
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, self.v_pid, accel_max,
brake_max, jerk_max, jerk_min, _DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = self.v_pid
self.v_acc_start = self.v_acc_sol
self.a_acc_start = self.a_acc_sol
self.acc_start_time = cur_time
# Interpolation of trajectory
dt = min(
cur_time - self.acc_start_time, _DT_MPC + _DT
) + _DT # no greater than dt mpc + dt, to prevent too high extraps
self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (
self.a_acc - self.a_acc_start)
self.v_acc_sol = self.v_acc_start + dt * (
self.a_acc_sol + self.a_acc_start) / 2.0
# we will try to feed forward the pedal position.... we might want to feed the last output_gb....
# it's all about testing now.
vTarget = clip(self.v_acc_sol, 0, self.v_pid)
self.vTargetFuture = clip(self.v_acc_future, 0, self.v_pid)
t_go, t_brake = self.LoC.update(
self.enable_pedal_cruise, CS.v_ego, CS.brake_pressed != 0,
CS.standstill, False, self.v_pid, vTarget,
self.vTargetFuture, self.a_acc_sol, CS.CP, None)
output_gb = t_go - t_brake
#print "Output GB Follow:", output_gb
else:
self.LoC.reset(CS.v_ego)
print "PID reset"
output_gb = 0.
starting = self.LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.a_ego, 0.0)
reset_speed = MIN_CAN_SPEED if starting else CS.v_ego
reset_accel = CS.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.v_acc_sol = reset_speed
self.a_acc_sol = reset_accel
self.v_pid = reset_speed
##############################################################
# This mode uses the longitudinal MPC built in OP
#
# we use the values from actuator.accel and actuator.brake
##############################################################
elif PCCModes.is_selected(OpMode(), CS.cstm_btns):
output_gb = actuators.gas - actuators.brake
##############################################################
# This is an experimental mode that is probably broken.
#
# Don't use it.
#
# Ratios are centered at 1. They can be multiplied together.
# Factors are centered around 0. They can be multiplied by constants.
# For example +9% is a 1.06 ratio or 0.09 factor.
##############################################################
elif PCCModes.is_selected(ExperimentalMode(), CS.cstm_btns):
output_gb = 0.0
if enabled and self.enable_pedal_cruise:
MAX_DECEL_RATIO = 0
MAX_ACCEL_RATIO = 1.1
available_speed_kph = self.pedal_speed_kph - CS.v_ego * CV.MS_TO_KPH
# Hold accel if radar gives intermittent readings at great distance.
# Makes the car less skittish when first making radar contact.
if (_is_present(self.lead_1)
and self.continuous_lead_sightings < 8
and _sec_til_collision(self.lead_1) > 3
and self.lead_1.dRel > 60):
output_gb = self.last_output_gb
# Hold speed in turns if no car is seen
elif CS.angle_steers >= 5.0 and not _is_present(self.lead_1):
pass
# Try to stay 2 seconds behind lead, matching their speed.
elif _is_present(self.lead_1):
weighted_d_ratio = _weighted_distance_ratio(
self.lead_1, CS.v_ego, MAX_DECEL_RATIO,
MAX_ACCEL_RATIO)
weighted_v_ratio = _weighted_velocity_ratio(
self.lead_1, CS.v_ego, MAX_DECEL_RATIO,
MAX_ACCEL_RATIO)
# Don't bother decelerating if the lead is already pulling away
if weighted_d_ratio < 1 and weighted_v_ratio > 1.01:
gas_brake_ratio = max(1, self.last_output_gb + 1)
else:
gas_brake_ratio = weighted_d_ratio * weighted_v_ratio
# rescale around 0 rather than 1.
output_gb = gas_brake_ratio - 1
# If no lead has been seen recently, accelerate to max speed.
else:
# An acceleration factor that drops off as we aproach max speed.
max_speed_factor = min(available_speed_kph, 3) / 3
# An acceleration factor that increases as time passes without seeing
# a lead car.
time_factor = (_current_time_millis() -
self.lead_last_seen_time_ms) / 3000
time_factor = clip(time_factor, 0, 1)
output_gb = 0.14 * max_speed_factor * time_factor
# If going above the max configured PCC speed, slow. This should always
# be in force so it is not part of the if/else block above.
if available_speed_kph < 0:
# linearly brake harder, hitting -1 at 10kph over
speed_limited_gb = max(available_speed_kph, -10) / 10.0
# This is a minimum braking amount. The logic above may ask for more.
output_gb = min(output_gb, speed_limited_gb)
######################################################################################
# Determine pedal "zero"
#
#save position for cruising (zero acc, zero brake, no torque) when we are above 10 MPH
######################################################################################
if (CS.torqueLevel < TORQUE_LEVEL_ACC
and CS.torqueLevel > TORQUE_LEVEL_DECEL
and CS.v_ego >= 10. * CV.MPH_TO_MS and abs(CS.torqueLevel) <
abs(self.lastTorqueForPedalForZeroTorque)):
self.PedalForZeroTorque = self.prev_tesla_accel
self.lastTorqueForPedalForZeroTorque = CS.torqueLevel
#print "Detected new Pedal For Zero Torque at %s" % (self.PedalForZeroTorque)
#print "Torque level at detection %s" % (CS.torqueLevel)
#print "Speed level at detection %s" % (CS.v_ego * CV.MS_TO_MPH)
self.last_output_gb = output_gb
# accel and brake
apply_accel = clip(output_gb, 0., accel_max)
MPC_BRAKE_MULTIPLIER = 6.
apply_brake = -clip(output_gb * MPC_BRAKE_MULTIPLIER, -brake_max, 0.)
# if speed is over 5mpg, the "zero" is at PedalForZeroTorque; otherwise it is zero
pedal_zero = 0.
if CS.v_ego >= 5. * CV.MPH_TO_MS:
pedal_zero = self.PedalForZeroTorque
tesla_brake = clip((1. - apply_brake) * pedal_zero, 0, pedal_zero)
tesla_accel = clip(apply_accel * MAX_PEDAL_VALUE, 0,
MAX_PEDAL_VALUE - tesla_brake)
tesla_pedal = tesla_brake + tesla_accel
tesla_pedal, self.accel_steady = accel_hysteresis(
tesla_pedal, self.accel_steady, enabled)
tesla_pedal = clip(tesla_pedal, self.prev_tesla_pedal - PEDAL_MAX_DOWN,
self.prev_tesla_pedal + PEDAL_MAX_UP)
tesla_pedal = clip(tesla_pedal, 0.,
MAX_PEDAL_VALUE) if self.enable_pedal_cruise else 0.
enable_pedal = 1. if self.enable_pedal_cruise else 0.
self.torqueLevel_last = CS.torqueLevel
self.prev_tesla_pedal = tesla_pedal * enable_pedal
self.prev_tesla_accel = apply_accel * enable_pedal
self.prev_v_ego = CS.v_ego
self.last_md_ts = self.md_ts
self.last_l100_ts = self.l100_ts
return self.prev_tesla_pedal, enable_pedal, idx
def update(self, sm, pm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
# dp
self.dp_profile = sm['dragonConf'].dpAccelProfile
self.dp_slow_on_curve = sm['dragonConf'].dpSlowOnCurve
# dp - slow on curve from 0.7.6.1
if self.dp_slow_on_curve and len(sm['model'].path.poly):
path = list(sm['model'].path.poly)
# Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function
# y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b
# k = y'' / (1 + y'^2)^1.5
# TODO: compute max speed without using a list of points and without numpy
y_p = 3 * path[0] * self.path_x**2 + 2 * path[
1] * self.path_x + path[2]
y_pp = 6 * path[0] * self.path_x + 2 * path[1]
curv = y_pp / (1. + y_p**2)**1.5
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
model_speed = np.min(v_curvature)
model_speed = max(20.0 * CV.MPH_TO_MS,
model_speed) # Don't slow down below 20mph
else:
model_speed = MAX_SPEED
# Calculate speed for normal cruise control
pedal_pressed = sm['carState'].gasPressed or sm['carState'].brakePressed
if enabled and not self.first_loop and not pedal_pressed:
if self.dp_profile == DP_OFF:
accel_limits = [
float(x)
for x in calc_cruise_accel_limits(v_ego, following)
]
else:
accel_limits = [
float(x) for x in dp_calc_cruise_accel_limits(
v_ego, following, self.dp_profile)
]
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(
v_ego, sm['carState'].steeringAngle, 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])
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
# dp - slow on curve from 0.7.6.1
if self.dp_slow_on_curve:
self.v_model, self.a_model = speed_smoother(
self.v_acc_start, self.a_acc_start, model_speed,
2 * accel_limits[1], accel_limits[0], 2 * jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(pm, sm['carState'], lead_1)
self.mpc2.update(pm, sm['carState'], lead_2)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, sm['controlsState'].active,
v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead,
lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw,
blinkers) and not sm['carState'].brakePressed
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
radar_dead = not sm.alive['radarState']
radar_errors = list(sm['radarState'].radarErrors)
radar_fault = car.RadarData.Error.fault in radar_errors
radar_can_error = car.RadarData.Error.canError in radar_errors
# **** send the plan ****
plan_send = messaging.new_message('plan')
plan_send.valid = sm.all_alive_and_valid(
service_list=['carState', 'controlsState', 'radarState'])
plan_send.plan.mdMonoTime = sm.logMonoTime['model']
plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
# longitudal plan
plan_send.plan.vCruise = float(self.v_cruise)
plan_send.plan.aCruise = float(self.a_cruise)
plan_send.plan.vStart = float(self.v_acc_start)
plan_send.plan.aStart = float(self.a_acc_start)
plan_send.plan.vTarget = float(self.v_acc)
plan_send.plan.aTarget = float(self.a_acc)
plan_send.plan.vTargetFuture = float(self.v_acc_future)
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(radar_can_error)
plan_send.plan.processingDelay = (plan_send.logMonoTime /
1e9) - sm.rcv_time['radarState']
# Send out fcw
plan_send.plan.fcw = fcw
pm.send('plan', plan_send)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (
self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (
a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
self.first_loop = False
def update(self, sm, pm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
# Calculate speed for normal cruise control
if enabled and not self.first_loop and not sm['carState'].gasPressed:
accel_limits = [
float(x) for x in calc_cruise_accel_limits(v_ego, following)
]
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(
v_ego, sm['carState'].steeringAngle, 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])
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = self.CP.minSpeedCan if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(pm, sm['carState'], lead_1)
self.mpc2.update(pm, sm['carState'], lead_2)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, sm['controlsState'].active,
v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead,
lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw,
blinkers) and not sm['carState'].brakePressed
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
radar_dead = not sm.alive['radarState']
radar_errors = list(sm['radarState'].radarErrors)
radar_fault = car.RadarData.Error.fault in radar_errors
radar_can_error = car.RadarData.Error.canError in radar_errors
# **** send the plan ****
plan_send = messaging.new_message('plan')
plan_send.valid = sm.all_alive_and_valid(
service_list=['carState', 'controlsState', 'radarState'])
plan_send.plan.mdMonoTime = sm.logMonoTime['model']
plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
# longitudal plan
plan_send.plan.vCruise = float(self.v_cruise)
plan_send.plan.aCruise = float(self.a_cruise)
plan_send.plan.vStart = float(self.v_acc_start)
plan_send.plan.aStart = float(self.a_acc_start)
plan_send.plan.vTarget = float(self.v_acc)
plan_send.plan.aTarget = float(self.a_acc)
plan_send.plan.vTargetFuture = float(self.v_acc_future)
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(radar_can_error)
plan_send.plan.processingDelay = (plan_send.logMonoTime /
1e9) - sm.rcv_time['radarState']
# Send out fcw
plan_send.plan.fcw = fcw
pm.send('plan', plan_send)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (
self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (
a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
self.first_loop = False
def update(self, sm, CP, VM, PP, live_map_data):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
v_speedlimit = NO_CURVATURE_SPEED
v_curvature = NO_CURVATURE_SPEED
#map_age = cur_time - rcv_times['liveMapData']
map_valid = False # live_map_data.liveMapData.mapValid and map_age < 10.0
# Speed limit and curvature
set_speed_limit_active = self.params.get(
"LimitSetSpeed") == "1" and self.params.get(
"SpeedLimitOffset") is not None
if set_speed_limit_active and map_valid:
if live_map_data.liveMapData.speedLimitValid:
speed_limit = live_map_data.liveMapData.speedLimit
offset = float(self.params.get("SpeedLimitOffset"))
v_speedlimit = speed_limit + offset
if live_map_data.liveMapData.curvatureValid:
curvature = abs(live_map_data.liveMapData.curvature)
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = math.sqrt(a_y_max / max(1e-4, curvature))
v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
decel_for_turn = bool(
v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.]))
v_cruise_setpoint = min([v_cruise_setpoint, v_curvature, v_speedlimit])
# Calculate speed for normal cruise control
if enabled:
accel_limits = [
float(x) for x in calc_cruise_accel_limits(v_ego, following)
]
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
accel_limits = limit_accel_in_turns(v_ego,
sm['carState'].steeringAngle,
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])
# Change accel limits based on time remaining to turn
if decel_for_turn:
time_to_turn = max(
1.0, live_map_data.liveMapData.distToTurn /
max(self.v_cruise, 1.))
required_decel = min(0, (v_curvature - self.v_cruise) /
time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits[1], accel_limits[0], jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(sm['carState'], lead_1, v_cruise_setpoint)
self.mpc2.update(sm['carState'], lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, v_ego, sm['carState'].aEgo,
lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat,
lead_1.fcw, blinkers) and not sm['carState'].brakePressed
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
radar_dead = not sm.alive['radarState']
radar_errors = list(sm['radarState'].radarErrors)
radar_fault = car.RadarData.Error.fault in radar_errors
radar_can_error = car.RadarData.Error.canError in radar_errors
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
plan_send.valid = sm.all_alive_and_valid(
service_list=['carState', 'controlsState', 'radarState'])
plan_send.plan.mdMonoTime = sm.logMonoTime['model']
plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
# longitudal plan
plan_send.plan.vCruise = self.v_cruise
plan_send.plan.aCruise = self.a_cruise
plan_send.plan.vStart = self.v_acc_start
plan_send.plan.aStart = self.a_acc_start
plan_send.plan.vTarget = self.v_acc
plan_send.plan.aTarget = self.a_acc
plan_send.plan.vTargetFuture = self.v_acc_future
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
plan_send.plan.vCurvature = v_curvature
plan_send.plan.decelForTurn = decel_for_turn
plan_send.plan.mapValid = map_valid
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(radar_can_error)
plan_send.plan.processingDelay = (plan_send.logMonoTime /
1e9) - sm.rcv_time['radarState']
# Send out fcw
fcw = fcw and (self.fcw_enabled
or long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (DT_PLAN / LON_MPC_STEP) * (
self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + DT_PLAN * (a_acc_sol +
self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
def update(self, sm, pm, CP, VM, PP):
self.arne182 = None
for socket, _ in self.poller.poll(0):
if socket is self.arne182Status:
self.arne182 = arne182.Arne182Status.from_bytes(socket.recv())
if self.arne182 is None:
gasbuttonstatus = 0
else:
gasbuttonstatus = self.arne182.gasbuttonstatus
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
if gasbuttonstatus == 1:
speed_ahead_distance = 150
elif gasbuttonstatus == 2:
speed_ahead_distance = 350
else:
speed_ahead_distance = 250
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
v_speedlimit = NO_CURVATURE_SPEED
v_curvature_map = NO_CURVATURE_SPEED
v_speedlimit_ahead = NO_CURVATURE_SPEED
if len(sm['model'].path.poly):
path = list(sm['model'].path.poly)
# Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function
# y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b
# k = y'' / (1 + y'^2)^1.5
# TODO: compute max speed without using a list of points and without numpy
y_p = 3 * path[0] * self.path_x**2 + 2 * path[
1] * self.path_x + path[2]
y_pp = 6 * path[0] * self.path_x + 2 * path[1]
curv = y_pp / (1. + y_p**2)**1.5
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
model_speed = np.min(v_curvature)
model_speed = max(20.0 * CV.MPH_TO_MS,
model_speed) # Don't slow down below 20mph
else:
model_speed = MAX_SPEED
now = datetime.now()
try:
if sm['liveMapData'].speedLimitValid and osm and (
sm['liveMapData'].lastGps.timestamp -
time.mktime(now.timetuple()) * 1000) < 10000:
speed_limit = sm['liveMapData'].speedLimit
v_speedlimit = speed_limit + offset
else:
speed_limit = None
if sm['liveMapData'].speedLimitAheadValid and sm[
'liveMapData'].speedLimitAheadDistance < speed_ahead_distance and (
sm['liveMapData'].lastGps.timestamp -
time.mktime(now.timetuple()) * 1000) < 10000:
distanceatlowlimit = 50
if sm['liveMapData'].speedLimitAhead < 21 / 3.6:
distanceatlowlimit = speed_ahead_distance = (
v_ego - sm['liveMapData'].speedLimitAhead) * 3.6 * 2
if distanceatlowlimit < 50:
distanceatlowlimit = 0
distanceatlowlimit = min(distanceatlowlimit, 100)
speed_ahead_distance = (
v_ego - sm['liveMapData'].speedLimitAhead) * 3.6 * 5
speed_ahead_distance = min(speed_ahead_distance, 300)
speed_ahead_distance = max(speed_ahead_distance, 50)
if speed_limit is not None and sm[
'liveMapData'].speedLimitAheadDistance > distanceatlowlimit and v_ego + 3 < sm[
'liveMapData'].speedLimitAhead + (
speed_limit - sm['liveMapData'].speedLimitAhead
) * sm[
'liveMapData'].speedLimitAheadDistance / speed_ahead_distance:
speed_limit_ahead = sm['liveMapData'].speedLimitAhead + (
speed_limit - sm['liveMapData'].speedLimitAhead) * (
sm['liveMapData'].speedLimitAheadDistance -
distanceatlowlimit) / (speed_ahead_distance -
distanceatlowlimit)
else:
speed_limit_ahead = sm['liveMapData'].speedLimitAhead
v_speedlimit_ahead = speed_limit_ahead + offset
if sm['liveMapData'].curvatureValid and osm and (
sm['liveMapData'].lastGps.timestamp -
time.mktime(now.timetuple()) * 1000) < 10000:
curvature = abs(sm['liveMapData'].curvature)
radius = 1 / max(1e-4, curvature)
if radius > 500:
c = 0.7 # 0.7 at 1000m = 95 kph
elif radius > 250:
c = 2.7 - 1 / 250 * radius # 1.7 at 264m 76 kph
else:
c = 3.0 - 13 / 2500 * radius # 3.0 at 15m 24 kph
v_curvature_map = math.sqrt(c * radius)
v_curvature_map = min(NO_CURVATURE_SPEED, v_curvature_map)
except KeyError:
pass
decel_for_turn = bool(v_curvature_map < min(
[v_cruise_setpoint, v_speedlimit, v_ego + 1.]))
v_cruise_setpoint = min([
v_cruise_setpoint, v_curvature_map, v_speedlimit,
v_speedlimit_ahead
])
# Calculate speed for normal cruise control
if enabled:
accel_limits = [
float(x) for x in calc_cruise_accel_limits(
v_ego, following, gasbuttonstatus)
]
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(
v_ego, sm['carState'].steeringAngle, 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])
if decel_for_turn and sm[
'liveMapData'].distToTurn < speed_ahead_distance:
time_to_turn = max(
1.0, sm['liveMapData'].distToTurn / max(
(v_ego + v_curvature_map) / 2, 1.))
required_decel = min(0,
(v_curvature_map - v_ego) / time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
if v_speedlimit_ahead < v_speedlimit and self.longitudinalPlanSource == 'cruise' and v_ego > v_speedlimit_ahead:
required_decel = min(
0,
(v_speedlimit_ahead * v_speedlimit_ahead - v_ego * v_ego) /
(sm['liveMapData'].speedLimitAheadDistance * 2))
required_decel = max(required_decel, -3.0)
accel_limits[0] = required_decel
accel_limits[1] = required_decel
self.a_acc_start = required_decel
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
self.v_model, self.a_model = speed_smoother(
self.v_acc_start, self.a_acc_start, model_speed,
2 * accel_limits[1], accel_limits[0], 2 * jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint)
self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, sm['controlsState'].active,
v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead,
lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw,
blinkers) and not sm['carState'].brakePressed
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
radar_dead = not sm.alive['radarState']
radar_errors = list(sm['radarState'].radarErrors)
radar_fault = car.RadarData.Error.fault in radar_errors
radar_can_error = car.RadarData.Error.canError in radar_errors
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
plan_send.valid = sm.all_alive_and_valid(
service_list=['carState', 'controlsState', 'radarState'])
plan_send.plan.mdMonoTime = sm.logMonoTime['model']
plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
# longitudal plan
plan_send.plan.vCruise = float(self.v_cruise)
plan_send.plan.aCruise = float(self.a_cruise)
plan_send.plan.vStart = float(self.v_acc_start)
plan_send.plan.aStart = float(self.a_acc_start)
plan_send.plan.vTarget = float(self.v_acc)
plan_send.plan.aTarget = float(self.a_acc)
plan_send.plan.vTargetFuture = float(self.v_acc_future)
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
plan_send.plan.vCurvature = float(v_curvature_map)
plan_send.plan.decelForTurn = bool(
decel_for_turn
or v_speedlimit_ahead < min([v_speedlimit, v_ego + 1.]))
plan_send.plan.mapValid = True
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(radar_can_error)
plan_send.plan.processingDelay = (plan_send.logMonoTime /
1e9) - sm.rcv_time['radarState']
# Send out fcw
plan_send.plan.fcw = fcw
pm.send('plan', plan_send)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (DT_PLAN / LON_MPC_STEP) * (
self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + DT_PLAN * (a_acc_sol +
self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
def update_pdl(self, enabled, CS, frame, actuators, pcm_speed):
cur_time = sec_since_boot()
idx = self.pedal_idx
self.pedal_idx = (self.pedal_idx + 1) % 16
if not CS.pedal_interceptor_available or not enabled:
return 0., 0, idx
# Alternative speed decision logic that uses the lead car's distance
# and speed more directly.
# Bring in the lead car distance from the Live20 feed
l20 = None
if enabled:
for socket, _ in self.poller.poll(0):
if socket is self.live20:
l20 = messaging.recv_one(socket)
break
if l20 is not None:
self.lead_1 = l20.live20.leadOne
if _is_present(self.lead_1):
self.lead_last_seen_time_ms = _current_time_millis()
self.continuous_lead_sightings += 1
else:
self.continuous_lead_sightings = 0
self.md_ts = l20.live20.mdMonoTime
self.l100_ts = l20.live20.l100MonoTime
brake_max, accel_max = calc_cruise_accel_limits(CS, self.lead_1)
output_gb = 0
####################################################################
# this mode (Follow) uses the Follow logic created by JJ for ACC
#
# once the speed is detected we have to use our own PID to determine
# how much accel and break we have to do
####################################################################
if PCCModes.is_selected(FollowMode(), CS.cstm_btns):
self.v_pid = self.calc_follow_speed_ms(CS)
# cruise speed can't be negative even is user is distracted
self.v_pid = max(self.v_pid, 0.)
enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [
LongCtrlState.pid, LongCtrlState.stopping
]
if self.enable_pedal_cruise:
jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1,
self.pedal_speed_kph,
self.lead_last_seen_time_ms)
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, self.v_pid, accel_max,
brake_max, jerk_max, jerk_min, _DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = self.v_pid
self.v_acc_start = self.v_acc_sol
self.a_acc_start = self.a_acc_sol
self.acc_start_time = cur_time
# Interpolation of trajectory
dt = min(
cur_time - self.acc_start_time, _DT_MPC + _DT
) + _DT # no greater than dt mpc + dt, to prevent too high extraps
self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (
self.a_acc - self.a_acc_start)
self.v_acc_sol = self.v_acc_start + dt * (
self.a_acc_sol + self.a_acc_start) / 2.0
# we will try to feed forward the pedal position.... we might want to feed the last output_gb....
# it's all about testing now.
vTarget = clip(self.v_acc_sol, 0, self.v_pid)
self.vTargetFuture = clip(self.v_acc_future, 0, self.v_pid)
t_go, t_brake = self.LoC.update(
self.enable_pedal_cruise, CS.v_ego, CS.brake_pressed != 0,
CS.standstill, False, self.v_pid, vTarget,
self.vTargetFuture, self.a_acc_sol, CS.CP, None)
output_gb = t_go - t_brake
#print "Output GB Follow:", output_gb
else:
self.LoC.reset(CS.v_ego)
#print "PID reset"
output_gb = 0.
starting = self.LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.a_ego, 0.0)
reset_speed = MIN_CAN_SPEED if starting else CS.v_ego
reset_accel = CS.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.v_acc_sol = reset_speed
self.a_acc_sol = reset_accel
self.v_pid = reset_speed
##############################################################
# This mode uses the longitudinal MPC built in OP
#
# we use the values from actuator.accel and actuator.brake
##############################################################
elif PCCModes.is_selected(OpMode(), CS.cstm_btns):
output_gb = actuators.gas - actuators.brake
######################################################################################
# Determine pedal "zero"
#
#save position for cruising (zero acc, zero brake, no torque) when we are above 10 MPH
######################################################################################
if (CS.torqueLevel < TORQUE_LEVEL_ACC
and CS.torqueLevel > TORQUE_LEVEL_DECEL
and CS.v_ego >= 10. * CV.MPH_TO_MS and abs(CS.torqueLevel) <
abs(self.lastTorqueForPedalForZeroTorque)):
self.PedalForZeroTorque = self.prev_tesla_accel
self.lastTorqueForPedalForZeroTorque = CS.torqueLevel
#print "Detected new Pedal For Zero Torque at %s" % (self.PedalForZeroTorque)
#print "Torque level at detection %s" % (CS.torqueLevel)
#print "Speed level at detection %s" % (CS.v_ego * CV.MS_TO_MPH)
self.last_output_gb = output_gb
# accel and brake
apply_accel = clip(output_gb, 0., accel_max)
MPC_BRAKE_MULTIPLIER = 6.
apply_brake = -clip(output_gb * MPC_BRAKE_MULTIPLIER, -brake_max, 0.)
# if speed is over 5mpg, the "zero" is at PedalForZeroTorque; otherwise it is zero
pedal_zero = 0.
if CS.v_ego >= 5. * CV.MPH_TO_MS:
pedal_zero = self.PedalForZeroTorque
tesla_brake = clip((1. - apply_brake) * pedal_zero, 0, pedal_zero)
tesla_accel = clip(apply_accel * MAX_PEDAL_VALUE, 0,
MAX_PEDAL_VALUE - tesla_brake)
tesla_pedal = tesla_brake + tesla_accel
tesla_pedal, self.accel_steady = accel_hysteresis(
tesla_pedal, self.accel_steady, enabled)
tesla_pedal = clip(tesla_pedal, self.prev_tesla_pedal - PEDAL_MAX_DOWN,
self.prev_tesla_pedal + PEDAL_MAX_UP)
tesla_pedal = clip(tesla_pedal, 0.,
MAX_PEDAL_VALUE) if self.enable_pedal_cruise else 0.
enable_pedal = 1. if self.enable_pedal_cruise else 0.
self.torqueLevel_last = CS.torqueLevel
self.prev_tesla_pedal = tesla_pedal * enable_pedal
self.prev_tesla_accel = apply_accel * enable_pedal
self.prev_v_ego = CS.v_ego
self.last_md_ts = self.md_ts
self.last_l100_ts = self.l100_ts
return self.prev_tesla_pedal, enable_pedal, idx
def update(self, sm, CP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
self.vego = v_ego
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
self.v_acc_start = self.v_acc_next
self.a_acc_start = self.a_acc_next
self.target_speed_map_counter += 1
if self.target_speed_map_counter >= (
50 + self.target_speed_map_counter1
) and self.target_speed_map_counter_check == False:
self.target_speed_map_counter_check = True
os.system(
"logcat -d -s opkrspdlimit,opkrspd2limit | grep opkrspd | tail -n 1 | awk \'{print $7}\' > /data/params/d/LimitSetSpeedCamera &"
)
os.system(
"logcat -d -s opkrspddist | grep opkrspd | tail -n 1 | awk \'{print $7}\' > /data/params/d/LimitSetSpeedCameraDist &"
)
self.target_speed_map_counter3 += 1
if self.target_speed_map_counter3 > 2:
self.target_speed_map_counter3 = 0
os.system("logcat -c &")
elif self.target_speed_map_counter >= (75 +
self.target_speed_map_counter1):
self.target_speed_map_counter1 = 0
self.target_speed_map_counter = 0
self.target_speed_map_counter_check = False
mapspeed = self.params.get("LimitSetSpeedCamera", encoding="utf8")
mapspeeddist = self.params.get("LimitSetSpeedCameraDist",
encoding="utf8")
if mapspeed is not None and mapspeeddist is not None:
mapspeed = int(float(mapspeed.rstrip('\n')))
mapspeeddist = int(float(mapspeeddist.rstrip('\n')))
if mapspeed > 29:
self.target_speed_map = mapspeed
self.target_speed_map_dist = mapspeeddist
if self.target_speed_map_dist > 1001:
self.target_speed_map_block = True
self.target_speed_map_counter1 = 80
os.system("echo -n 1 > /data/params/d/OpkrSafetyCamera &")
os.system("logcat -c &")
else:
self.target_speed_map = 0
self.target_speed_map_dist = 0
self.target_speed_map_block = False
elif mapspeed is None and mapspeeddist is None and self.target_speed_map_counter2 < 2:
self.target_speed_map_counter2 += 1
self.target_speed_map_counter = 51
self.target_speed_map = 0
self.target_speed_map_dist = 0
self.target_speed_map_counter_check = True
self.target_speed_map_block = False
self.target_speed_map_sign = False
else:
self.target_speed_map_counter = 49
self.target_speed_map_counter2 = 0
self.target_speed_map = 0
self.target_speed_map_dist = 0
self.target_speed_map_counter_check = False
self.target_speed_map_block = False
self.target_speed_map_sign = False
if self.params.get("OpkrSafetyCamera", encoding="utf8") == "1":
os.system("echo -n 0 > /data/params/d/OpkrSafetyCamera &")
# Calculate speed for normal cruise control
if enabled and not self.first_loop and not sm[
'carState'].brakePressed and not sm['carState'].gasPressed:
accel_limits = [
float(x) for x in calc_cruise_accel_limits(v_ego, following)
]
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
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])
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = self.CP.minSpeedCan if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(sm['carState'], lead_1)
self.mpc2.update(sm['carState'], lead_2)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
self.fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, sm['controlsState'].active,
v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead,
lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw,
blinkers) and not sm['carState'].brakePressed
if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (
self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (
a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_next = v_acc_sol
self.a_acc_next = a_acc_sol
self.first_loop = False
def update_pdl(self, enabled, CS, frame, actuators, pcm_speed,
speed_limit_ms, set_speed_limit_active, speed_limit_offset,
alca_enabled):
idx = self.pedal_idx
self.prev_speed_limit_kph = self.speed_limit_kph
######################################################################################
# Determine pedal "zero"
#
#save position for cruising (zero acc, zero brake, no torque) when we are above 10 MPH
######################################################################################
if (CS.torqueLevel < TORQUE_LEVEL_ACC
and CS.torqueLevel > TORQUE_LEVEL_DECEL
and CS.v_ego >= 10. * CV.MPH_TO_MS and
abs(CS.torqueLevel) < abs(self.lastTorqueForPedalForZeroTorque)
and self.prev_tesla_accel > 0.):
self.PedalForZeroTorque = self.prev_tesla_accel
self.lastTorqueForPedalForZeroTorque = CS.torqueLevel
#print ("Detected new Pedal For Zero Torque at %s" % (self.PedalForZeroTorque))
#print ("Torque level at detection %s" % (CS.torqueLevel))
#print ("Speed level at detection %s" % (CS.v_ego * CV.MS_TO_MPH))
if set_speed_limit_active and speed_limit_ms > 0:
self.speed_limit_kph = (speed_limit_ms +
speed_limit_offset) * CV.MS_TO_KPH
if int(self.prev_speed_limit_kph) != int(self.speed_limit_kph):
self.pedal_speed_kph = self.speed_limit_kph
# reset MovingAverage for fleet speed when speed limit changes
self.fleet_speed.reset_averager()
else: # reset internal speed limit, so double pull doesn't set higher speed than current (e.g. after leaving the highway)
self.speed_limit_kph = 0.
self.pedal_idx = (self.pedal_idx + 1) % 16
if not self.pcc_available or not enabled:
return 0., 0, idx
# Alternative speed decision logic that uses the lead car's distance
# and speed more directly.
# Bring in the lead car distance from the radarState feed
radSt = messaging.recv_one_or_none(self.radarState)
mapd = messaging.recv_one_or_none(self.live_map_data)
if radSt is not None:
self.lead_1 = radSt.radarState.leadOne
if _is_present(self.lead_1):
self.lead_last_seen_time_ms = _current_time_millis()
self.continuous_lead_sightings += 1
else:
self.continuous_lead_sightings = 0
v_ego = CS.v_ego
following = self.lead_1.status and self.lead_1.dRel < MAX_RADAR_DISTANCE and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)]
accel_limits[1] *= _accel_limit_multiplier(CS, self.lead_1)
accel_limits[0] = _decel_limit(accel_limits[0], CS.v_ego, self.lead_1,
CS, self.pedal_speed_kph)
jerk_limits = [
min(-0.1, accel_limits[0] / 2.),
max(0.1, accel_limits[1] / 2.)
] # TODO: make a separate lookup for jerk tuning
#accel_limits = limit_accel_in_turns(v_ego, CS.angle_steers, accel_limits, CS.CP)
output_gb = 0
####################################################################
# this mode (Follow) uses the Follow logic created by JJ for ACC
#
# once the speed is detected we have to use our own PID to determine
# how much accel and break we have to do
####################################################################
if PCCModes.is_selected(FollowMode(), CS.cstm_btns):
enabled = self.enable_pedal_cruise and self.LoC.long_control_state in [
LongCtrlState.pid, LongCtrlState.stopping
]
# determine if pedal is pressed by human
self.prev_accelerator_pedal_pressed = self.accelerator_pedal_pressed
self.accelerator_pedal_pressed = CS.pedal_interceptor_value > 10
#reset PID if we just lifted foot of accelerator
if (not self.accelerator_pedal_pressed
) and self.prev_accelerator_pedal_pressed:
self.reset(CS.v_ego)
if self.enable_pedal_cruise and not self.accelerator_pedal_pressed:
self.v_pid = self.calc_follow_speed_ms(CS, alca_enabled)
if mapd is not None:
v_curve = max_v_in_mapped_curve_ms(mapd.liveMapData,
self.pedal_speed_kph)
if v_curve:
self.v_pid = min(self.v_pid, v_curve)
# take fleet speed into consideration
self.v_pid = min(
self.v_pid,
self.fleet_speed.adjust(
CS, self.pedal_speed_kph * CV.KPH_TO_MS, frame))
# cruise speed can't be negative even if user is distracted
self.v_pid = max(self.v_pid, 0.)
jerk_min, jerk_max = _jerk_limits(CS.v_ego, self.lead_1,
self.v_pid * CV.MS_TO_KPH,
self.lead_last_seen_time_ms,
CS)
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start,
self.a_acc_start,
self.v_pid,
accel_limits[1],
accel_limits[0],
jerk_limits[1],
jerk_limits[0], #jerk_max, jerk_min,
_DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = self.v_pid
# Interpolation of trajectory
self.a_acc_sol = self.a_acc_start + (_DT / _DT_MPC) * (
self.a_acc - self.a_acc_start)
self.v_acc_sol = self.v_acc_start + _DT * (
self.a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_start = self.v_acc_sol
self.a_acc_start = self.a_acc_sol
# we will try to feed forward the pedal position.... we might want to feed the last_output_gb....
# op feeds forward self.a_acc_sol
# it's all about testing now.
vTarget = clip(self.v_acc_sol, 0, self.v_cruise)
self.vTargetFuture = clip(self.v_acc_future, 0, self.v_pid)
feedforward = self.a_acc_sol
#feedforward = self.last_output_gb
t_go, t_brake = self.LoC.update(
self.enable_pedal_cruise, CS.v_ego, CS.brake_pressed != 0,
CS.standstill, False, self.v_cruise, vTarget,
self.vTargetFuture, feedforward, CS.CP)
output_gb = t_go - t_brake
#print ("Output GB Follow:", output_gb)
else:
self.reset(CS.v_ego)
#print ("PID reset")
output_gb = 0.
starting = self.LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.a_ego, 0.0)
reset_speed = MIN_CAN_SPEED if starting else CS.v_ego
reset_accel = CS.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.v_acc_sol = reset_speed
self.a_acc_sol = reset_accel
self.v_pid = reset_speed
self.last_speed_kph = None
##############################################################
# This mode uses the longitudinal MPC built in OP
#
# we use the values from actuator.accel and actuator.brake
##############################################################
elif PCCModes.is_selected(OpMode(), CS.cstm_btns):
output_gb = actuators.gas - actuators.brake
self.v_pid = -10.
self.last_output_gb = output_gb
# accel and brake
apply_accel = clip(
output_gb, 0.,
_accel_pedal_max(CS.v_ego, self.v_pid, self.lead_1,
self.prev_tesla_accel, CS))
MPC_BRAKE_MULTIPLIER = 6.
apply_brake = -clip(
output_gb * MPC_BRAKE_MULTIPLIER,
_brake_pedal_min(CS.v_ego, self.v_pid, self.lead_1, CS,
self.pedal_speed_kph), 0.)
# if speed is over 5mph, the "zero" is at PedalForZeroTorque; otherwise it is zero
pedal_zero = 0.
if CS.v_ego >= 5. * CV.MPH_TO_MS:
pedal_zero = self.PedalForZeroTorque
tesla_brake = clip((1. - apply_brake) * pedal_zero, 0, pedal_zero)
tesla_accel = clip(apply_accel * (MAX_PEDAL_VALUE - pedal_zero), 0,
MAX_PEDAL_VALUE - pedal_zero)
tesla_pedal = tesla_brake + tesla_accel
tesla_pedal = self.pedal_hysteresis(tesla_pedal, enabled)
tesla_pedal = clip(tesla_pedal, self.prev_tesla_pedal - PEDAL_MAX_DOWN,
self.prev_tesla_pedal + PEDAL_MAX_UP)
tesla_pedal = clip(tesla_pedal, 0.,
MAX_PEDAL_VALUE) if self.enable_pedal_cruise else 0.
enable_pedal = 1. if self.enable_pedal_cruise else 0.
self.torqueLevel_last = CS.torqueLevel
self.prev_tesla_pedal = tesla_pedal * enable_pedal
self.prev_tesla_accel = apply_accel * enable_pedal
self.prev_v_ego = CS.v_ego
return self.prev_tesla_pedal, enable_pedal, idx
def update(self, rcv_times, CS, CP, VM, PP, live20, live100, md,
live_map_data):
"""Gets called when new live20 is available"""
cur_time = sec_since_boot()
v_ego = CS.carState.vEgo
gasbuttonstatus = CS.carState.gasbuttonstatus
long_control_state = live100.live100.longControlState
v_cruise_kph = live100.live100.vCruise
force_slow_decel = live100.live100.forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
for socket, event in self.poller.poll(0):
if socket is self.lat_Control:
self.lastlat_Control = messaging.recv_one(socket).latControl
lead_1 = live20.live20.leadOne
lead_2 = live20.live20.leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
v_speedlimit = NO_CURVATURE_SPEED
v_curvature = NO_CURVATURE_SPEED
v_speedlimit_ahead = NO_CURVATURE_SPEED
map_age = cur_time - rcv_times['liveMapData']
map_valid = True #live_map_data.liveMapData.mapValid and map_age < 10.0
# Speed limit and curvature
set_speed_limit_active = self.params.get(
"LimitSetSpeed") == "1" and self.params.get(
"SpeedLimitOffset") is not None
if set_speed_limit_active and map_valid:
offset = float(self.params.get("SpeedLimitOffset"))
if live_map_data.liveMapData.speedLimitValid:
speed_limit = live_map_data.liveMapData.speedLimit
v_speedlimit = speed_limit + offset
if live_map_data.liveMapData.speedLimitAheadValid and live_map_data.liveMapData.speedLimitAheadDistance < 200:
speed_limit_ahead = live_map_data.liveMapData.speedLimitAhead
#print "Speed Ahead found"
#print speed_limit_ahead
v_speedlimit_ahead = speed_limit_ahead + offset
if live_map_data.liveMapData.curvatureValid:
curvature = abs(live_map_data.liveMapData.curvature)
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = math.sqrt(
a_y_max / max(1e-4, curvature)) / 1.3 * _brake_factor
v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
decel_for_turn = bool(
v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.]))
v_cruise_setpoint = min(
[v_cruise_setpoint, v_curvature, v_speedlimit, v_speedlimit_ahead])
# Calculate speed for normal cruise control
if enabled:
accel_limits = map(
float,
calc_cruise_accel_limits(v_ego, following, gasbuttonstatus))
if gasbuttonstatus == 0:
accellimitmaxdynamic = -0.0018 * v_ego + 0.2
jerk_limits = [
min(-0.1, accel_limits[0]),
max(accellimitmaxdynamic, accel_limits[1])
] # dynamic
elif gasbuttonstatus == 1:
accellimitmaxsport = -0.002 * v_ego + 0.4
jerk_limits = [
min(-0.25, accel_limits[0]),
max(accellimitmaxsport, accel_limits[1])
] # sport
elif gasbuttonstatus == 2:
accellimitmaxeco = -0.0015 * v_ego + 0.1
jerk_limits = [
min(-0.1, accel_limits[0]),
max(accellimitmaxeco, accel_limits[1])
] # eco
if not CS.carState.leftBlinker and not CS.carState.rightBlinker:
steering_angle = CS.carState.steeringAngle
if self.lastlat_Control and v_ego > 11:
angle_later = self.lastlat_Control.anglelater
else:
angle_later = 0
else:
angle_later = 0
steering_angle = 0
accel_limits = limit_accel_in_turns(
v_ego, steering_angle, accel_limits, self.CP,
angle_later * self.CP.steerRatio)
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])
if v_speedlimit_ahead < v_speedlimit:
time_to_speedlimit = max(
1.0, live_map_data.liveMapData.speedLimitAheadDistance /
max(self.v_cruise, 1.))
#print "Decelerating in "
#print time_to_speedlimit
required_decel = min(0, (v_speedlimit_ahead - self.v_cruise) /
time_to_speedlimit) * 5
if live_map_data.liveMapData.speedLimitAheadDistance < 100.0:
max(required_decel * 10.0, -3.0)
#print "required_decel"
#print required_decel
#print "accel_limits 0"
#print accel_limits[0]
#print "accel_limits 1"
#print accel_limits[1]
accel_limits[0] = min(accel_limits[0], required_decel)
# Change accel limits based on time remaining to turn
if decel_for_turn:
time_to_turn = max(
1.0, live_map_data.liveMapData.distToTurn /
max(self.v_cruise, 1.))
required_decel = min(0, (v_curvature - self.v_cruise) /
time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits[1], accel_limits[0], jerk_limits[1],
jerk_limits[0], _DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(CS.carState.aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(CS, lead_1, v_cruise_setpoint)
self.mpc2.update(CS, lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.carState.leftBlinker or CS.carState.rightBlinker
fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo,
lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat,
lead_1.fcw, blinkers) and not CS.carState.brakePressed
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
radar_dead = cur_time - rcv_times['live20'] > 0.5
radar_errors = list(live20.live20.radarErrors)
radar_fault = car.RadarState.Error.fault in radar_errors
radar_comm_issue = car.RadarState.Error.commIssue in radar_errors
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
plan_send.plan.mdMonoTime = md.logMonoTime
plan_send.plan.l20MonoTime = live20.logMonoTime
# longitudal plan
plan_send.plan.vCruise = self.v_cruise
plan_send.plan.aCruise = self.a_cruise
plan_send.plan.vStart = self.v_acc_start
plan_send.plan.aStart = self.a_acc_start
plan_send.plan.vTarget = self.v_acc
plan_send.plan.aTarget = self.a_acc
plan_send.plan.vTargetFuture = self.v_acc_future
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.hasrightLaneDepart = bool(
PP.r_poly[3] > -1.1 and not CS.carState.rightBlinker)
plan_send.plan.hasleftLaneDepart = bool(
PP.l_poly[3] < 1.05 and not CS.carState.leftBlinker)
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
plan_send.plan.vCurvature = v_curvature
plan_send.plan.decelForTurn = bool(
decel_for_turn
or v_speedlimit_ahead < min([v_speedlimit, v_ego + 1.]))
plan_send.plan.mapValid = map_valid
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCommIssue = bool(radar_comm_issue)
plan_send.plan.processingDelay = (plan_send.logMonoTime /
1e9) - rcv_times['live20']
# Send out fcw
fcw = fcw and (self.fcw_enabled
or long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
# Interpolate 0.05 seconds and save as starting point for next iteration
dt = 0.05 # s
a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc -
self.a_acc_start)
v_acc_sol = self.v_acc_start + dt * (a_acc_sol +
self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
def update(self, sm, pm, CP, VM, PP, arne_sm):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
# we read offset value every 5 seconds
fixed_offset = 0.0
if not travis:
fixed_offset = op_params.get('speed_offset', 0.0)
if self.last_time > 5:
try:
self.offset = int(
self.params.get("SpeedLimitOffset", encoding='utf8'))
except (TypeError, ValueError):
self.params.delete("SpeedLimitOffset")
self.offset = 0
self.osm = self.params.get("LimitSetSpeed",
encoding='utf8') == "1"
self.last_time = 0
self.last_time = self.last_time + 1
gas_button_status = arne_sm['arne182Status'].gasbuttonstatus
v_ego = sm['carState'].vEgo
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
if blinkers:
steering_angle = sm['carState'].steeringAngle * 0.8
if v_ego < 11:
angle_later = 0.
else:
angle_later = arne_sm['latControl'].anglelater * 0.8
else:
steering_angle = sm['carState'].steeringAngle
if v_ego < 11:
angle_later = 0.
else:
angle_later = arne_sm['latControl'].anglelater
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = (lead_1.status and lead_1.dRel < 40.0
and lead_1.vRel < 0.0) or (lead_2.status
and lead_2.dRel < 40.0
and lead_2.vRel < 0.0)
if gas_button_status == 1:
speed_ahead_distance = 150
elif gas_button_status == 2:
speed_ahead_distance = 350
else:
speed_ahead_distance = default_brake_distance
v_speedlimit = NO_CURVATURE_SPEED
v_curvature_map = NO_CURVATURE_SPEED
v_speedlimit_ahead = NO_CURVATURE_SPEED
now = datetime.now()
try:
if sm['liveMapData'].speedLimitValid and osm and self.osm and (
sm['liveMapData'].lastGps.timestamp -
time.mktime(now.timetuple()) * 1000) < 10000 and (
smart_speed or smart_speed_max_vego > v_ego):
speed_limit = sm['liveMapData'].speedLimit
if speed_limit is not None:
v_speedlimit = speed_limit
# offset is in percentage,.
if v_ego > offset_limit:
v_speedlimit = v_speedlimit * (1. +
self.offset / 100.0)
if v_speedlimit > fixed_offset:
v_speedlimit = v_speedlimit + fixed_offset
else:
speed_limit = None
if sm['liveMapData'].speedLimitAheadValid and osm and self.osm and sm[
'liveMapData'].speedLimitAheadDistance < speed_ahead_distance and (
sm['liveMapData'].lastGps.timestamp -
time.mktime(now.timetuple()) * 1000) < 10000 and (
smart_speed or smart_speed_max_vego > v_ego):
distanceatlowlimit = 50
if sm['liveMapData'].speedLimitAhead < 21 / 3.6:
distanceatlowlimit = speed_ahead_distance = (
v_ego - sm['liveMapData'].speedLimitAhead) * 3.6 * 2
if distanceatlowlimit < 50:
distanceatlowlimit = 0
distanceatlowlimit = min(distanceatlowlimit, 100)
speed_ahead_distance = (
v_ego - sm['liveMapData'].speedLimitAhead) * 3.6 * 5
speed_ahead_distance = min(speed_ahead_distance, 300)
speed_ahead_distance = max(speed_ahead_distance, 50)
if speed_limit is not None and sm[
'liveMapData'].speedLimitAheadDistance > distanceatlowlimit and v_ego + 3 < sm[
'liveMapData'].speedLimitAhead + (
speed_limit - sm['liveMapData'].speedLimitAhead
) * sm[
'liveMapData'].speedLimitAheadDistance / speed_ahead_distance:
speed_limit_ahead = sm['liveMapData'].speedLimitAhead + (
speed_limit - sm['liveMapData'].speedLimitAhead) * (
sm['liveMapData'].speedLimitAheadDistance -
distanceatlowlimit) / (speed_ahead_distance -
distanceatlowlimit)
else:
speed_limit_ahead = sm['liveMapData'].speedLimitAhead
if speed_limit_ahead is not None:
v_speedlimit_ahead = speed_limit_ahead
if v_ego > offset_limit:
v_speedlimit_ahead = v_speedlimit_ahead * (
1. + self.offset / 100.0)
if v_speedlimit_ahead > fixed_offset:
v_speedlimit_ahead = v_speedlimit_ahead + fixed_offset
if sm['liveMapData'].curvatureValid and sm[
'liveMapData'].distToTurn < speed_ahead_distance and osm and self.osm and (
sm['liveMapData'].lastGps.timestamp -
time.mktime(now.timetuple()) * 1000) < 10000:
curvature = abs(sm['liveMapData'].curvature)
radius = 1 / max(1e-4, curvature) * curvature_factor
if gas_button_status == 1:
radius = radius * 2.0
elif gas_button_status == 2:
radius = radius * 1.0
else:
radius = radius * 1.5
if radius > 500:
c = 0.9 # 0.9 at 1000m = 108 kph
elif radius > 250:
c = 3.5 - 13 / 2500 * radius # 2.2 at 250m 84 kph
else:
c = 3.0 - 2 / 625 * radius # 3.0 at 15m 24 kph
v_curvature_map = math.sqrt(c * radius)
v_curvature_map = min(NO_CURVATURE_SPEED, v_curvature_map)
except KeyError:
pass
decel_for_turn = bool(v_curvature_map < min(
[v_cruise_setpoint, v_speedlimit, v_ego + 1.]))
# Calculate speed for normal cruise control
if enabled and not self.first_loop and not sm[
'carState'].brakePressed and not sm['carState'].gasPressed:
accel_limits = [
float(x) for x in calc_cruise_accel_limits(
v_ego, following and
(self.longitudinalPlanSource == 'mpc1' or
self.longitudinalPlanSource == 'mpc2'), gas_button_status)
]
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(v_ego, steering_angle,
accel_limits, self.CP,
angle_later)
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])
if decel_for_turn and sm[
'liveMapData'].distToTurn < speed_ahead_distance and not following:
time_to_turn = max(
1.0, sm['liveMapData'].distToTurn / max(
(v_ego + v_curvature_map) / 2, 1.))
required_decel = min(0,
(v_curvature_map - v_ego) / time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
if v_speedlimit_ahead < v_speedlimit and v_ego > v_speedlimit_ahead and sm[
'liveMapData'].speedLimitAheadDistance > 1.0 and not following:
required_decel = min(
0,
(v_speedlimit_ahead * v_speedlimit_ahead - v_ego * v_ego) /
(sm['liveMapData'].speedLimitAheadDistance * 2))
required_decel = max(required_decel, -3.0)
decel_for_turn = True
accel_limits[0] = required_decel
accel_limits[1] = required_decel
self.a_acc_start = required_decel
v_speedlimit_ahead = v_ego
v_cruise_setpoint = min([
v_cruise_setpoint, v_curvature_map, v_speedlimit,
v_speedlimit_ahead
])
#if (self.mpc1.prev_lead_status and self.mpc1.v_mpc < v_ego*0.99) or (self.mpc2.prev_lead_status and self.mpc2.v_mpc < v_ego*0.99):
# v_cruise_setpoint = v_ego
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc_model.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint)
self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint)
self.mpc_model.update(sm['carState'].vEgo, sm['carState'].vEgo,
sm['model'].longitudinal.distances,
sm['model'].longitudinal.speeds,
sm['model'].longitudinal.accelerations)
self.choose_solution(v_cruise_setpoint, enabled, lead_1, lead_2,
sm['carState'].steeringAngle)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, sm['controlsState'].active,
v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead,
lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw,
blinkers) and not sm['carState'].brakePressed
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
radar_dead = not sm.alive['radarState']
radar_errors = list(sm['radarState'].radarErrors)
radar_fault = car.RadarData.Error.fault in radar_errors
radar_can_error = car.RadarData.Error.canError in radar_errors
# **** send the plan ****
plan_send = messaging.new_message('plan')
plan_send.valid = sm.all_alive_and_valid(
service_list=['carState', 'controlsState', 'radarState'])
plan_send.plan.mdMonoTime = sm.logMonoTime['model']
plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
# longitudal plan
plan_send.plan.vCruise = float(self.v_cruise)
plan_send.plan.aCruise = float(self.a_cruise)
plan_send.plan.vStart = float(self.v_acc_start)
plan_send.plan.aStart = float(self.a_acc_start)
plan_send.plan.vTarget = float(self.v_acc)
plan_send.plan.aTarget = float(self.a_acc)
plan_send.plan.vTargetFuture = float(self.v_acc_future)
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
plan_send.plan.vCurvature = float(v_curvature_map)
plan_send.plan.decelForTurn = bool(decel_for_turn)
plan_send.plan.mapValid = True
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(radar_can_error)
plan_send.plan.processingDelay = (plan_send.logMonoTime /
1e9) - sm.rcv_time['radarState']
# Send out fcw
plan_send.plan.fcw = fcw
pm.send('plan', plan_send)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (
self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (
a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
self.first_loop = False
def update(self, sm, CP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
try:
input_event = self.input_queue.get_nowait()
if input_event == InputEvent.LONG_PRESS:
self.TR_override = 1.8 if self.TR_override is None else None
# Output current status
output_event = OutputEvent.LONG_DIM if self.TR_override is None else OutputEvent.SHORT_DIM
try:
self.output_queue.put_nowait(output_event)
except Full:
log_f = open('/data/openpilot-patch/mpc_update_log.txt', 'a')
log_f.write(f"{datetime.now()} Output queue is full\n")
log_f.flush()
subprocess.Popen(
['python', '/data/openpilot-patch/util/error.py'])
except TimeoutError:
pass
except Empty:
pass
v_ego = sm['carState'].vEgo
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
self.v_acc_start = self.v_acc_next
self.a_acc_start = self.a_acc_next
# Calculate speed for normal cruise control
if enabled and not self.first_loop and not sm['carState'].gasPressed:
accel_limits = [
float(x) for x in calc_cruise_accel_limits(v_ego, following)
]
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
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])
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = self.CP.minSpeedCan if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(sm['carState'], lead_1, self.TR_override)
self.mpc2.update(sm['carState'], lead_2, self.TR_override)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
self.fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, sm['controlsState'].active,
v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead,
lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw,
blinkers) and not sm['carState'].brakePressed
if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (
self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (
a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_next = v_acc_sol
self.a_acc_next = a_acc_sol
self.first_loop = False
def run_following_distance_simulation(TR_override, v_lead, t_end=200.0):
dt = 0.2
t = 0.
lead_pos = LeadPos(v_lead)
x_ego = 0.0
v_ego = v_lead
a_ego = 0.0
v_cruise_setpoint = v_lead + 10.
pm = FakePubMaster()
mpc = LongitudinalMpc(1, TR_override)
datapoints = [{'t': t, 'x_ego': x_ego, 'x_lead': lead_pos.x}]
first = True
while t < t_end:
# Run cruise control
accel_limits = [
float(x) for x in calc_cruise_accel_limits(v_ego, False)
]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint,
accel_limits[1], accel_limits[0],
jerk_limits[1], jerk_limits[0], dt)
# Setup CarState
CS = messaging.new_message('carState')
CS.carState.vEgo = v_ego
CS.carState.aEgo = a_ego
# Setup lead packet
lead = log.RadarState.LeadData.new_message()
lead.status = True
lead.dRel = lead_pos.x - x_ego
lead.vLead = lead_pos.v
lead.aLeadK = 0.0
# Run MPC
mpc.set_cur_state(v_ego, a_ego)
if first: # Make sure MPC is converged on first timestep
for _ in range(20):
mpc.update(CS.carState, lead)
mpc.publish(pm)
mpc.update(CS.carState, lead)
mpc.publish(pm)
# Choose slowest of two solutions
if v_cruise < mpc.v_mpc:
v_ego, a_ego = v_cruise, a_cruise
else:
v_ego, a_ego = mpc.v_mpc, mpc.a_mpc
# Update state
lead_pos.update(t, dt)
x_ego += v_ego * dt
t += dt
first = False
datapoints.append({'t': t, 'x_ego': x_ego, 'x_lead': lead_pos.x})
filename = f'test_out/{v_lead}_{TR_override}.json'
with open(filename, 'w') as datafile:
json.dump(datapoints, datafile)
return lead_pos.x - x_ego
