def run_following_distance_simulation(v_lead, t_end=200.0):
dt = 0.2
t = 0.
x_lead = 200.0
x_ego = 0.0
v_ego = v_lead
a_ego = 0.0
v_cruise_setpoint = v_lead + 10.
pm = FakePubMaster()
mpc = LongitudinalMpc(1)
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 = x_lead - x_ego
lead.vLead = v_lead
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
x_lead += v_lead * dt
x_ego += v_ego * dt
t += dt
first = False
return x_lead - x_ego
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
