def __init__(self, CP):
self.CP = CP
self.poller = zmq.Poller()
self.arne182Status = messaging.sub_sock(
service_list['arne182Status'].port,
poller=self.poller,
conflate=True)
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
def __init__(self, CP):
self.CP = CP
self.op_params = opParams()
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.osm = True
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
self.first_loop = True
self.offset = 0
self.last_time = 0
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
self.first_loop = True
# dp
self.dp_profile = DP_OFF
# dp - slow on curve from 0.7.6.1
self.dp_slow_on_curve = False
self.v_model = 0.0
self.a_model = 0.0
def __init__(self, CP):
self.CP = CP
self.op_params = opParams()
self.slowdown_for_curves = self.op_params.get('slowdown_for_curves')
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.mpc_model = LongitudinalMpcModel()
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
self.first_loop = True
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc_next = 0.0
self.a_acc_next = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.source = Source.cruiseCoast
self.cruise_source = Source.cruiseCoast
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.fcw = False
self.params = Params()
self.kegman = kegman_conf()
self.mpc_frame = 0
self.first_loop = True
self.coast_enabled = True
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc_next = 0.0
self.a_acc_next = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.fcw = False
self.params = Params()
self.first_loop = True
def __init__(self, CP, fcw_enabled):
self.CP = CP
self.plan = messaging.pub_sock(service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(service_list['liveLongitudinalMpc'].port)
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
self.model_v_kf = KF1D([[0.0],[0.0]], _MODEL_V_A, _MODEL_V_C, _MODEL_V_K)
self.model_v_kf_ready = False
self.params = Params()
def __init__(self, CP, fcw_enabled):
context = zmq.Context()
self.CP = CP
self.poller = zmq.Poller()
self.lat_Control = messaging.sub_sock(context,
service_list['latControl'].port,
conflate=True,
poller=self.poller)
self.plan = messaging.pub_sock(context, service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(
context, service_list['liveLongitudinalMpc'].port)
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
self.lastlat_Control = None
self.params = Params()
def __init__(self, CP):
self.CP = CP
self.op_params = opParams()
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.mpc_model = LongitudinalMpcModel()
self.model_mpc_helper = ModelMpcHelper()
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc_next = 0.0
self.a_acc_next = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.solution = Solution(0., 0.)
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.fcw = False
self.params = Params()
self.first_loop = True
def __init__(self, CP):
self.CP = CP
self.mpcs = {}
self.mpcs['lead0'] = LeadMpc(0)
self.mpcs['lead1'] = LeadMpc(1)
self.mpcs['cruise'] = LongitudinalMpc()
self.fcw = False
self.fcw_checker = FCWChecker()
self.v_desired = 0.0
self.a_desired = 0.0
self.longitudinalPlanSource = 'cruise'
self.alpha = np.exp(-DT_MDL / 2.0)
self.lead_0 = log.ModelDataV2.LeadDataV3.new_message()
self.lead_1 = log.ModelDataV2.LeadDataV3.new_message()
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.params = Params()
self.target_speed_map = 0
self.target_speed_map_dist = 0
self.target_speed_map_dist_prev = 0
self.target_speed_map_block = False
self.target_speed_map_sign = False
self.map_sign = 0
self.vego = 0
self.second = 0
self.map_enabled = False
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
if not travis:
pm = messaging.PubMaster(['smiskolData']) # why is this here?
self.mpc1.set_pm(pm)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
self.first_loop = True
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
self.first_loop = True
# dp
self.dragon_slow_on_curve = True
self.dragon_alt_accel_profile = False
self.dragon_fast_accel = False
self.dragon_accel_profile = AP_OFF
self.last_ts = 0.
self.dp_last_modified = None
def __init__(self, CP):
self.CP = CP
self.mpcs = {}
self.mpcs['lead0'] = LeadMpc(0)
self.mpcs['lead1'] = LeadMpc(1)
self.mpcs['cruise'] = LongitudinalMpc()
self.fcw = False
self.fcw_checker = FCWChecker()
self.v_desired = 0.0
self.a_desired = 0.0
self.longitudinalPlanSource = 'cruise'
self.alpha = np.exp(-DT_MDL / 2.0)
self.lead_0 = log.ModelDataV2.LeadDataV3.new_message()
self.lead_1 = log.ModelDataV2.LeadDataV3.new_message()
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
# dp
self.dp_accel_profile_ctrl = False
self.dp_accel_profile = DP_ACCEL_ECO
self.dp_following_profile_ctrl = False
self.dp_following_profile = 3
self.dp_following_dist = 1.8 # default val
def __init__(self, CP, fcw_enabled):
self.CP = CP
self.plan = messaging.pub_sock(service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(
service_list['liveLongitudinalMpc'].port)
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
self.path_x = np.arange(192)
self.params = Params()
def __init__(self, CP):
self.CP = CP
self.mpcs = {}
self.mpcs['lead0'] = LeadMpc(0)
self.mpcs['lead1'] = LeadMpc(1)
self.mpcs['cruise'] = LongitudinalMpc()
self.mpcs['custom'] = LimitsLongitudinalMpc()
self.fcw = False
self.fcw_checker = FCWChecker()
self.v_desired = 0.0
self.a_desired = 0.0
self.longitudinalPlanSource = 'cruise'
self.alpha = np.exp(-CP.radarTimeStep/2.0)
self.lead_0 = log.ModelDataV2.LeadDataV3.new_message()
self.lead_1 = log.ModelDataV2.LeadDataV3.new_message()
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.vision_turn_controller = VisionTurnController(CP)
self.speed_limit_controller = SpeedLimitController()
self.events = Events()
self.turn_speed_controller = TurnSpeedController()
self._params = Params()
self.params_check_last_t = 0.
self.params_check_freq = 0.1 # check params at 10Hz
self.accel_mode = int(self._params.get("AccelMode", encoding="utf8")) # 0 = normal, 1 = sport; 2 = eco; 3 = creep
self.coasting_lead_d = -1. # [m] lead distance. -1. if no lead
self.coasting_lead_v = -10. # lead "absolute"" velocity
self.tr = 1.8
self.sessionInitTime = sec_since_boot()
self.debug_logging = False
self.debug_log_time_step = 0.333
self.last_debug_log_t = 0.
self.debug_log_path = "/data/openpilot/long_debug.csv"
if self.debug_logging:
with open(self.debug_log_path,"w") as f:
f.write(",".join([
"t",
"vEgo",
"vEgo (mph)",
"a lim low",
"a lim high",
"out a",
"out long plan"]) + "\n")
def __init__(self, CP):
self.CP = CP
self.mpcs = {}
self.mpcs['lead0'] = LeadMpc(0)
self.mpcs['lead1'] = LeadMpc(1)
self.mpcs['cruise'] = LongitudinalMpc()
self.fcw = False
self.fcw_checker = FCWChecker()
self.v_desired = 0.0
self.a_desired = 0.0
self.longitudinalPlanSource = 'cruise'
self.alpha = np.exp(-DT_MDL / 2.0)
self.lead_0 = log.ModelDataV2.LeadDataV3.new_message()
self.lead_1 = log.ModelDataV2.LeadDataV3.new_message()
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.mpc_model = LongitudinalMpcModel()
self.model_mpc_helper = ModelMpcHelper()
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc_next = 0.0
self.a_acc_next = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.fcw = False
self.params = Params()
self.first_loop = True
self.target_speed_map = 0.0
self.target_speed_map_counter = 0
self.target_speed_map_counter_check = False
self.target_speed_map_counter1 = 0
self.target_speed_map_counter2 = 0
self.target_speed_map_counter3 = 0
self.target_speed_map_dist = 0
self.target_speed_map_block = False
self.target_speed_map_sign = False
self.tartget_speed_offset = int(
self.params.get("OpkrSpeedLimitOffset", encoding="utf8"))
self.vego = 0
def __init__(self, CP):
self.CP = CP
self.mpcs = {}
self.mpcs['lead0'] = LeadMpc(0)
self.mpcs['lead1'] = LeadMpc(1)
self.mpcs['cruise'] = LongitudinalMpc()
self.fcw = False
self.fcw_checker = FCWChecker()
self.v_desired = 0.0
self.a_desired = 0.0
self.longitudinalPlanSource = 'cruise'
self.alpha = np.exp(-DT_MDL / 2.0)
self.lead_0 = log.ModelDataV2.LeadDataV3.new_message()
self.lead_1 = log.ModelDataV2.LeadDataV3.new_message()
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.use_cluster_speed = Params().get_bool('UseClusterSpeed')
self.long_control_enabled = Params().get_bool('LongControlEnabled')
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
self.kegman = kegman_conf()
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc_next = 0.0
self.a_acc_next = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.fcw = False
self.params = Params()
self.first_loop = True
self.input_queue = Queue()
self.input_thread = Thread(target=input_loop,
args=(self.input_queue, ))
self.input_thread.start()
self.output_queue = Queue()
self.output_thread = Thread(target=output_loop,
args=(self.output_queue, ))
self.output_thread.start()
self.TR_override = None
class Planner():
def __init__(self, CP):
self.CP = CP
self.op_params = opParams()
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.osm = True
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
self.first_loop = True
self.offset = 0
self.last_time = 0
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
solutions = {'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
slowest = min(solutions, key=solutions.get)
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = min([
self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint
])
def update(self, sm, pm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
# we read offset value every 5 seconds
fixed_offset = 0.0
fixed_offset = op_params.get('speed_offset')
if self.last_time > 5:
try:
#self.offset = int(self.params.get("SpeedLimitOffset", encoding='utf8'))
self.offset = 0
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
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
speed_ahead_distance = default_brake_distance
v_speedlimit = NO_CURVATURE_SPEED
v_curvature_map = NO_CURVATURE_SPEED
v_speedlimit_ahead = 0
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 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 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)
]
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 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
if (v_ego * 3.6) <= 50:
if sm['liveMapData'].speedLimitAhead and sm[
'liveMapData'].speedLimitAheadDistance < (v_ego * 3.6 *
2.5):
v_speedlimit_ahead = sm['liveMapData'].speedLimitAhead
elif (v_ego * 3.6) <= 70:
if sm['liveMapData'].speedLimitAhead and sm[
'liveMapData'].speedLimitAheadDistance < (v_ego * 3.6 *
3.5):
v_speedlimit_ahead = sm['liveMapData'].speedLimitAhead
elif (v_ego * 3.6) > 70:
if sm['liveMapData'].speedLimitAhead and sm[
'liveMapData'].speedLimitAheadDistance < (v_ego * 3.6 *
4.5):
v_speedlimit_ahead = sm['liveMapData'].speedLimitAhead
#v_cruise_setpoint = min([v_cruise_setpoint, v_curvature_map, v_speedlimit, v_speedlimit_ahead])
v_cruise_setpoint = min(
[v_cruise_setpoint, v_curvature_map, v_speedlimit])
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
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
# Send radarstate(dRel, vRel, yRel)
plan_send.plan.dRel1 = lead_1.dRel
plan_send.plan.yRel1 = lead_1.yRel
plan_send.plan.vRel1 = lead_1.vRel
plan_send.plan.dRel2 = lead_2.dRel
plan_send.plan.yRel2 = lead_2.yRel
plan_send.plan.vRel2 = lead_2.vRel
plan_send.plan.status2 = lead_2.status
plan_send.plan.targetSpeed = v_cruise_setpoint * CV.MS_TO_KPH
plan_send.plan.targetSpeedCamera = v_speedlimit_ahead * CV.MS_TO_KPH
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
class Planner(object):
def __init__(self, CP, fcw_enabled):
self.CP = CP
self.plan = messaging.pub_sock(service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(service_list['liveLongitudinalMpc'].port)
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
self.model_v_kf = KF1D([[0.0],[0.0]], _MODEL_V_A, _MODEL_V_C, _MODEL_V_K)
self.model_v_kf_ready = False
self.params = Params()
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
solutions = {'cruise': self.v_cruise, 'model': self.v_model}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
slowest = min(solutions, key=solutions.get)
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
elif slowest == 'model':
self.v_acc = self.v_model
self.a_acc = self.a_model
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
def update(self, sm, 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_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
if not self.model_v_kf_ready:
self.model_v_kf.x = [[v_ego],[0.0]]
self.model_v_kf_ready = True
if len(sm['model'].speed):
self.model_v_kf.update(sm['model'].speed[SPEED_PERCENTILE_IDX])
if self.params.get("LimitSetSpeedNeural") == "1":
model_speed = self.model_v_kf.x[0][0]
else:
model_speed = MAX_SPEED
# 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_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)
# accel and jerk up limits are higher here to make model not limiting accel
# mainly done to prevent flickering of slowdown icon
self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start,
model_speed,
2*accel_limits[1], 3*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(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,
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 = 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
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
class Planner(object):
def __init__(self, CP, fcw_enabled):
self.CP = CP
self.poller = zmq.Poller()
self.plan = messaging.pub_sock(service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(
service_list['liveLongitudinalMpc'].port)
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
self.params = Params()
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
solutions = {'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
slowest = min(solutions, key=solutions.get)
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = min([
self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint
])
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
class Planner():
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc_next = 0.0
self.a_acc_next = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.source = Source.cruiseCoast
self.cruise_source = Source.cruiseCoast
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.fcw = False
self.params = Params()
self.kegman = kegman_conf()
self.mpc_frame = 0
self.first_loop = True
self.coast_enabled = True
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
solutions = {self.cruise_source: self.v_cruise}
if self.mpc1.prev_lead_status:
solutions[Source.mpc1] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions[Source.mpc2] = self.mpc2.v_mpc
slowest = min(solutions, key=solutions.get)
self.source = slowest
# Choose lowest of MPC and cruise
if slowest == Source.mpc1:
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == Source.mpc2:
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == self.cruise_source:
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = min([
self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint
])
def choose_cruise(self, v_ego, a_ego, v_cruise_setpoint,
accel_limits_turns, jerk_limits, gasbrake):
# WARNING: Logic is carefully verified. On change, review test_longitudinal.py output!
# Standard cruise
if not self.coast_enabled:
self.cruise_source = Source.cruiseGas
return 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)
# If coasting, reset starting state for gas and brake plans
if self.source == Source.cruiseCoast:
self.v_acc_start = v_ego
self.a_acc_start = a_ego
elif self.source in [Source.mpc1, Source.mpc2]:
self.cruise_source = Source.cruiseGas if gasbrake >= 0 else Source.cruiseBrake
# Coast to (current state)
v_coast, a_coast = v_ego, a_ego
# Gas to (v_cruise_setpoint)
v_gas, a_gas = 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)
# Brake to (v_cruise_setpoint + COAST_SPEED)
v_brake, a_brake = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint + COAST_SPEED,
accel_limits_turns[1],
accel_limits_turns[0],
jerk_limits[1], jerk_limits[0],
LON_MPC_STEP)
cruise = {
Source.cruiseCoast: (v_coast, a_coast),
Source.cruiseGas: (v_gas, a_gas),
Source.cruiseBrake: (v_brake, a_brake),
}
# Entry conditions
if gasbrake == 0:
if a_brake <= a_coast:
self.cruise_source = Source.cruiseBrake
elif a_gas >= a_coast:
self.cruise_source = Source.cruiseGas
elif (a_brake >= a_coast >= a_gas):
self.cruise_source = Source.cruiseCoast
return cruise[self.cruise_source]
def update(self, sm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
a_ego = sm['carState'].aEgo
gasbrake = sm['carControl'].actuators.gas - sm[
'carControl'].actuators.brake
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 in [
LongCtrlState.pid, LongCtrlState.stopping
]
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
if self.mpc_frame % 1000 == 0:
self.kegman = kegman_conf()
self.mpc_frame = 0
self.mpc_frame += 1
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, self.kegman.conf['accelerationMode'])
]
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 = accel_limits # limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits_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 = self.choose_cruise(
v_ego, a_ego, v_cruise_setpoint, accel_limits_turns,
jerk_limits, gasbrake)
# 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(a_ego, 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.cruise_source = Source.cruiseCoast
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 publish(self, sm, pm):
self.mpc1.publish(pm)
self.mpc2.publish(pm)
plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_alive_and_valid(
service_list=['carState', 'controlsState', 'radarState'])
longitudinalPlan = plan_send.longitudinalPlan
longitudinalPlan.mdMonoTime = sm.logMonoTime['modelV2']
longitudinalPlan.radarStateMonoTime = sm.logMonoTime['radarState']
longitudinalPlan.vCruise = float(self.v_cruise)
longitudinalPlan.aCruise = float(self.a_cruise)
longitudinalPlan.vStart = float(self.v_acc_start)
longitudinalPlan.aStart = float(self.a_acc_start)
longitudinalPlan.vTarget = float(self.v_acc)
longitudinalPlan.aTarget = float(self.a_acc)
longitudinalPlan.vTargetFuture = float(self.v_acc_future)
longitudinalPlan.hasLead = self.mpc1.prev_lead_status
longitudinalPlan.longitudinalPlanSource = self.source
longitudinalPlan.fcw = self.fcw
longitudinalPlan.processingDelay = (plan_send.logMonoTime /
1e9) - sm.rcv_time['radarState']
pm.send('longitudinalPlan', plan_send)
class Planner():
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
self.kegman = kegman_conf()
self.mpc_frame = 0
self.first_loop = True
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
solutions = {'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
slowest = min(solutions, key=solutions.get)
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = min([
self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint
])
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
if self.mpc_frame % 1000 == 0:
self.kegman = kegman_conf()
self.mpc_frame = 0
self.mpc_frame += 1
# 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, self.kegman.conf['accelerationMode'])
]
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 = 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
class Planner(object):
def __init__(self, CP, fcw_enabled):
context = zmq.Context()
self.CP = CP
self.poller = zmq.Poller()
self.lat_Control = messaging.sub_sock(context,
service_list['latControl'].port,
conflate=True,
poller=self.poller)
self.plan = messaging.pub_sock(context, service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(
context, service_list['liveLongitudinalMpc'].port)
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
self.lastlat_Control = None
self.params = Params()
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
solutions = {'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
slowest = min(solutions, key=solutions.get)
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = min([
self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint
])
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
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
self.lead_1 = live20.live20.leadOne
self.lead_2 = live20.live20.leadTwo
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
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"))
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)) / 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])
# 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] * 0.5),
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] * 0.5),
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])
# 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)
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.SOFT_DISABLE]))
radar_errors = list(live20.live20.radarErrors)
if 'commIssue' in radar_errors:
events.append(
create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if 'fault' in 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.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 = decel_for_turn
plan_send.plan.mapValid = map_valid
# 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
class Planner():
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.mpc_model = LongitudinalMpcModel()
self.model_mpc_helper = ModelMpcHelper()
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc_next = 0.0
self.a_acc_next = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.fcw = False
self.params = Params()
self.first_loop = True
self.target_speed_map = 0.0
self.target_speed_map_counter = 0
self.target_speed_map_counter_check = False
self.target_speed_map_counter1 = 0
self.target_speed_map_counter2 = 0
self.target_speed_map_counter3 = 0
self.target_speed_map_dist = 0
self.target_speed_map_block = False
self.target_speed_map_sign = False
self.tartget_speed_offset = int(self.params.get("OpkrSpeedLimitOffset", encoding="utf8"))
self.vego = 0
def choose_solution(self, v_cruise_setpoint, enabled, model_enabled):
possible_futures = [self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]
if enabled:
solutions = {'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
if self.mpc_model.valid and model_enabled:
solutions['model'] = self.mpc_model.v_mpc
possible_futures.append(self.mpc_model.v_mpc_future) # only used when using model
slowest = min(solutions, key=solutions.get)
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
elif slowest == 'model':
self.v_acc = self.mpc_model.v_mpc
self.a_acc = self.mpc_model.a_mpc
self.v_acc_future = min(possible_futures)
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
if self.params.get_bool("OpenpilotLongitudinalControl"):
v_cruise_kph = sm['carState'].vSetDis
else:
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
if self.params.get_bool("OpkrMapEnable"):
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("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
# 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 and False: # awareness decel is disabled for now
# 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.mpc_model.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)
distances, speeds, accelerations = self.model_mpc_helper.convert_data(sm)
self.mpc_model.update(sm['carState'].vEgo, sm['carState'].aEgo,
distances,
speeds,
accelerations)
self.choose_solution(v_cruise_setpoint, enabled, self.params.get_bool("ModelLongEnabled"))
# 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 publish(self, sm, pm):
self.mpc1.publish(pm)
self.mpc2.publish(pm)
plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState'])
longitudinalPlan = plan_send.longitudinalPlan
longitudinalPlan.mdMonoTime = sm.logMonoTime['modelV2']
longitudinalPlan.radarStateMonoTime = sm.logMonoTime['radarState']
longitudinalPlan.vCruise = float(self.v_cruise)
longitudinalPlan.aCruise = float(self.a_cruise)
longitudinalPlan.vStart = float(self.v_acc_start)
longitudinalPlan.aStart = float(self.a_acc_start)
longitudinalPlan.vTarget = float(self.v_acc)
longitudinalPlan.aTarget = float(self.a_acc)
longitudinalPlan.vTargetFuture = float(self.v_acc_future)
longitudinalPlan.hasLead = self.mpc1.prev_lead_status
longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource
longitudinalPlan.fcw = self.fcw
longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState']
# Send radarstate(dRel, vRel, yRel)
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
longitudinalPlan.dRel1 = float(lead_1.dRel)
longitudinalPlan.yRel1 = float(lead_1.yRel)
longitudinalPlan.vRel1 = float(lead_1.vRel)
longitudinalPlan.dRel2 = float(lead_2.dRel)
longitudinalPlan.yRel2 = float(lead_2.yRel)
longitudinalPlan.vRel2 = float(lead_2.vRel)
longitudinalPlan.status2 = bool(lead_2.status)
cam_distance_calc = 0
cam_distance_calc = interp(self.vego*CV.MS_TO_KPH, [30,60,100,160], [3.75,5.5,6,7])
consider_speed = interp((self.vego*CV.MS_TO_KPH - self.target_speed_map), [10, 30], [1, 1.3])
if self.target_speed_map > 29 and self.target_speed_map_dist < cam_distance_calc*consider_speed*self.vego*CV.MS_TO_KPH:
longitudinalPlan.targetSpeedCamera = float(self.target_speed_map)
longitudinalPlan.targetSpeedCameraDist = float(self.target_speed_map_dist)
self.target_speed_map_sign = True
elif self.target_speed_map > 29 and self.target_speed_map_dist >= cam_distance_calc*consider_speed*self.vego*CV.MS_TO_KPH and self.target_speed_map_block:
longitudinalPlan.targetSpeedCamera = float(self.target_speed_map)
longitudinalPlan.targetSpeedCameraDist = float(self.target_speed_map_dist)
self.target_speed_map_sign = True
elif self.target_speed_map > 29 and self.target_speed_map_sign:
longitudinalPlan.targetSpeedCamera = float(self.target_speed_map)
longitudinalPlan.targetSpeedCameraDist = float(self.target_speed_map_dist)
else:
longitudinalPlan.targetSpeedCamera = 0
longitudinalPlan.targetSpeedCameraDist = 0
pm.send('longitudinalPlan', plan_send)
class Planner():
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
self.kegman = KegmanConf()
self.mpc_frame = 0
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
solutions = {'model': self.v_model, 'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
slowest = min(solutions, key=solutions.get)
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
elif slowest == 'model':
self.v_acc = self.v_model
self.a_acc = self.a_model
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
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_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)
if self.mpc_frame % 1000 == 0:
self.kegman = KegmanConf()
self.mpc_frame = 0
self.mpc_frame += 1
if len(sm['model'].path.poly) and int(self.kegman.conf['slowOnCurves']):
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
if enabled:
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)]
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)
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
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
class Planner():
def __init__(self, CP):
self.CP = CP
self.poller = zmq.Poller()
self.arne182Status = messaging.sub_sock(
service_list['arne182Status'].port,
poller=self.poller,
conflate=True)
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
solutions = {'model': self.v_model, 'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
slowest = min(solutions, key=solutions.get)
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
elif slowest == 'model':
self.v_acc = self.v_model
self.a_acc = self.a_model
self.v_acc_future = min([
self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint
])
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
class Planner():
def __init__(self, CP):
self.CP = CP
self.mpcs = {}
self.mpcs['lead0'] = LeadMpc(0)
self.mpcs['lead1'] = LeadMpc(1)
self.mpcs['cruise'] = LongitudinalMpc()
self.fcw = False
self.fcw_checker = FCWChecker()
self.v_desired = 0.0
self.a_desired = 0.0
self.longitudinalPlanSource = 'cruise'
self.alpha = np.exp(-DT_MDL / 2.0)
self.lead_0 = log.ModelDataV2.LeadDataV3.new_message()
self.lead_1 = log.ModelDataV2.LeadDataV3.new_message()
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
# dp
self.dp_accel_profile_ctrl = False
self.dp_accel_profile = DP_ACCEL_ECO
self.dp_following_profile_ctrl = False
self.dp_following_profile = 3
self.dp_following_dist = 1.8 # default val
def update(self, sm, CP):
# dp
self.dp_accel_profile_ctrl = sm['dragonConf'].dpAccelProfileCtrl
self.dp_accel_profile = sm['dragonConf'].dpAccelProfile
self.dp_following_profile_ctrl = sm['dragonConf'].dpAccelProfileCtrl
self.dp_following_profile = sm['dragonConf'].dpFollowingProfile
self.dp_following_dist = DP_FOLLOWING_DIST[
0 if not self.dp_following_profile_ctrl else self.
dp_following_profile]
self.mpcs['lead0'].set_following_distance(self.dp_following_dist)
self.mpcs['lead1'].set_following_distance(self.dp_following_dist)
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
a_ego = sm['carState'].aEgo
v_cruise_kph = sm['controlsState'].vCruise
v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
v_cruise = v_cruise_kph * CV.KPH_TO_MS
long_control_state = sm['controlsState'].longControlState
force_slow_decel = sm['controlsState'].forceDecel
self.lead_0 = sm['radarState'].leadOne
self.lead_1 = sm['radarState'].leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
if not enabled or sm['carState'].gasPressed:
self.v_desired = v_ego
self.a_desired = a_ego
# Prevent divergence, smooth in current v_ego
self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * v_ego
self.v_desired = max(0.0, self.v_desired)
if not self.dp_accel_profile_ctrl:
accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
else:
accel_limits = dp_calc_cruise_accel_limits(v_cruise,
self.dp_accel_profile)
accel_limits_turns = limit_accel_in_turns(
v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0],
accel_limits_turns[1])
# clip limits, cannot init MPC outside of bounds
accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired)
accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired)
self.mpcs['cruise'].set_accel_limits(accel_limits_turns[0],
accel_limits_turns[1])
next_a = np.inf
for key in self.mpcs:
self.mpcs[key].set_cur_state(self.v_desired, self.a_desired)
self.mpcs[key].update(sm['carState'], sm['radarState'], v_cruise)
if self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a:
self.longitudinalPlanSource = key
self.v_desired_trajectory = self.mpcs[
key].v_solution[:CONTROL_N]
self.a_desired_trajectory = self.mpcs[
key].a_solution[:CONTROL_N]
self.j_desired_trajectory = self.mpcs[
key].j_solution[:CONTROL_N]
next_a = self.mpcs[key].a_solution[5]
# determine fcw
if self.mpcs['lead0'].new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
self.fcw = self.fcw_checker.update(
self.mpcs['lead0'].mpc_solution, cur_time,
sm['controlsState'].active, v_ego, sm['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 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_prev = self.a_desired
self.a_desired = float(
interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory))
self.v_desired = self.v_desired + DT_MDL * (self.a_desired +
a_prev) / 2.0
def publish(self, sm, pm):
plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_alive_and_valid(
service_list=['carState', 'controlsState'])
longitudinalPlan = plan_send.longitudinalPlan
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
longitudinalPlan.processingDelay = (plan_send.logMonoTime /
1e9) - sm.logMonoTime['modelV2']
longitudinalPlan.speeds = [float(x) for x in self.v_desired_trajectory]
longitudinalPlan.accels = [float(x) for x in self.a_desired_trajectory]
longitudinalPlan.jerks = [float(x) for x in self.j_desired_trajectory]
longitudinalPlan.hasLead = self.mpcs['lead0'].status
longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource
longitudinalPlan.fcw = self.fcw
pm.send('longitudinalPlan', plan_send)
