Skip to content

Commit

Permalink
Bring in kegman-0.5.12 long control (commaai#174)
Browse files Browse the repository at this point in the history
  • Loading branch information
kegman authored Jun 6, 2019
1 parent 063382f commit dba5450
Showing 1 changed file with 48 additions and 77 deletions.
125 changes: 48 additions & 77 deletions selfdrive/controls/lib/planner.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,22 +24,24 @@
ONE_BAR_DISTANCE = 0.9 # in seconds
TWO_BAR_DISTANCE = 1.3 # in seconds
THREE_BAR_DISTANCE = 1.8 # in seconds
FOUR_BAR_DISTANCE = 2.5 # in seconds
FOUR_BAR_DISTANCE = 2.3 # in seconds

# Variables that change braking profiles
TR = TWO_BAR_DISTANCE # default interval

# Variables that change braking profiles
CITY_SPEED = 19.44 # braking profile changes when below this speed based on following dynamics below [m/s]
GAP_CLOSURE_SPEED = -1 # relative velocity between you and lead car which activates braking profile change [m/s]
RAPID_GAP_CLOSURE_SPEED = -3 # relative velocity between you and lead car which activates a broking profile change + RAPID_DELTA [m/s]
RAPID_DELTA = 0.15 # increased braking profile for approaching lead car at RAPID_GAP_CLOSURE_SPEED [s]
TAILGATE_DISTANCE = 17.5 # when below this distance between you and lead car, braking profile change is active based on PULLAWAY_REL_V [m]
PULLAWAY_REL_V = 0.25 # within TAILGATE_DISTANCE, if the car is pulling away w/ rel velocity that exceeds this value, then change BACK to set bar distance [m/s]
MIN_DISTANCE = 7 # keep a minimum distance between you and lead car (when below this, activates braking profile change) [m]
STOPPING_DISTANCE = 2 # increase distance from lead car when stopped

# Braking profile changes (makes the car brake harder because it wants to be farther from the lead car - increase to brake harder)
BRAKING_ONE_BAR_DISTANCE = 2.3 # more aggressive braking when using one bar distance by increasing follow distance [s]
BRAKING_TWO_BAR_DISTANCE = 2.2 # more aggressive braking when using two bar distance by increasing follow distance [s]
BRAKING_THREE_BAR_DISTANCE = 2.1 # no change in braking profile
ONE_BAR_PROFILE = [ONE_BAR_DISTANCE, 2.5]
ONE_BAR_PROFILE_BP = [0.0, 3.0]

TWO_BAR_PROFILE = [TWO_BAR_DISTANCE, 2.5]
TWO_BAR_PROFILE_BP = [0.0, 3.0]

THREE_BAR_PROFILE = [THREE_BAR_DISTANCE, 2.5]
THREE_BAR_PROFILE_BP = [0.0, 4.0]


# Max lateral acceleration, used to caclulate how much to slow down in turns
A_Y_MAX = 1.85 # m/s^2
Expand Down Expand Up @@ -179,10 +181,8 @@ def __init__(self, mpc_id, live_longitudinal_mpc):
self.lastTR = 2
self.last_cloudlog_t = 0.0
self.v_rel = 10
self.tailgating = 0
self.street_speed = 0
self.lead_car_gap_shrinking = 0
self.lead_car_rapid_gap_shrinking = 0


def send_mpc_solution(self, qp_iterations, calculation_time):
qp_iterations = max(0, qp_iterations)
Expand Down Expand Up @@ -264,78 +264,49 @@ def update(self, CS, lead, v_cruise_setpoint):
else:
self.street_speed = 0

# Is the gap from the lead car shrinking?
if self.v_rel < GAP_CLOSURE_SPEED:
self.lead_car_gap_shrinking = 1
else:
self.lead_car_gap_shrinking = 0

# Is the car tailgating the lead car?
if x_lead < MIN_DISTANCE or (x_lead < TAILGATE_DISTANCE and self.v_rel < PULLAWAY_REL_V):
self.tailgating = 1
else:
self.tailgating = 0

# Is the lead car gap closing fast?
if self.v_rel < RAPID_GAP_CLOSURE_SPEED:
self.lead_car_rapid_gap_shrinking = 1
else:
self.lead_car_rapid_gap_shrinking = 0


# Calculate mpc
# Adjust distance from lead car when distance button pressed
if CS.readdistancelines == 1:
if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
TR = BRAKING_ONE_BAR_DISTANCE
if self.lead_car_rapid_gap_shrinking:
TR = TR + RAPID_DELTA # add more braking if lead car is coming in fast
if self.lastTR != -CS.readdistancelines:
self.libmpc.init(MPC_COST_LONG.TTC, 0.0850, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = -CS.readdistancelines
if CS.carState.readdistancelines == 1:
#if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
if self.street_speed:
TR = interp(-self.v_rel, ONE_BAR_PROFILE_BP, ONE_BAR_PROFILE)
else:
TR = ONE_BAR_DISTANCE
if CS.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, 1.0, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.readdistancelines

elif CS.readdistancelines == 2:
if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
TR = BRAKING_TWO_BAR_DISTANCE
if self.lead_car_rapid_gap_shrinking:
TR = TR + RAPID_DELTA # add more braking if lead car is coming in fast
if self.lastTR != -CS.readdistancelines:
self.libmpc.init(MPC_COST_LONG.TTC, 0.0875, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = -CS.readdistancelines
if CS.carState.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, 1.0, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.carState.readdistancelines

elif CS.carState.readdistancelines == 2:
#if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
if self.street_speed:
TR = interp(-self.v_rel, TWO_BAR_PROFILE_BP, TWO_BAR_PROFILE)
else:
TR = TWO_BAR_DISTANCE
if CS.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.readdistancelines

elif CS.readdistancelines == 3:
if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
TR = BRAKING_THREE_BAR_DISTANCE
if self.lead_car_rapid_gap_shrinking:
TR = TR + RAPID_DELTA # add more braking if lead car is coming in fast
if self.lastTR != -CS.readdistancelines:
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = -CS.readdistancelines
if CS.carState.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.carState.readdistancelines

elif CS.carState.readdistancelines == 3:
if self.street_speed:
#if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
TR = interp(-self.v_rel, THREE_BAR_PROFILE_BP, THREE_BAR_PROFILE)
else:
TR = THREE_BAR_DISTANCE
if CS.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.readdistancelines
elif CS.readdistancelines == 4:
if CS.carState.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.carState.readdistancelines

elif CS.carState.readdistancelines == 4:
TR = FOUR_BAR_DISTANCE
if CS.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, 0.05, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.readdistancelines
if CS.carState.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.lastTR = CS.carState.readdistancelines

else:
TR = TWO_BAR_DISTANCE # if readdistancelines != 1,2,3,4
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)

TR = TWO_BAR_DISTANCE # if readdistancelines != 1,2,3,4
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)


n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, TR)
duration = int((sec_since_boot() - t) * 1e9)
self.send_mpc_solution(n_its, duration)
Expand Down

0 comments on commit dba5450

Please sign in to comment.