mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-08-23 09:13:47 +08:00
dragonpilot 0.7.10
This commit is contained in:
Rick Lan
@@ -44,6 +44,8 @@ class Controls:
|
||||
def __init__(self, sm=None, pm=None, can_sock=None):
|
||||
config_realtime_process(3, Priority.CTRL_HIGH)
|
||||
|
||||
params = Params()
|
||||
|
||||
# Setup sockets
|
||||
self.pm = pm
|
||||
if self.pm is None:
|
||||
@@ -52,8 +54,10 @@ class Controls:
|
||||
|
||||
self.sm = sm
|
||||
if self.sm is None:
|
||||
self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration',
|
||||
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'])
|
||||
socks = ['thermal', 'health', 'model', 'liveCalibration',
|
||||
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman', 'dragonConf']
|
||||
ignore_alive = None if params.get('dp_driver_monitor') == b'1' else ['dMonitoringState']
|
||||
self.sm = messaging.SubMaster(socks, ignore_alive=ignore_alive)
|
||||
|
||||
self.can_sock = can_sock
|
||||
if can_sock is None:
|
||||
@@ -72,7 +76,7 @@ class Controls:
|
||||
params = Params()
|
||||
self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
|
||||
self.is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
|
||||
internet_needed = (params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None) and (params.get("DisableUpdates") != b"1")
|
||||
internet_needed = False #(params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None) and (params.get("DisableUpdates") != b"1")
|
||||
community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1"
|
||||
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1"
|
||||
passive = params.get("Passive", encoding='utf8') == "1" or \
|
||||
@@ -102,7 +106,9 @@ class Controls:
|
||||
self.LoC = LongControl(self.CP, self.CI.compute_gb)
|
||||
self.VM = VehicleModel(self.CP)
|
||||
|
||||
if self.CP.lateralTuning.which() == 'pid':
|
||||
if params.get('dp_lqr') == b'1':
|
||||
self.LaC = LatControlLQR(self.CP)
|
||||
elif self.CP.lateralTuning.which() == 'pid':
|
||||
self.LaC = LatControlPID(self.CP)
|
||||
elif self.CP.lateralTuning.which() == 'indi':
|
||||
self.LaC = LatControlINDI(self.CP)
|
||||
@@ -133,21 +139,27 @@ class Controls:
|
||||
|
||||
self.startup_event = get_startup_event(car_recognized, controller_available, hw_type)
|
||||
|
||||
if not sounds_available:
|
||||
self.events.add(EventName.soundsUnavailable, static=True)
|
||||
if internet_needed:
|
||||
self.events.add(EventName.internetConnectivityNeeded, static=True)
|
||||
# if not sounds_available:
|
||||
# self.events.add(EventName.soundsUnavailable, static=True)
|
||||
# if internet_needed:
|
||||
# self.events.add(EventName.internetConnectivityNeeded, static=True)
|
||||
if community_feature_disallowed:
|
||||
self.events.add(EventName.communityFeatureDisallowed, static=True)
|
||||
if not car_recognized:
|
||||
self.events.add(EventName.carUnrecognized, static=True)
|
||||
if hw_type == HwType.whitePanda:
|
||||
self.events.add(EventName.whitePandaUnsupported, static=True)
|
||||
# if hw_type == HwType.whitePanda:
|
||||
# self.events.add(EventName.whitePandaUnsupported, static=True)
|
||||
|
||||
# controlsd is driven by can recv, expected at 100Hz
|
||||
self.rk = Ratekeeper(100, print_delay_threshold=None)
|
||||
self.prof = Profiler(False) # off by default
|
||||
|
||||
# dp
|
||||
self.dp_lead_count = 0
|
||||
self.dp_camera_offset = CAMERA_OFFSET * 100
|
||||
self.sm['dragonConf'].dpAtl = False
|
||||
self.sm['dragonConf'].dpCameraOffset = 6
|
||||
|
||||
def update_events(self, CS):
|
||||
"""Compute carEvents from carState"""
|
||||
|
||||
@@ -196,15 +208,15 @@ class Controls:
|
||||
self.events.add(EventName.laneChangeBlocked)
|
||||
else:
|
||||
if direction == LaneChangeDirection.left:
|
||||
self.events.add(EventName.preLaneChangeLeft)
|
||||
self.events.add(EventName.preLaneChangeLeftALC if self.sm['pathPlan'].dpALCAllowed else EventName.preLaneChangeLeft)
|
||||
else:
|
||||
self.events.add(EventName.preLaneChangeRight)
|
||||
self.events.add(EventName.preLaneChangeRightALC if self.sm['pathPlan'].dpALCAllowed else EventName.preLaneChangeRight)
|
||||
elif self.sm['pathPlan'].laneChangeState in [LaneChangeState.laneChangeStarting,
|
||||
LaneChangeState.laneChangeFinishing]:
|
||||
self.events.add(EventName.laneChange)
|
||||
|
||||
if self.can_rcv_error or (not CS.canValid and self.sm.frame > 5 / DT_CTRL):
|
||||
self.events.add(EventName.canError)
|
||||
self.events.add(EventName.pcmDisable if self.sm['dragonConf'].dpAtl else EventName.canError)
|
||||
if self.mismatch_counter >= 200:
|
||||
self.events.add(EventName.controlsMismatch)
|
||||
if not self.sm.alive['plan'] and self.sm.alive['pathPlan']:
|
||||
@@ -213,14 +225,14 @@ class Controls:
|
||||
elif not self.sm.all_alive_and_valid():
|
||||
self.events.add(EventName.commIssue)
|
||||
if not self.sm['pathPlan'].mpcSolutionValid:
|
||||
self.events.add(EventName.plannerError)
|
||||
if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
|
||||
if self.sm.frame > 5 / DT_CTRL: # Give locationd some time to receive all the inputs
|
||||
self.events.add(EventName.sensorDataInvalid)
|
||||
if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000):
|
||||
# Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
|
||||
if not (SIMULATION or NOSENSOR): # TODO: send GPS in carla
|
||||
self.events.add(EventName.noGps)
|
||||
self.events.add(EventName.steerTempUnavailable if self.sm['dragonConf'].dpAtl else EventName.plannerError)
|
||||
# if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
|
||||
# if self.sm.frame > 5 / DT_CTRL: # Give locationd some time to receive all the inputs
|
||||
# self.events.add(EventName.sensorDataInvalid)
|
||||
# if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000):
|
||||
# # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
|
||||
# if not (SIMULATION or NOSENSOR): # TODO: send GPS in carla
|
||||
# self.events.add(EventName.noGps)
|
||||
if not self.sm['pathPlan'].paramsValid:
|
||||
self.events.add(EventName.vehicleModelInvalid)
|
||||
if not self.sm['liveLocationKalman'].posenetOK:
|
||||
@@ -239,16 +251,30 @@ class Controls:
|
||||
self.events.add(EventName.modeldLagging)
|
||||
|
||||
# Only allow engagement with brake pressed when stopped behind another stopped car
|
||||
if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \
|
||||
if not self.sm['dragonConf'].dpAtl and CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \
|
||||
and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
|
||||
self.events.add(EventName.noTarget)
|
||||
|
||||
# dp lead car moving alert
|
||||
if self.sm['dragonConf'].dpLeadCarAlert:
|
||||
if not self.CP.radarOffCan and self.sm['plan'].hasLead and CS.vEgo <= 0.01 and 0.3 >= abs(self.sm['plan'].vTarget) >= 0:
|
||||
self.dp_lead_count += 1
|
||||
else:
|
||||
self.dp_lead_count = 0
|
||||
|
||||
if self.dp_lead_count >= 300 and abs(self.sm['plan'].vTargetFuture) >= 0.1:
|
||||
self.events.add(EventName.leadCarMoving)
|
||||
self.dp_lead_count = 0
|
||||
|
||||
if CS.vEgo > 0. or CS.gearShifter in [car.CarState.GearShifter.reverse, car.CarState.GearShifter.park]:
|
||||
self.dp_lead_count = 0
|
||||
|
||||
def data_sample(self):
|
||||
"""Receive data from sockets and update carState"""
|
||||
|
||||
# Update carState from CAN
|
||||
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
|
||||
CS = self.CI.update(self.CC, can_strs)
|
||||
CS = self.CI.update(self.CC, can_strs, self.sm['dragonConf'])
|
||||
|
||||
self.sm.update(0)
|
||||
|
||||
@@ -266,7 +292,7 @@ class Controls:
|
||||
if not self.enabled:
|
||||
self.mismatch_counter = 0
|
||||
|
||||
if not self.sm['health'].controlsAllowed and self.enabled:
|
||||
if not self.sm['dragonConf'].dpAtl and not self.sm['health'].controlsAllowed and self.enabled:
|
||||
self.mismatch_counter += 1
|
||||
|
||||
self.distance_traveled += CS.vEgo * DT_CTRL
|
||||
@@ -365,7 +391,7 @@ class Controls:
|
||||
|
||||
if not self.active:
|
||||
self.LaC.reset()
|
||||
self.LoC.reset(v_pid=CS.vEgo)
|
||||
self.LoC.reset(v_pid=plan.vTargetFuture)
|
||||
|
||||
plan_age = DT_CTRL * (self.sm.frame - self.sm.rcv_frame['plan'])
|
||||
# no greater than dt mpc + dt, to prevent too high extraps
|
||||
@@ -389,14 +415,15 @@ class Controls:
|
||||
self.saturated_count = 0
|
||||
|
||||
# Send a "steering required alert" if saturation count has reached the limit
|
||||
if (lac_log.saturated and not CS.steeringPressed) or \
|
||||
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
|
||||
# Check if we deviated from the path
|
||||
left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1
|
||||
right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1
|
||||
if self.sm['dragonConf'].dpLatCtrl and self.sm['dragonConf'].dpSteeringLimitAlert:
|
||||
if (lac_log.saturated and not CS.steeringPressed) or \
|
||||
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
|
||||
# Check if we deviated from the path
|
||||
left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1
|
||||
right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1
|
||||
|
||||
if left_deviation or right_deviation:
|
||||
self.events.add(EventName.steerSaturated)
|
||||
if left_deviation or right_deviation:
|
||||
self.events.add(EventName.steerSaturated)
|
||||
|
||||
return actuators, v_acc_sol, a_acc_sol, lac_log
|
||||
|
||||
@@ -433,10 +460,12 @@ class Controls:
|
||||
|
||||
meta = self.sm['model'].meta
|
||||
if len(meta.desirePrediction) and ldw_allowed:
|
||||
if self.sm.updated['dragonConf']:
|
||||
self.dp_camera_offset = self.sm['dragonConf'].dpCameraOffset * 0.01 if self.sm['dragonConf'].dpCameraOffset != 0 else 0
|
||||
l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1]
|
||||
r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1]
|
||||
l_lane_close = left_lane_visible and (self.sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET))
|
||||
r_lane_close = right_lane_visible and (self.sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET))
|
||||
l_lane_close = left_lane_visible and (self.sm['pathPlan'].lPoly[3] < (1.08 - self.dp_camera_offset))
|
||||
r_lane_close = right_lane_visible and (self.sm['pathPlan'].rPoly[3] > -(1.08 + self.dp_camera_offset))
|
||||
|
||||
CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
|
||||
CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
|
||||
|
||||
@@ -0,0 +1,340 @@
|
||||
import math
|
||||
import numpy as np
|
||||
from common.realtime import sec_since_boot
|
||||
from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
|
||||
from common.numpy_fast import interp, clip
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.params import Params
|
||||
from common.dp_time import LAST_MODIFIED_DYNAMIC_FOLLOW
|
||||
from common.dp_common import get_last_modified, param_get, param_get_if_updated
|
||||
|
||||
from selfdrive.controls.lib.dynamic_follow.auto_df import predict
|
||||
from selfdrive.controls.lib.dynamic_follow.support import LeadData, CarData, dfData, dfProfiles
|
||||
travis = False
|
||||
|
||||
# dp
|
||||
PROFILE_AUTO = 4
|
||||
PROFILE_LONG = 3
|
||||
PROFILE_NORMAL = 2
|
||||
PROFILE_SHORT = 1
|
||||
PROFILE_OFF = 0
|
||||
|
||||
class DynamicFollow:
|
||||
def __init__(self, mpc_id):
|
||||
self.mpc_id = mpc_id
|
||||
self.df_profiles = dfProfiles()
|
||||
self.global_df_mod = 1.
|
||||
self.min_TR = 0.9
|
||||
|
||||
# Model variables
|
||||
mpc_rate = 1 / 20.
|
||||
self.model_scales = {'v_ego': [-0.06112159043550491, 37.96522521972656], 'a_lead': [-3.109330892562866, 3.3612186908721924], 'v_lead': [0.0, 35.27671432495117], 'x_lead': [2.4600000381469727, 141.44000244140625]}
|
||||
self.predict_rate = 1 / 4.
|
||||
self.skip_every = round(0.25 / mpc_rate)
|
||||
self.model_input_len = round(45 / mpc_rate)
|
||||
|
||||
# Dynamic follow variables
|
||||
self.default_TR = 1.8
|
||||
self.TR = 1.8
|
||||
# self.v_lead_retention = 2.0 # keep only last x seconds
|
||||
self.v_ego_retention = 2.5
|
||||
self.v_rel_retention = 1.5
|
||||
|
||||
self.sng_TR = 1.8 # reacceleration stop and go TR
|
||||
self.sng_speed = 18.0 * CV.MPH_TO_MS
|
||||
|
||||
# dp params
|
||||
self.last_ts = 0.
|
||||
self.modified = None
|
||||
self.last_modified = None
|
||||
self.last_modified_check = None
|
||||
self.dp_dynamic_follow = PROFILE_OFF
|
||||
self.dp_dynamic_follow_last_modified = None
|
||||
self.dp_dynamic_follow_multiplier_last_modified = None
|
||||
self.dp_dynamic_follow_min_tr_last_modified = None
|
||||
self.params = Params()
|
||||
|
||||
self._setup_changing_variables()
|
||||
|
||||
def _setup_changing_variables(self):
|
||||
self.TR = self.default_TR
|
||||
self.model_profile = None
|
||||
|
||||
self.sng = False
|
||||
self.car_data = CarData()
|
||||
self.lead_data = LeadData()
|
||||
self.df_data = dfData() # dynamic follow data
|
||||
|
||||
self.last_cost = 0.0
|
||||
self.last_predict_time = 0.0
|
||||
self.auto_df_model_data = []
|
||||
self._get_live_params() # so they're defined just in case
|
||||
|
||||
def update(self, CS, libmpc):
|
||||
self._get_live_params()
|
||||
self._update_car(CS)
|
||||
self._get_profiles()
|
||||
|
||||
if not self.lead_data.status or self.dp_dynamic_follow == PROFILE_OFF:
|
||||
self.TR = self.default_TR
|
||||
else:
|
||||
self._store_df_data()
|
||||
self.TR = self._get_TR()
|
||||
|
||||
if not travis:
|
||||
self._change_cost(libmpc)
|
||||
|
||||
return self.TR
|
||||
|
||||
def _get_profiles(self):
|
||||
"""This receives profile change updates from dfManager and runs the auto-df prediction if auto mode"""
|
||||
if self.dp_dynamic_follow == PROFILE_AUTO: # todo: find some way to share prediction between the two mpcs to reduce processing overhead
|
||||
self._get_pred() # sets self.model_profile, all other checks are inside function
|
||||
|
||||
def _norm(self, x, name):
|
||||
self.x = x
|
||||
return np.interp(x, self.model_scales[name], [0, 1])
|
||||
|
||||
def _change_cost(self, libmpc):
|
||||
TRs = [0.9, 1.8, 2.7]
|
||||
costs = [1.0, 0.115, 0.05]
|
||||
cost = interp(self.TR, TRs, costs)
|
||||
if self.last_cost != cost:
|
||||
libmpc.change_tr(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
|
||||
self.last_cost = cost
|
||||
|
||||
def _store_df_data(self):
|
||||
cur_time = sec_since_boot()
|
||||
# Store custom relative accel over time
|
||||
if self.lead_data.status:
|
||||
if self.lead_data.new_lead:
|
||||
self.df_data.v_rels = [] # reset when new lead
|
||||
else:
|
||||
self.df_data.v_rels = self._remove_old_entries(self.df_data.v_rels, cur_time, self.v_rel_retention)
|
||||
self.df_data.v_rels.append({'v_ego': self.car_data.v_ego, 'v_lead': self.lead_data.v_lead, 'time': cur_time})
|
||||
|
||||
# Store our velocity for better sng
|
||||
self.df_data.v_egos = self._remove_old_entries(self.df_data.v_egos, cur_time, self.v_ego_retention)
|
||||
self.df_data.v_egos.append({'v_ego': self.car_data.v_ego, 'time': cur_time})
|
||||
|
||||
# Store data for auto-df model
|
||||
self.auto_df_model_data.append([self._norm(self.car_data.v_ego, 'v_ego'),
|
||||
self._norm(self.lead_data.v_lead, 'v_lead'),
|
||||
self._norm(self.lead_data.a_lead, 'a_lead'),
|
||||
self._norm(self.lead_data.x_lead, 'x_lead')])
|
||||
while len(self.auto_df_model_data) > self.model_input_len:
|
||||
del self.auto_df_model_data[0]
|
||||
|
||||
def _get_pred(self):
|
||||
cur_time = sec_since_boot()
|
||||
if self.car_data.cruise_enabled and self.lead_data.status:
|
||||
if cur_time - self.last_predict_time > self.predict_rate:
|
||||
if len(self.auto_df_model_data) == self.model_input_len:
|
||||
pred = predict(np.array(self.auto_df_model_data[::self.skip_every], dtype=np.float32).flatten())
|
||||
self.last_predict_time = cur_time
|
||||
self.model_profile = int(np.argmax(pred))
|
||||
|
||||
def _remove_old_entries(self, lst, cur_time, retention):
|
||||
return [sample for sample in lst if cur_time - sample['time'] <= retention]
|
||||
|
||||
def _calculate_relative_accel_new(self):
|
||||
# """
|
||||
# Moving window returning the following: (final relative velocity - initial relative velocity) / dT with a few extra mods
|
||||
# Output properties:
|
||||
# When the lead is starting to decelerate, and our car remains the same speed, the output decreases (and vice versa)
|
||||
# However when our car finally starts to decelerate at the same rate as the lead car, the output will move to near 0
|
||||
# >>> a = [(15 - 18), (14 - 17)]
|
||||
# >>> (a[-1] - a[0]) / 1
|
||||
# > 0.0
|
||||
# """
|
||||
min_consider_time = 0.5 # minimum amount of time required to consider calculation
|
||||
if len(self.df_data.v_rels) > 0: # if not empty
|
||||
elapsed_time = self.df_data.v_rels[-1]['time'] - self.df_data.v_rels[0]['time']
|
||||
if elapsed_time > min_consider_time:
|
||||
x = [-2.6822, -1.7882, -0.8941, -0.447, -0.2235, 0.0, 0.2235, 0.447, 0.8941, 1.7882, 2.6822]
|
||||
y = [0.3245, 0.277, 0.11075, 0.08106, 0.06325, 0.0, -0.09, -0.09375, -0.125, -0.3, -0.35]
|
||||
|
||||
v_lead_start = self.df_data.v_rels[0]['v_lead'] # setup common variables
|
||||
v_ego_start = self.df_data.v_rels[0]['v_ego']
|
||||
v_lead_end = self.df_data.v_rels[-1]['v_lead']
|
||||
v_ego_end = self.df_data.v_rels[-1]['v_ego']
|
||||
|
||||
v_ego_change = v_ego_end - v_ego_start
|
||||
v_lead_change = v_lead_end - v_lead_start
|
||||
|
||||
if v_lead_change - v_ego_change == 0 or v_lead_change + v_ego_change == 0:
|
||||
return None
|
||||
|
||||
initial_v_rel = v_lead_start - v_ego_start
|
||||
cur_v_rel = v_lead_end - v_ego_end
|
||||
delta_v_rel = (cur_v_rel - initial_v_rel) / elapsed_time
|
||||
|
||||
neg_pos = False
|
||||
if v_ego_change == 0 or v_lead_change == 0: # FIXME: this all is a mess, but works. need to simplify
|
||||
lead_factor = v_lead_change / (v_lead_change - v_ego_change)
|
||||
|
||||
elif (v_ego_change < 0) != (v_lead_change < 0): # one is negative and one is positive, or ^ = XOR
|
||||
lead_factor = v_lead_change / (v_lead_change - v_ego_change)
|
||||
if v_ego_change > 0 > v_lead_change:
|
||||
delta_v_rel = -delta_v_rel # switch when appropriate
|
||||
neg_pos = True
|
||||
|
||||
elif v_ego_change * v_lead_change > 0: # both are negative or both are positive
|
||||
lead_factor = v_lead_change / (v_lead_change + v_ego_change)
|
||||
if v_ego_change > 0 and v_lead_change > 0: # both are positive
|
||||
if v_ego_change < v_lead_change:
|
||||
delta_v_rel = -delta_v_rel # switch when appropriate
|
||||
elif v_ego_change > v_lead_change: # both are negative and v_ego_change > v_lead_change
|
||||
delta_v_rel = -delta_v_rel
|
||||
|
||||
else:
|
||||
raise Exception('Uncovered case! Should be impossible to be be here')
|
||||
|
||||
if not neg_pos: # negative and positive require different mod code to be correct
|
||||
rel_vel_mod = (-delta_v_rel * abs(lead_factor)) + (delta_v_rel * (1 - abs(lead_factor)))
|
||||
else:
|
||||
rel_vel_mod = math.copysign(delta_v_rel, v_lead_change - v_ego_change) * lead_factor
|
||||
|
||||
calc_mod = np.interp(rel_vel_mod, x, y)
|
||||
if v_lead_end > v_ego_end and calc_mod >= 0:
|
||||
# if we're accelerating quicker than lead but lead is still faster, reduce mod
|
||||
# todo: could remove this since we restrict this mod where called
|
||||
x = np.array([0, 2, 4, 8]) * CV.MPH_TO_MS
|
||||
y = [1.0, -0.25, -0.65, -0.95]
|
||||
v_rel_mod = np.interp(v_lead_end - v_ego_end, x, y)
|
||||
calc_mod *= v_rel_mod
|
||||
return calc_mod
|
||||
return None
|
||||
|
||||
def global_profile_mod(self, profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist):
|
||||
"""
|
||||
This function modifies the y_dist list used by dynamic follow in accordance with global_df_mod
|
||||
It also intelligently adjusts the profile mods at each breakpoint based on the change in TR
|
||||
"""
|
||||
if self.global_df_mod == 1.:
|
||||
return profile_mod_pos, profile_mod_neg, y_dist
|
||||
global_df_mod = 1 - self.global_df_mod
|
||||
|
||||
# Calculate new TRs
|
||||
speeds = [0, self.sng_speed, 18, x_vel[-1]] # [0, 18 mph, ~40 mph, highest profile mod speed (~78 mph)]
|
||||
mods = [0, 0.1, 0.7, 1] # how much to limit global_df_mod at each speed, 1 is full effect
|
||||
y_dist_new = [y - (y * global_df_mod * np.interp(x, speeds, mods)) for x, y in zip(x_vel, y_dist)]
|
||||
|
||||
# Calculate how to change profile mods based on change in TR
|
||||
# eg. if df mod is 0.7, then increase positive mod and decrease negative mod
|
||||
calc_profile_mods = [(np.interp(mod_x, x_vel, y_dist) - np.interp(mod_x, x_vel, y_dist_new) + 1) for mod_x in profile_mod_x]
|
||||
profile_mod_pos = [mod_pos * mod for mod_pos, mod in zip(profile_mod_pos, calc_profile_mods)]
|
||||
profile_mod_neg = [mod_neg * ((1 - mod) + 1) for mod_neg, mod in zip(profile_mod_neg, calc_profile_mods)]
|
||||
|
||||
return profile_mod_pos, profile_mod_neg, y_dist_new
|
||||
|
||||
def _get_TR(self):
|
||||
x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities
|
||||
profile_mod_x = [2.2352, 13.4112, 24.5872, 35.7632] # profile mod speeds, mph: [5., 30., 55., 80.]
|
||||
|
||||
if self.dp_dynamic_follow == PROFILE_AUTO: # decide which profile to use, model profile will be updated before this
|
||||
# df is 0 = traffic, 1 = relaxed, 2 = roadtrip, 3 = auto
|
||||
# dp is 0 = off, 1 = short, 2 = normal, 3 = long, 4 = auto
|
||||
# if it's model profile, we need to convert it
|
||||
if self.model_profile is None:
|
||||
# when its none, we use normal instead
|
||||
df_profile = PROFILE_NORMAL
|
||||
else:
|
||||
df_profile = self.model_profile + 1
|
||||
else:
|
||||
df_profile = self.dp_dynamic_follow
|
||||
|
||||
if df_profile == PROFILE_LONG:
|
||||
y_dist = [1.3978, 1.4132, 1.4318, 1.4536, 1.485, 1.5229, 1.5819, 1.6203, 1.7238, 1.8231, 1.8379, 1.8495, 1.8535] # TRs
|
||||
profile_mod_pos = [0.92, 0.7, 0.25, 0.15]
|
||||
profile_mod_neg = [1.1, 1.3, 2.0, 2.3]
|
||||
elif df_profile == PROFILE_SHORT: # for in congested traffic
|
||||
x_vel = [0.0, 1.892, 3.7432, 5.8632, 8.0727, 10.7301, 14.343, 17.6275, 22.4049, 28.6752, 34.8858, 40.35]
|
||||
# y_dist = [1.3781, 1.3791, 1.3802, 1.3825, 1.3984, 1.4249, 1.4194, 1.3162, 1.1916, 1.0145, 0.9855, 0.9562] # original
|
||||
# y_dist = [1.3781, 1.3791, 1.3112, 1.2442, 1.2306, 1.2112, 1.2775, 1.1977, 1.0963, 0.9435, 0.9067, 0.8749] # avg. 7.3 ft closer from 18 to 90 mph
|
||||
y_dist = [1.3781, 1.3791, 1.3457, 1.3134, 1.3145, 1.318, 1.3485, 1.257, 1.144, 0.979, 0.9461, 0.9156]
|
||||
profile_mod_pos = [1.05, 1.55, 2.6, 3.75]
|
||||
profile_mod_neg = [0.84, .275, 0.1, 0.05]
|
||||
elif df_profile == PROFILE_NORMAL: # default to relaxed/stock
|
||||
y_dist = [1.385, 1.394, 1.406, 1.421, 1.444, 1.474, 1.516, 1.534, 1.546, 1.568, 1.579, 1.593, 1.614]
|
||||
profile_mod_pos = [1.0] * 4
|
||||
profile_mod_neg = [1.0] * 4
|
||||
else:
|
||||
raise Exception('Unknown profile type: {}'.format(df_profile))
|
||||
|
||||
# Global df mod
|
||||
profile_mod_pos, profile_mod_neg, y_dist = self.global_profile_mod(profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist)
|
||||
|
||||
# Profile modifications - Designed so that each profile reacts similarly to changing lead dynamics
|
||||
profile_mod_pos = interp(self.car_data.v_ego, profile_mod_x, profile_mod_pos)
|
||||
profile_mod_neg = interp(self.car_data.v_ego, profile_mod_x, profile_mod_neg)
|
||||
|
||||
if self.car_data.v_ego > self.sng_speed: # keep sng distance until we're above sng speed again
|
||||
self.sng = False
|
||||
|
||||
if (self.car_data.v_ego >= self.sng_speed or self.df_data.v_egos[0]['v_ego'] >= self.car_data.v_ego) and not self.sng:
|
||||
# if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use sng_TR and slowly decrease
|
||||
TR = interp(self.car_data.v_ego, x_vel, y_dist)
|
||||
else: # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating
|
||||
self.sng = True
|
||||
x = [self.sng_speed * 0.7, self.sng_speed] # decrease TR between 12.6 and 18 mph from 1.8s to defined TR above at 18mph while accelerating
|
||||
y = [self.sng_TR, interp(self.sng_speed, x_vel, y_dist)]
|
||||
TR = interp(self.car_data.v_ego, x, y)
|
||||
|
||||
TR_mods = []
|
||||
# Dynamic follow modifications (the secret sauce)
|
||||
x = [-26.8224, -20.0288, -15.6871, -11.1965, -7.8645, -4.9472, -3.0541, -2.2244, -1.5045, -0.7908, -0.3196, 0.0, 0.5588, 1.3682, 1.898, 2.7316, 4.4704] # relative velocity values
|
||||
y = [.76, 0.62323, 0.49488, 0.40656, 0.32227, 0.23914, 0.12269, 0.10483, 0.08074, 0.04886, 0.0072, 0.0, -0.05648, -0.0792, -0.15675, -0.23289, -0.315] # modification values
|
||||
TR_mods.append(interp(self.lead_data.v_lead - self.car_data.v_ego, x, y))
|
||||
|
||||
x = [-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466, 0.5144, 0.6903, 0.9302] # lead acceleration values
|
||||
y = [0.24, 0.16, 0.092, 0.0515, 0.0305, 0.022, 0.0, -0.0153, -0.042, -0.053, -0.059] # modification values
|
||||
TR_mods.append(interp(self.lead_data.a_lead, x, y))
|
||||
|
||||
rel_accel_mod = self._calculate_relative_accel_new()
|
||||
if rel_accel_mod is not None: # if available
|
||||
deadzone = 2 * CV.MPH_TO_MS
|
||||
if self.lead_data.v_lead - deadzone > self.car_data.v_ego:
|
||||
TR_mods.append(rel_accel_mod)
|
||||
|
||||
x = [self.sng_speed / 5.0, self.sng_speed] # as we approach 0, apply x% more distance
|
||||
y = [1.05, 1.0]
|
||||
profile_mod_pos *= interp(self.car_data.v_ego, x, y) # but only for currently positive mods
|
||||
|
||||
TR_mod = sum([mod * profile_mod_neg if mod < 0 else mod * profile_mod_pos for mod in TR_mods]) # alter TR modification according to profile
|
||||
TR += TR_mod
|
||||
|
||||
if self.car_data.left_blinker or self.car_data.right_blinker and df_profile != self.df_profiles.traffic:
|
||||
x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph
|
||||
y = [1.0, .75, .65]
|
||||
TR *= interp(self.car_data.v_ego, x, y) # reduce TR when changing lanes
|
||||
|
||||
return float(clip(TR, self.min_TR, 2.7))
|
||||
|
||||
def update_lead(self, v_lead=None, a_lead=None, x_lead=None, status=False, new_lead=False):
|
||||
self.lead_data.v_lead = v_lead
|
||||
self.lead_data.a_lead = a_lead
|
||||
self.lead_data.x_lead = x_lead
|
||||
|
||||
self.lead_data.status = status
|
||||
self.lead_data.new_lead = new_lead
|
||||
|
||||
def _update_car(self, CS):
|
||||
self.car_data.v_ego = CS.vEgo
|
||||
self.car_data.a_ego = CS.aEgo
|
||||
|
||||
self.car_data.left_blinker = CS.leftBlinker
|
||||
self.car_data.right_blinker = CS.rightBlinker
|
||||
self.car_data.cruise_enabled = CS.cruiseState.enabled
|
||||
|
||||
def _get_live_params(self):
|
||||
self.last_modified_check, self.modified = get_last_modified(LAST_MODIFIED_DYNAMIC_FOLLOW, self.last_modified_check, self.modified)
|
||||
if self.last_modified != self.modified:
|
||||
self.dp_dynamic_follow, self.dp_dynamic_follow_last_modified = param_get_if_updated("dp_dynamic_follow", "int", self.dp_dynamic_follow, self.dp_dynamic_follow_last_modified)
|
||||
self.global_df_mod, self.dp_dynamic_follow_multiplier_last_modified = param_get_if_updated("dp_dynamic_follow_multiplier", "float", self.global_df_mod, self.dp_dynamic_follow_multiplier_last_modified)
|
||||
if self.global_df_mod != 1.:
|
||||
self.global_df_mod = clip(self.global_df_mod, .85, 1.2)
|
||||
self.min_TR, self.dp_dynamic_follow_min_tr_last_modified = param_get_if_updated("dp_dynamic_follow_min_tr", "float", self.min_TR, self.dp_dynamic_follow_min_tr_last_modified)
|
||||
if self.min_TR != .9:
|
||||
self.min_TR = clip(self.min_TR, .85, 1.6)
|
||||
self.last_modified = self.modified
|
||||
@@ -0,0 +1,20 @@
|
||||
"""
|
||||
Generated using Konverter: https://github.com/ShaneSmiskol/Konverter
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
wb = np.load('/data/openpilot/selfdrive/controls/lib/dynamic_follow/auto_df_weights.npz', allow_pickle=True)
|
||||
w, b = wb['wb']
|
||||
|
||||
def softmax(x):
|
||||
return np.exp(x) / np.sum(np.exp(x), axis=0)
|
||||
|
||||
def predict(x):
|
||||
l0 = np.dot(x, w[0]) + b[0]
|
||||
l0 = np.maximum(0, l0)
|
||||
l1 = np.dot(l0, w[1]) + b[1]
|
||||
l1 = np.maximum(0, l1)
|
||||
l2 = np.dot(l1, w[2]) + b[2]
|
||||
l2 = softmax(l2)
|
||||
return l2
|
||||
Binary file not shown.
@@ -0,0 +1,31 @@
|
||||
class LeadData:
|
||||
v_lead = None
|
||||
x_lead = None
|
||||
a_lead = None
|
||||
status = False
|
||||
new_lead = False
|
||||
|
||||
|
||||
class CarData:
|
||||
v_ego = 0.0
|
||||
a_ego = 0.0
|
||||
|
||||
left_blinker = False
|
||||
right_blinker = False
|
||||
cruise_enabled = True
|
||||
|
||||
|
||||
class dfData:
|
||||
v_egos = []
|
||||
v_rels = []
|
||||
|
||||
|
||||
class dfProfiles:
|
||||
traffic = 0
|
||||
relaxed = 1
|
||||
roadtrip = 2
|
||||
auto = 3
|
||||
to_profile = {0: 'traffic', 1: 'relaxed', 2: 'roadtrip', 3: 'auto'}
|
||||
to_idx = {v: k for k, v in to_profile.items()}
|
||||
|
||||
default = relaxed
|
||||
+229
-154
@@ -1,4 +1,7 @@
|
||||
# This Python file uses the following encoding: utf-8
|
||||
# -*- coding: utf-8 -*-
|
||||
from enum import IntEnum
|
||||
from functools import total_ordering
|
||||
from typing import Dict, Union, Callable, Any
|
||||
|
||||
from cereal import log, car
|
||||
@@ -6,6 +9,8 @@ import cereal.messaging as messaging
|
||||
from common.realtime import DT_CTRL
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.locationd.calibrationd import MIN_SPEED_FILTER
|
||||
from common.i18n import events
|
||||
_ = events()
|
||||
|
||||
AlertSize = log.ControlsState.AlertSize
|
||||
AlertStatus = log.ControlsState.AlertStatus
|
||||
@@ -140,21 +145,21 @@ class Alert:
|
||||
class NoEntryAlert(Alert):
|
||||
def __init__(self, alert_text_2, audible_alert=AudibleAlert.chimeError,
|
||||
visual_alert=VisualAlert.none, duration_hud_alert=2.):
|
||||
super().__init__("openpilot Unavailable", alert_text_2, AlertStatus.normal,
|
||||
super().__init__(_("openpilot Unavailable"), alert_text_2, AlertStatus.normal,
|
||||
AlertSize.mid, Priority.LOW, visual_alert,
|
||||
audible_alert, .4, duration_hud_alert, 3.)
|
||||
|
||||
|
||||
class SoftDisableAlert(Alert):
|
||||
def __init__(self, alert_text_2):
|
||||
super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2,
|
||||
super().__init__(_("TAKE CONTROL IMMEDIATELY"), alert_text_2,
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.MID, VisualAlert.steerRequired,
|
||||
AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
|
||||
|
||||
|
||||
class ImmediateDisableAlert(Alert):
|
||||
def __init__(self, alert_text_2, alert_text_1="TAKE CONTROL IMMEDIATELY"):
|
||||
def __init__(self, alert_text_2, alert_text_1=_("TAKE CONTROL IMMEDIATELY")):
|
||||
super().__init__(alert_text_1, alert_text_2,
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired,
|
||||
@@ -179,8 +184,8 @@ def below_steer_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric:
|
||||
speed = int(round(CP.minSteerSpeed * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)))
|
||||
unit = "km/h" if metric else "mph"
|
||||
return Alert(
|
||||
"TAKE CONTROL",
|
||||
"Steer Unavailable Below %d %s" % (speed, unit),
|
||||
_("TAKE CONTROL"),
|
||||
_("Steer Unavailable Below %(speed)d %(unit)s") % ({"speed": speed, "unit": unit}),
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.none, 0., 0.4, .3)
|
||||
|
||||
@@ -188,23 +193,23 @@ def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, met
|
||||
speed = int(MIN_SPEED_FILTER * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
|
||||
unit = "km/h" if metric else "mph"
|
||||
return Alert(
|
||||
"Calibration in Progress: %d%%" % sm['liveCalibration'].calPerc,
|
||||
"Drive Above %d %s" % (speed, unit),
|
||||
_("Calibration in Progress: %d%%") % sm['liveCalibration'].calPerc,
|
||||
_("Drive Above %(speed)d %(unit)s") % ({"speed": speed, "unit": unit}),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2)
|
||||
|
||||
def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
|
||||
gps_integrated = sm['health'].hwType in [log.HealthData.HwType.uno, log.HealthData.HwType.dos]
|
||||
return Alert(
|
||||
"Poor GPS reception",
|
||||
"If sky is visible, contact support" if gps_integrated else "Check GPS antenna placement",
|
||||
_("Poor GPS reception"),
|
||||
_("If sky is visible, contact support") if gps_integrated else _("Check GPS antenna placement"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2, creation_delay=300.)
|
||||
|
||||
def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
|
||||
text = "Cruise Mode Disabled"
|
||||
text = _("Cruise Mode Disabled")
|
||||
if CP.carName == "honda":
|
||||
text = "Main Switch Off"
|
||||
text = _("Main Switch Off")
|
||||
return NoEntryAlert(text, duration_hud_alert=0.)
|
||||
|
||||
EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, bool], Alert]]]] = {
|
||||
@@ -214,7 +219,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
|
||||
|
||||
EventName.debugAlert: {
|
||||
ET.PERMANENT: Alert(
|
||||
"DEBUG ALERT",
|
||||
_("DEBUG ALERT"),
|
||||
"",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, .1, .1),
|
||||
@@ -222,32 +227,32 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
|
||||
|
||||
EventName.startup: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Be ready to take over at any time",
|
||||
"Always keep hands on wheel and eyes on road",
|
||||
_("Be ready to take over at any time"),
|
||||
_("Always keep hands on wheel and eyes on road"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
|
||||
},
|
||||
|
||||
EventName.startupMaster: {
|
||||
ET.PERMANENT: Alert(
|
||||
"WARNING: This branch is not tested",
|
||||
"Always keep hands on wheel and eyes on road",
|
||||
_("WARNING: This branch is not tested"),
|
||||
_("Always keep hands on wheel and eyes on road"),
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
|
||||
},
|
||||
|
||||
EventName.startupNoControl: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Dashcam mode",
|
||||
"Always keep hands on wheel and eyes on road",
|
||||
_("Dashcam mode"),
|
||||
_("Always keep hands on wheel and eyes on road"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
|
||||
},
|
||||
|
||||
EventName.startupNoCar: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Dashcam mode for unsupported car",
|
||||
"Always keep hands on wheel and eyes on road",
|
||||
_("Dashcam mode for unsupported car"),
|
||||
_("Always keep hands on wheel and eyes on road"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.),
|
||||
},
|
||||
@@ -262,25 +267,25 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
|
||||
|
||||
EventName.invalidGiraffeToyota: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Unsupported Giraffe Configuration",
|
||||
"Visit comma.ai/tg",
|
||||
_("Unsupported Giraffe Configuration"),
|
||||
_("Visit comma.ai/tg"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
},
|
||||
|
||||
EventName.whitePandaUnsupported: {
|
||||
ET.PERMANENT: Alert(
|
||||
"White Panda No Longer Supported",
|
||||
"Upgrade to comma two or black panda",
|
||||
_("White Panda No Longer Supported"),
|
||||
_("Upgrade to comma two or black panda"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
ET.NO_ENTRY: NoEntryAlert("Unsupported Hardware"),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Unsupported Hardware")),
|
||||
},
|
||||
|
||||
EventName.invalidLkasSetting: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Stock LKAS is turned on",
|
||||
"Turn off stock LKAS to engage",
|
||||
_("Stock LKAS is turned on"),
|
||||
_("Turn off stock LKAS to engage"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
},
|
||||
@@ -288,48 +293,48 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
|
||||
EventName.communityFeatureDisallowed: {
|
||||
# LOW priority to overcome Cruise Error
|
||||
ET.PERMANENT: Alert(
|
||||
"Community Feature Detected",
|
||||
"Enable Community Features in Developer Settings",
|
||||
_("Community Feature Detected"),
|
||||
_("Enable Community Features in Developer Settings"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
},
|
||||
|
||||
EventName.carUnrecognized: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Dashcam Mode",
|
||||
"Car Unrecognized",
|
||||
_("Dashcam Mode"),
|
||||
_("Car Unrecognized"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
},
|
||||
|
||||
EventName.stockAeb: {
|
||||
ET.PERMANENT: Alert(
|
||||
"BRAKE!",
|
||||
"Stock AEB: Risk of Collision",
|
||||
_("BRAKE!"),
|
||||
_("Stock AEB: Risk of Collision"),
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 1., 2., 2.),
|
||||
},
|
||||
|
||||
EventName.stockFcw: {
|
||||
ET.PERMANENT: Alert(
|
||||
"BRAKE!",
|
||||
"Stock FCW: Risk of Collision",
|
||||
_("BRAKE!"),
|
||||
_("Stock FCW: Risk of Collision"),
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 1., 2., 2.),
|
||||
},
|
||||
|
||||
EventName.fcw: {
|
||||
ET.PERMANENT: Alert(
|
||||
"BRAKE!",
|
||||
"Risk of Collision",
|
||||
_("BRAKE!"),
|
||||
_("Risk of Collision"),
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.chimeWarningRepeat, 1., 2., 2.),
|
||||
},
|
||||
|
||||
EventName.ldw: {
|
||||
ET.PERMANENT: Alert(
|
||||
"TAKE CONTROL",
|
||||
"Lane Departure Detected",
|
||||
_("TAKE CONTROL"),
|
||||
_("Lane Departure Detected"),
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 1., 2., 3.),
|
||||
},
|
||||
@@ -338,7 +343,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
|
||||
|
||||
EventName.gasPressed: {
|
||||
ET.PRE_ENABLE: Alert(
|
||||
"openpilot will not brake while gas pressed",
|
||||
_("openpilot will not brake while gas pressed"),
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .0, .0, .1, creation_delay=1.),
|
||||
@@ -346,7 +351,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
|
||||
|
||||
EventName.vehicleModelInvalid: {
|
||||
ET.WARNING: Alert(
|
||||
"Vehicle Parameter Identification Failed",
|
||||
_("Vehicle Parameter Identification Failed"),
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOWEST, VisualAlert.steerRequired, AudibleAlert.none, .0, .0, .1),
|
||||
@@ -354,15 +359,15 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
|
||||
|
||||
EventName.steerTempUnavailableMute: {
|
||||
ET.WARNING: Alert(
|
||||
"TAKE CONTROL",
|
||||
"Steering Temporarily Unavailable",
|
||||
_("TAKE CONTROL"),
|
||||
_("Steering Temporarily Unavailable"),
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, .2, .2, .2),
|
||||
},
|
||||
|
||||
EventName.preDriverDistracted: {
|
||||
ET.WARNING: Alert(
|
||||
"KEEP EYES ON ROAD: Driver Distracted",
|
||||
_("KEEP EYES ON ROAD: Driver Distracted"),
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1),
|
||||
@@ -370,23 +375,23 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
|
||||
|
||||
EventName.promptDriverDistracted: {
|
||||
ET.WARNING: Alert(
|
||||
"KEEP EYES ON ROAD",
|
||||
"Driver Distracted",
|
||||
_("KEEP EYES ON ROAD"),
|
||||
_("Driver Distracted"),
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2Repeat, .1, .1, .1),
|
||||
},
|
||||
|
||||
EventName.driverDistracted: {
|
||||
ET.WARNING: Alert(
|
||||
"DISENGAGE IMMEDIATELY",
|
||||
"Driver Distracted",
|
||||
_("DISENGAGE IMMEDIATELY"),
|
||||
_("Driver Distracted"),
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1),
|
||||
},
|
||||
|
||||
EventName.preDriverUnresponsive: {
|
||||
ET.WARNING: Alert(
|
||||
"TOUCH STEERING WHEEL: No Face Detected",
|
||||
_("TOUCH STEERING WHEEL: No Face Detected"),
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
|
||||
@@ -394,40 +399,40 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
|
||||
|
||||
EventName.promptDriverUnresponsive: {
|
||||
ET.WARNING: Alert(
|
||||
"TOUCH STEERING WHEEL",
|
||||
"Driver Unresponsive",
|
||||
_("TOUCH STEERING WHEEL"),
|
||||
_("Driver Unresponsive"),
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2Repeat, .1, .1, .1),
|
||||
},
|
||||
|
||||
EventName.driverUnresponsive: {
|
||||
ET.WARNING: Alert(
|
||||
"DISENGAGE IMMEDIATELY",
|
||||
"Driver Unresponsive",
|
||||
_("DISENGAGE IMMEDIATELY"),
|
||||
_("Driver Unresponsive"),
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, .1, .1),
|
||||
},
|
||||
|
||||
EventName.driverMonitorLowAcc: {
|
||||
ET.WARNING: Alert(
|
||||
"CHECK DRIVER FACE VISIBILITY",
|
||||
"Driver Monitoring Uncertain",
|
||||
_("CHECK DRIVER FACE VISIBILITY"),
|
||||
_("Driver Monitoring Uncertain"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .4, 0., 1.5),
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .4, 0., 1.),
|
||||
},
|
||||
|
||||
EventName.manualRestart: {
|
||||
ET.WARNING: Alert(
|
||||
"TAKE CONTROL",
|
||||
"Resume Driving Manually",
|
||||
_("TAKE CONTROL"),
|
||||
_("Resume Driving Manually"),
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
},
|
||||
|
||||
EventName.resumeRequired: {
|
||||
ET.WARNING: Alert(
|
||||
"STOPPED",
|
||||
"Press Resume to Move",
|
||||
_("STOPPED"),
|
||||
_("Press Resume to Move"),
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
},
|
||||
@@ -438,46 +443,46 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
|
||||
|
||||
EventName.preLaneChangeLeft: {
|
||||
ET.WARNING: Alert(
|
||||
"Steer Left to Start Lane Change",
|
||||
"Monitor Other Vehicles",
|
||||
_("Steer Left to Start Lane Change"),
|
||||
_("Monitor Other Vehicles"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
|
||||
},
|
||||
|
||||
EventName.preLaneChangeRight: {
|
||||
ET.WARNING: Alert(
|
||||
"Steer Right to Start Lane Change",
|
||||
"Monitor Other Vehicles",
|
||||
_("Steer Right to Start Lane Change"),
|
||||
_("Monitor Other Vehicles"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
|
||||
},
|
||||
|
||||
EventName.laneChangeBlocked: {
|
||||
ET.WARNING: Alert(
|
||||
"Car Detected in Blindspot",
|
||||
"Monitor Other Vehicles",
|
||||
_("Car Detected in Blindspot"),
|
||||
_("Monitor Other Vehicles"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1),
|
||||
},
|
||||
|
||||
EventName.laneChange: {
|
||||
ET.WARNING: Alert(
|
||||
"Changing Lane",
|
||||
"Monitor Other Vehicles",
|
||||
_("Changing Lane"),
|
||||
_("Monitor Other Vehicles"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1),
|
||||
},
|
||||
|
||||
EventName.steerSaturated: {
|
||||
ET.WARNING: Alert(
|
||||
"TAKE CONTROL",
|
||||
"Turn Exceeds Steering Limit",
|
||||
_("TAKE CONTROL"),
|
||||
_("Turn Exceeds Steering Limit"),
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 1., 1., 1.),
|
||||
},
|
||||
|
||||
EventName.fanMalfunction: {
|
||||
ET.PERMANENT: NormalPermanentAlert("Fan Malfunction", "Contact Support"),
|
||||
ET.PERMANENT: NormalPermanentAlert(_("Fan Malfunction"), _("Contact Support")),
|
||||
},
|
||||
|
||||
# ********** events that affect controls state transitions **********
|
||||
@@ -500,17 +505,17 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
|
||||
|
||||
EventName.brakeHold: {
|
||||
ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
|
||||
ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Brake Hold Active")),
|
||||
},
|
||||
|
||||
EventName.parkBrake: {
|
||||
ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
|
||||
ET.NO_ENTRY: NoEntryAlert("Park Brake Engaged"),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Park Brake Engaged")),
|
||||
},
|
||||
|
||||
EventName.pedalPressed: {
|
||||
ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
|
||||
ET.NO_ENTRY: NoEntryAlert("Pedal Pressed During Attempt",
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Pedal Pressed During Attempt"),
|
||||
visual_alert=VisualAlert.brakePressed),
|
||||
},
|
||||
|
||||
@@ -521,40 +526,40 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
|
||||
|
||||
EventName.wrongCruiseMode: {
|
||||
ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
|
||||
ET.NO_ENTRY: NoEntryAlert("Enable Adaptive Cruise"),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Enable Adaptive Cruise")),
|
||||
},
|
||||
|
||||
EventName.steerTempUnavailable: {
|
||||
ET.WARNING: Alert(
|
||||
"TAKE CONTROL",
|
||||
"Steering Temporarily Unavailable",
|
||||
_("TAKE CONTROL"),
|
||||
_("Steering Temporarily Unavailable"),
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.),
|
||||
ET.NO_ENTRY: NoEntryAlert("Steering Temporarily Unavailable",
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Steering Temporarily Unavailable"),
|
||||
duration_hud_alert=0.),
|
||||
},
|
||||
|
||||
EventName.outOfSpace: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Out of Storage",
|
||||
_("Out of Storage"),
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
ET.NO_ENTRY: NoEntryAlert("Out of Storage Space",
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Out of Storage Space"),
|
||||
duration_hud_alert=0.),
|
||||
},
|
||||
|
||||
EventName.belowEngageSpeed: {
|
||||
ET.NO_ENTRY: NoEntryAlert("Speed Too Low"),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Speed Too Low")),
|
||||
},
|
||||
|
||||
EventName.sensorDataInvalid: {
|
||||
ET.PERMANENT: Alert(
|
||||
"No Data from Device Sensors",
|
||||
"Reboot your Device",
|
||||
_("No Data from Device Sensors"),
|
||||
_("Reboot your Device"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2, creation_delay=1.),
|
||||
ET.NO_ENTRY: NoEntryAlert("No Data from Device Sensors"),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("No Data from Device Sensors")),
|
||||
},
|
||||
|
||||
EventName.noGps: {
|
||||
@@ -562,213 +567,283 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
|
||||
},
|
||||
|
||||
EventName.soundsUnavailable: {
|
||||
ET.PERMANENT: NormalPermanentAlert("Speaker not found", "Reboot your Device"),
|
||||
ET.PERMANENT: NormalPermanentAlert(_("Speaker not found"), _("Reboot your Device")),
|
||||
ET.NO_ENTRY: NoEntryAlert("Speaker not found"),
|
||||
},
|
||||
|
||||
EventName.tooDistracted: {
|
||||
ET.NO_ENTRY: NoEntryAlert("Distraction Level Too High"),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Distraction Level Too High")),
|
||||
},
|
||||
|
||||
EventName.overheat: {
|
||||
ET.PERMANENT: Alert(
|
||||
"System Overheated",
|
||||
_("System Overheated"),
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("System Overheated"),
|
||||
ET.NO_ENTRY: NoEntryAlert("System Overheated"),
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("System Overheated")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("System Overheated")),
|
||||
},
|
||||
|
||||
EventName.wrongGear: {
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Gear not D"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Gear not D"),
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("Gear not D")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Gear not D")),
|
||||
},
|
||||
|
||||
EventName.calibrationInvalid: {
|
||||
ET.PERMANENT: NormalPermanentAlert("Calibration Invalid", "Remount Device and Recalibrate"),
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Calibration Invalid: Remount Device & Recalibrate"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Calibration Invalid: Remount Device & Recalibrate"),
|
||||
ET.PERMANENT: NormalPermanentAlert(_("Calibration Invalid"), _("Remount Device and Recalibrate")),
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("Calibration Invalid: Remount Device & Recalibrate")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Calibration Invalid: Remount Device & Recalibrate")),
|
||||
},
|
||||
|
||||
EventName.calibrationIncomplete: {
|
||||
ET.PERMANENT: calibration_incomplete_alert,
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Calibration in Progress"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"),
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("Calibration in Progress")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Calibration in Progress")),
|
||||
},
|
||||
|
||||
EventName.doorOpen: {
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Door Open"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Door Open"),
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("Door Open")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Door Open")),
|
||||
},
|
||||
|
||||
EventName.seatbeltNotLatched: {
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Seatbelt Unlatched"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"),
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("Seatbelt Unlatched")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Seatbelt Unlatched")),
|
||||
},
|
||||
|
||||
EventName.espDisabled: {
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("ESP Off"),
|
||||
ET.NO_ENTRY: NoEntryAlert("ESP Off"),
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("ESP Off")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("ESP Off")),
|
||||
},
|
||||
|
||||
EventName.lowBattery: {
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Low Battery"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Low Battery"),
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("Low Battery")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Low Battery")),
|
||||
},
|
||||
|
||||
EventName.commIssue: {
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Communication Issue between Processes"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Communication Issue between Processes",
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("Communication Issue between Processes")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Communication Issue between Processes"),
|
||||
audible_alert=AudibleAlert.chimeDisengage),
|
||||
},
|
||||
|
||||
EventName.radarCommIssue: {
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Radar Communication Issue"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Radar Communication Issue",
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("Radar Communication Issue")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Radar Communication Issue"),
|
||||
audible_alert=AudibleAlert.chimeDisengage),
|
||||
},
|
||||
|
||||
EventName.radarCanError: {
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Radar Error: Restart the Car"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Radar Error: Restart the Car"),
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("Radar Error: Restart the Car")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Radar Error: Restart the Car")),
|
||||
},
|
||||
|
||||
EventName.radarFault: {
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Radar Error: Restart the Car"),
|
||||
ET.NO_ENTRY : NoEntryAlert("Radar Error: Restart the Car"),
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("Radar Error: Restart the Car")),
|
||||
ET.NO_ENTRY : NoEntryAlert(_("Radar Error: Restart the Car")),
|
||||
},
|
||||
|
||||
EventName.modeldLagging: {
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Driving model lagging"),
|
||||
ET.NO_ENTRY : NoEntryAlert("Driving model lagging"),
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("Driving model lagging")),
|
||||
ET.NO_ENTRY : NoEntryAlert(_("Driving model lagging")),
|
||||
},
|
||||
|
||||
EventName.posenetInvalid: {
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Model Output Uncertain"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Model Output Uncertain"),
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("Model Output Uncertain")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Model Output Uncertain")),
|
||||
},
|
||||
|
||||
EventName.deviceFalling: {
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Device Fell Off Mount"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Device Fell Off Mount"),
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("Device Fell Off Mount")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Device Fell Off Mount")),
|
||||
},
|
||||
|
||||
EventName.lowMemory: {
|
||||
ET.SOFT_DISABLE: SoftDisableAlert("Low Memory: Reboot Your Device"),
|
||||
ET.PERMANENT: NormalPermanentAlert("Low Memory", "Reboot your Device"),
|
||||
ET.NO_ENTRY : NoEntryAlert("Low Memory: Reboot Your Device",
|
||||
ET.SOFT_DISABLE: SoftDisableAlert(_("Low Memory: Reboot Your Device")),
|
||||
ET.PERMANENT: NormalPermanentAlert(_("Low Memory"), _("Reboot your Device")),
|
||||
ET.NO_ENTRY : NoEntryAlert(_("Low Memory: Reboot Your Device"),
|
||||
audible_alert=AudibleAlert.chimeDisengage),
|
||||
},
|
||||
|
||||
EventName.controlsFailed: {
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Failed"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Controls Failed"),
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert(_("Controls Failed")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Controls Failed")),
|
||||
},
|
||||
|
||||
EventName.controlsMismatch: {
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch"),
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert(_("Controls Mismatch")),
|
||||
},
|
||||
|
||||
EventName.canError: {
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN Error: Check Connections"),
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert(_("CAN Error: Check Connections")),
|
||||
ET.PERMANENT: Alert(
|
||||
"CAN Error: Check Connections",
|
||||
_("CAN Error: Check Connections"),
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, 0., 0., .2, creation_delay=1.),
|
||||
ET.NO_ENTRY: NoEntryAlert("CAN Error: Check Connections"),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("CAN Error: Check Connections")),
|
||||
},
|
||||
|
||||
EventName.steerUnavailable: {
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("LKAS Fault: Restart the Car"),
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert(_("LKAS Fault: Restart the Car")),
|
||||
ET.PERMANENT: Alert(
|
||||
"LKAS Fault: Restart the car to engage",
|
||||
_("LKAS Fault: Restart the car to engage"),
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
ET.NO_ENTRY: NoEntryAlert("LKAS Fault: Restart the Car"),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("LKAS Fault: Restart the Car")),
|
||||
},
|
||||
|
||||
EventName.brakeUnavailable: {
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Fault: Restart the Car"),
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert(_("Cruise Fault: Restart the Car")),
|
||||
ET.PERMANENT: Alert(
|
||||
"Cruise Fault: Restart the car to engage",
|
||||
_("Cruise Fault: Restart the car to engage"),
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Cruise Fault: Restart the Car")),
|
||||
},
|
||||
|
||||
EventName.reverseGear: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Reverse\nGear",
|
||||
_("Reverse\nGear"),
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2, creation_delay=0.5),
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Reverse Gear"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Reverse Gear"),
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert(_("Reverse Gear")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Reverse Gear")),
|
||||
},
|
||||
|
||||
EventName.cruiseDisabled: {
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Is Off"),
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert(_("Cruise Is Off")),
|
||||
},
|
||||
|
||||
EventName.plannerError: {
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Planner Solution Error"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Planner Solution Error"),
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert(_("Planner Solution Error")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Planner Solution Error")),
|
||||
},
|
||||
|
||||
EventName.relayMalfunction: {
|
||||
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Harness Malfunction"),
|
||||
ET.PERMANENT: NormalPermanentAlert("Harness Malfunction", "Check Hardware"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Harness Malfunction"),
|
||||
ET.PERMANENT: NormalPermanentAlert(_("Harness Malfunction"), _("Check Hardware")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Harness Malfunction")),
|
||||
},
|
||||
|
||||
EventName.noTarget: {
|
||||
ET.IMMEDIATE_DISABLE: Alert(
|
||||
"openpilot Canceled",
|
||||
"No close lead car",
|
||||
_("openpilot Canceled"),
|
||||
_("No close lead car"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
|
||||
ET.NO_ENTRY : NoEntryAlert("No Close Lead Car"),
|
||||
ET.NO_ENTRY : NoEntryAlert(_("No Close Lead Car")),
|
||||
},
|
||||
|
||||
EventName.speedTooLow: {
|
||||
ET.IMMEDIATE_DISABLE: Alert(
|
||||
"openpilot Canceled",
|
||||
"Speed too low",
|
||||
_("openpilot Canceled"),
|
||||
_("Speed too low"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
|
||||
},
|
||||
|
||||
EventName.speedTooHigh: {
|
||||
ET.WARNING: Alert(
|
||||
"Speed Too High",
|
||||
"Slow down to resume operation",
|
||||
_("Speed Too High"),
|
||||
_("Slow down to resume operation"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.none, 2.2, 3., 4.),
|
||||
ET.NO_ENTRY: Alert(
|
||||
"Speed Too High",
|
||||
"Slow down to engage",
|
||||
_("Speed Too High"),
|
||||
_("Slow down to engage"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
|
||||
},
|
||||
|
||||
# TODO: this is unclear, update check only happens offroad
|
||||
EventName.internetConnectivityNeeded: {
|
||||
ET.PERMANENT: NormalPermanentAlert("Connect to Internet", "An Update Check Is Required to Engage"),
|
||||
ET.NO_ENTRY: NoEntryAlert("Connect to Internet",
|
||||
ET.PERMANENT: NormalPermanentAlert(_("Connect to Internet"), _("An Update Check Is Required to Engage")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Connect to Internet"),
|
||||
audible_alert=AudibleAlert.chimeDisengage),
|
||||
},
|
||||
|
||||
EventName.lowSpeedLockout: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Cruise Fault: Restart the car to engage",
|
||||
_("Cruise Fault: Restart the car to engage"),
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Cruise Fault: Restart the Car")),
|
||||
},
|
||||
|
||||
# dp
|
||||
EventName.preLaneChangeLeftALC: {
|
||||
ET.WARNING: Alert(
|
||||
_("Left ALC will start in 3s"),
|
||||
_("Monitor Other Vehicles"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1, alert_rate=0.75),
|
||||
},
|
||||
|
||||
EventName.preLaneChangeRightALC: {
|
||||
ET.WARNING: Alert(
|
||||
_("Right ALC will start in 3s"),
|
||||
_("Monitor Other Vehicles"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1, alert_rate=0.75),
|
||||
},
|
||||
|
||||
EventName.manualSteeringRequired: {
|
||||
ET.WARNING: Alert(
|
||||
_("STEERING REQUIRED: Lane Keeping OFF"),
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, .0, .1, .1, alert_rate=0.25),
|
||||
},
|
||||
|
||||
EventName.manualSteeringRequiredBlinkersOn: {
|
||||
ET.WARNING: Alert(
|
||||
_("STEERING REQUIRED: Blinkers ON"),
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, .0, .1, .1, alert_rate=0.25),
|
||||
},
|
||||
|
||||
EventName.leadCarMoving: {
|
||||
ET.PERMANENT: Alert(
|
||||
_("Lead Car Is Moving"),
|
||||
"",
|
||||
AlertStatus.userPrompt, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, .1, .1, .1),
|
||||
ET.WARNING: Alert(
|
||||
_("Lead Car Is Moving"),
|
||||
"",
|
||||
AlertStatus.userPrompt, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, .1, .1, .1),
|
||||
},
|
||||
|
||||
# timebomb
|
||||
EventName.timebombWarn: {
|
||||
ET.WARNING: Alert(
|
||||
_("WARNING"),
|
||||
_("Grab wheel to start bypass"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.),
|
||||
},
|
||||
|
||||
EventName.timebombBypassing: {
|
||||
ET.WARNING: Alert(
|
||||
_("BYPASSING"),
|
||||
_("HOLD WHEEL"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.),
|
||||
},
|
||||
|
||||
EventName.timebombBypassed: {
|
||||
ET.WARNING: Alert(
|
||||
_("Bypassed!"),
|
||||
_("Release wheel when ready"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, 3., 2., 3.),
|
||||
},
|
||||
}
|
||||
|
||||
@@ -1,6 +1,8 @@
|
||||
from common.numpy_fast import interp
|
||||
import numpy as np
|
||||
from cereal import log
|
||||
from common.dp_common import get_last_modified, param_get_if_updated
|
||||
from common.dp_time import LAST_MODIFIED_LANE_PLANNER
|
||||
|
||||
CAMERA_OFFSET = 0.06 # m from center car to camera
|
||||
|
||||
@@ -52,9 +54,9 @@ class LanePlanner():
|
||||
self.p_poly = [0., 0., 0., 0.]
|
||||
self.d_poly = [0., 0., 0., 0.]
|
||||
|
||||
self.lane_width_estimate = 3.7
|
||||
self.lane_width_estimate = 2.85
|
||||
self.lane_width_certainty = 1.0
|
||||
self.lane_width = 3.7
|
||||
self.lane_width = 2.85
|
||||
|
||||
self.l_prob = 0.
|
||||
self.r_prob = 0.
|
||||
@@ -65,6 +67,13 @@ class LanePlanner():
|
||||
self._path_pinv = compute_path_pinv()
|
||||
self.x_points = np.arange(50)
|
||||
|
||||
# dp
|
||||
self.dp_camera_offset = CAMERA_OFFSET * 100
|
||||
self.last_modified_dp_camera_offset = None
|
||||
self.modified = None
|
||||
self.last_modified = None
|
||||
self.last_modified_check = None
|
||||
|
||||
def parse_model(self, md):
|
||||
if len(md.leftLane.poly):
|
||||
self.l_poly = np.array(md.leftLane.poly)
|
||||
@@ -83,14 +92,20 @@ class LanePlanner():
|
||||
|
||||
def update_d_poly(self, v_ego):
|
||||
# only offset left and right lane lines; offsetting p_poly does not make sense
|
||||
self.l_poly[3] += CAMERA_OFFSET
|
||||
self.r_poly[3] += CAMERA_OFFSET
|
||||
self.last_modified_check, self.modified = get_last_modified(LAST_MODIFIED_LANE_PLANNER, self.last_modified_check, self.modified)
|
||||
if self.last_modified != self.modified:
|
||||
self.dp_camera_offset, self.last_modified_dp_camera_offset = param_get_if_updated("dp_camera_offset", "int", self.dp_camera_offset, self.last_modified_dp_camera_offset)
|
||||
self.last_modified = self.modified
|
||||
offset = self.dp_camera_offset * 0.01 if self.dp_camera_offset != 0 else 0
|
||||
self.l_poly[3] += offset
|
||||
self.r_poly[3] += offset
|
||||
self.p_poly[3] += offset
|
||||
|
||||
# Find current lanewidth
|
||||
self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob - self.lane_width_certainty)
|
||||
current_lane_width = abs(self.l_poly[3] - self.r_poly[3])
|
||||
self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
|
||||
speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
|
||||
speed_lane_width = interp(v_ego, [0., 14., 20.], [2.5, 3., 3.5]) # German Standards
|
||||
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
|
||||
(1 - self.lane_width_certainty) * speed_lane_width
|
||||
|
||||
|
||||
@@ -7,6 +7,7 @@ from common.realtime import sec_since_boot
|
||||
from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
|
||||
from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
|
||||
from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
|
||||
from selfdrive.controls.lib.dynamic_follow import DynamicFollow
|
||||
|
||||
LOG_MPC = os.environ.get('LOG_MPC', False)
|
||||
|
||||
@@ -14,7 +15,7 @@ LOG_MPC = os.environ.get('LOG_MPC', False)
|
||||
class LongitudinalMpc():
|
||||
def __init__(self, mpc_id):
|
||||
self.mpc_id = mpc_id
|
||||
|
||||
self.dynamic_follow = DynamicFollow(mpc_id)
|
||||
self.setup_mpc()
|
||||
self.v_mpc = 0.0
|
||||
self.v_mpc_future = 0.0
|
||||
@@ -77,11 +78,13 @@ class LongitudinalMpc():
|
||||
self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau)
|
||||
self.new_lead = True
|
||||
|
||||
self.dynamic_follow.update_lead(v_lead, a_lead, x_lead, lead.status, self.new_lead)
|
||||
self.prev_lead_status = True
|
||||
self.prev_lead_x = x_lead
|
||||
self.cur_state[0].x_l = x_lead
|
||||
self.cur_state[0].v_l = v_lead
|
||||
else:
|
||||
self.dynamic_follow.update_lead(new_lead=self.new_lead)
|
||||
self.prev_lead_status = False
|
||||
# Fake a fast lead car, so mpc keeps running
|
||||
self.cur_state[0].x_l = 50.0
|
||||
@@ -91,7 +94,8 @@ class LongitudinalMpc():
|
||||
|
||||
# Calculate mpc
|
||||
t = sec_since_boot()
|
||||
n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
|
||||
TR = self.dynamic_follow.update(CS, self.libmpc) # update dynamic follow
|
||||
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)
|
||||
|
||||
if LOG_MPC:
|
||||
|
||||
@@ -82,7 +82,7 @@ class LongControl():
|
||||
|
||||
v_ego_pid = max(CS.vEgo, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
|
||||
|
||||
if self.long_control_state == LongCtrlState.off or CS.gasPressed:
|
||||
if self.long_control_state == LongCtrlState.off or CS.gasPressed or CS.brakePressed:
|
||||
self.reset(v_ego_pid)
|
||||
output_gb = 0.
|
||||
|
||||
|
||||
@@ -68,7 +68,7 @@ acadoWorkspace.evGu[lRun1 * 3 + 2] = acadoWorkspace.state[14];
|
||||
return ret;
|
||||
}
|
||||
|
||||
void acado_evaluateLSQ(const real_t* in, real_t* out)
|
||||
void acado_evaluateLSQ(const real_t* in, real_t* out, double TR)
|
||||
{
|
||||
const real_t* xd = in;
|
||||
const real_t* u = in + 3;
|
||||
@@ -78,29 +78,29 @@ real_t* a = acadoWorkspace.objAuxVar;
|
||||
|
||||
/* Compute intermediate quantities: */
|
||||
a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
|
||||
a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
|
||||
a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
|
||||
a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01)));
|
||||
a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
|
||||
a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
|
||||
a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]);
|
||||
a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
|
||||
a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
|
||||
a[7] = (a[6]*(real_t)(5.0000000000000000e-01));
|
||||
a[8] = (a[2]*a[2]);
|
||||
a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]);
|
||||
a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]);
|
||||
a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
|
||||
a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
|
||||
a[12] = (a[10]*a[10]);
|
||||
|
||||
/* Compute outputs: */
|
||||
out[0] = (a[1]-(real_t)(1.0000000000000000e+00));
|
||||
out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
|
||||
out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
|
||||
out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)));
|
||||
out[3] = (u[0]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)));
|
||||
out[4] = a[4];
|
||||
out[5] = a[9];
|
||||
out[6] = (real_t)(0.0000000000000000e+00);
|
||||
out[7] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]);
|
||||
out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12]));
|
||||
out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12]));
|
||||
out[9] = (real_t)(0.0000000000000000e+00);
|
||||
out[10] = (real_t)(0.0000000000000000e+00);
|
||||
out[11] = (xd[2]*(real_t)(1.0000000000000001e-01));
|
||||
@@ -114,7 +114,7 @@ out[18] = (real_t)(0.0000000000000000e+00);
|
||||
out[19] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00));
|
||||
}
|
||||
|
||||
void acado_evaluateLSQEndTerm(const real_t* in, real_t* out)
|
||||
void acado_evaluateLSQEndTerm(const real_t* in, real_t* out, double TR)
|
||||
{
|
||||
const real_t* xd = in;
|
||||
const real_t* od = in + 3;
|
||||
@@ -123,28 +123,28 @@ real_t* a = acadoWorkspace.objAuxVar;
|
||||
|
||||
/* Compute intermediate quantities: */
|
||||
a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
|
||||
a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
|
||||
a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
|
||||
a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01)));
|
||||
a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
|
||||
a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
|
||||
a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]);
|
||||
a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
|
||||
a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
|
||||
a[7] = (a[6]*(real_t)(5.0000000000000000e-01));
|
||||
a[8] = (a[2]*a[2]);
|
||||
a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]);
|
||||
a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]);
|
||||
a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
|
||||
a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
|
||||
a[12] = (a[10]*a[10]);
|
||||
|
||||
/* Compute outputs: */
|
||||
out[0] = (a[1]-(real_t)(1.0000000000000000e+00));
|
||||
out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
|
||||
out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
|
||||
out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)));
|
||||
out[3] = a[4];
|
||||
out[4] = a[9];
|
||||
out[5] = (real_t)(0.0000000000000000e+00);
|
||||
out[6] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]);
|
||||
out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12]));
|
||||
out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12]));
|
||||
out[8] = (real_t)(0.0000000000000000e+00);
|
||||
out[9] = (real_t)(0.0000000000000000e+00);
|
||||
out[10] = (xd[2]*(real_t)(1.0000000000000001e-01));
|
||||
@@ -207,7 +207,7 @@ tmpQN1[7] = + tmpQN2[6]*tmpFx[1] + tmpQN2[7]*tmpFx[4] + tmpQN2[8]*tmpFx[7];
|
||||
tmpQN1[8] = + tmpQN2[6]*tmpFx[2] + tmpQN2[7]*tmpFx[5] + tmpQN2[8]*tmpFx[8];
|
||||
}
|
||||
|
||||
void acado_evaluateObjective( )
|
||||
void acado_evaluateObjective( double TR )
|
||||
{
|
||||
int runObj;
|
||||
for (runObj = 0; runObj < 20; ++runObj)
|
||||
@@ -219,7 +219,7 @@ acadoWorkspace.objValueIn[3] = acadoVariables.u[runObj];
|
||||
acadoWorkspace.objValueIn[4] = acadoVariables.od[runObj * 2];
|
||||
acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 2 + 1];
|
||||
|
||||
acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
|
||||
acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR );
|
||||
acadoWorkspace.Dy[runObj * 4] = acadoWorkspace.objValueOut[0];
|
||||
acadoWorkspace.Dy[runObj * 4 + 1] = acadoWorkspace.objValueOut[1];
|
||||
acadoWorkspace.Dy[runObj * 4 + 2] = acadoWorkspace.objValueOut[2];
|
||||
@@ -235,7 +235,7 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[61];
|
||||
acadoWorkspace.objValueIn[2] = acadoVariables.x[62];
|
||||
acadoWorkspace.objValueIn[3] = acadoVariables.od[40];
|
||||
acadoWorkspace.objValueIn[4] = acadoVariables.od[41];
|
||||
acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
|
||||
acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR );
|
||||
|
||||
acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0];
|
||||
acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1];
|
||||
@@ -4589,12 +4589,12 @@ acado_multEDu( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVar
|
||||
acado_multEDu( &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 60 ]) );
|
||||
}
|
||||
|
||||
int acado_preparationStep( )
|
||||
int acado_preparationStep( double TR )
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = acado_modelSimulation();
|
||||
acado_evaluateObjective( );
|
||||
acado_evaluateObjective( TR );
|
||||
acado_condensePrep( );
|
||||
return ret;
|
||||
}
|
||||
@@ -4726,7 +4726,7 @@ kkt += fabs(acadoWorkspace.ubA[index] * prd);
|
||||
return kkt;
|
||||
}
|
||||
|
||||
real_t acado_getObjective( )
|
||||
real_t acado_getObjective( TR )
|
||||
{
|
||||
real_t objVal;
|
||||
|
||||
@@ -4746,7 +4746,7 @@ acadoWorkspace.objValueIn[3] = acadoVariables.u[lRun1];
|
||||
acadoWorkspace.objValueIn[4] = acadoVariables.od[lRun1 * 2];
|
||||
acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 2 + 1];
|
||||
|
||||
acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
|
||||
acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR );
|
||||
acadoWorkspace.Dy[lRun1 * 4] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 4];
|
||||
acadoWorkspace.Dy[lRun1 * 4 + 1] = acadoWorkspace.objValueOut[1] - acadoVariables.y[lRun1 * 4 + 1];
|
||||
acadoWorkspace.Dy[lRun1 * 4 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 4 + 2];
|
||||
@@ -4757,7 +4757,7 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[61];
|
||||
acadoWorkspace.objValueIn[2] = acadoVariables.x[62];
|
||||
acadoWorkspace.objValueIn[3] = acadoVariables.od[40];
|
||||
acadoWorkspace.objValueIn[4] = acadoVariables.od[41];
|
||||
acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
|
||||
acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR );
|
||||
acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0];
|
||||
acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1];
|
||||
acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2] - acadoVariables.yN[2];
|
||||
|
||||
@@ -29,8 +29,9 @@ def _get_libmpc(mpc_id):
|
||||
|
||||
void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost);
|
||||
void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l);
|
||||
void change_tr(double ttcCost, double distanceCost, double accelerationCost, double jerkCost);
|
||||
int run_mpc(state_t * x0, log_t * solution,
|
||||
double l, double a_l_0);
|
||||
double l, double a_l_0, double TR);
|
||||
""")
|
||||
|
||||
return (ffi, ffi.dlopen(libmpc_fn))
|
||||
|
||||
@@ -68,6 +68,25 @@ void init(double ttcCost, double distanceCost, double accelerationCost, double j
|
||||
|
||||
}
|
||||
|
||||
void change_tr(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){
|
||||
int i;
|
||||
const int STEP_MULTIPLIER = 3;
|
||||
|
||||
for (i = 0; i < N; i++) {
|
||||
int f = 1;
|
||||
if (i > 4){
|
||||
f = STEP_MULTIPLIER;
|
||||
}
|
||||
acadoVariables.W[16 * i + 0] = ttcCost * f; // exponential cost for time-to-collision (ttc)
|
||||
acadoVariables.W[16 * i + 5] = distanceCost * f; // desired distance
|
||||
acadoVariables.W[16 * i + 10] = accelerationCost * f; // acceleration
|
||||
acadoVariables.W[16 * i + 15] = jerkCost * f; // jerk
|
||||
}
|
||||
acadoVariables.WN[0] = ttcCost * STEP_MULTIPLIER; // exponential cost for danger zone
|
||||
acadoVariables.WN[4] = distanceCost * STEP_MULTIPLIER; // desired distance
|
||||
acadoVariables.WN[8] = accelerationCost * STEP_MULTIPLIER; // acceleration
|
||||
}
|
||||
|
||||
void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0, double l){
|
||||
int i;
|
||||
|
||||
@@ -112,7 +131,7 @@ void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0
|
||||
for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0;
|
||||
}
|
||||
|
||||
int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){
|
||||
int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0, double TR){
|
||||
// Calculate lead vehicle predictions
|
||||
int i;
|
||||
double t = 0.;
|
||||
@@ -152,7 +171,7 @@ int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){
|
||||
acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego;
|
||||
acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego;
|
||||
|
||||
acado_preparationStep();
|
||||
acado_preparationStep(TR);
|
||||
acado_feedbackStep();
|
||||
|
||||
for (i = 0; i <= N; i++){
|
||||
@@ -164,7 +183,7 @@ int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){
|
||||
solution->j_ego[i] = acadoVariables.u[i];
|
||||
}
|
||||
}
|
||||
solution->cost = acado_getObjective();
|
||||
solution->cost = acado_getObjective(TR);
|
||||
|
||||
// Dont shift states here. Current solution is closer to next timestep than if
|
||||
// we shift by 0.2 seconds.
|
||||
|
||||
@@ -62,6 +62,13 @@ class PathPlanner():
|
||||
self.lane_change_ll_prob = 1.0
|
||||
self.prev_one_blinker = False
|
||||
|
||||
# dp
|
||||
self.dragon_auto_lc_allowed = False
|
||||
self.dragon_auto_lc_timer = None
|
||||
self.dragon_auto_lc_delay = 2.
|
||||
self.dp_continuous_auto_lc = False
|
||||
self.dp_did_auto_lc = False
|
||||
|
||||
def setup_mpc(self):
|
||||
self.libmpc = libmpc_py.libmpc
|
||||
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost)
|
||||
@@ -99,7 +106,7 @@ class PathPlanner():
|
||||
|
||||
# Lane change logic
|
||||
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
|
||||
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
|
||||
below_lane_change_speed = v_ego < (sm['dragonConf'].dpAssistedLcMinMph * CV.MPH_TO_MS)
|
||||
|
||||
if sm['carState'].leftBlinker:
|
||||
self.lane_change_direction = LaneChangeDirection.left
|
||||
@@ -119,6 +126,34 @@ class PathPlanner():
|
||||
|
||||
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
|
||||
|
||||
# dp alc
|
||||
cur_time = sec_since_boot()
|
||||
if not below_lane_change_speed and sm['dragonConf'].dpAutoLc and v_ego >= (sm['dragonConf'].dpAutoLcMinMph * CV.MPH_TO_MS):
|
||||
# we allow auto lc when speed reached dragon_auto_lc_min_mph
|
||||
self.dragon_auto_lc_allowed = True
|
||||
else:
|
||||
# if too slow, we reset all the variables
|
||||
self.dragon_auto_lc_allowed = False
|
||||
self.dragon_auto_lc_timer = None
|
||||
|
||||
# disable auto lc when continuous is off and already did auto lc once
|
||||
if self.dragon_auto_lc_allowed and not sm['dragonConf'].dpAutoLcCont and self.dp_did_auto_lc:
|
||||
self.dragon_auto_lc_allowed = False
|
||||
|
||||
if self.dragon_auto_lc_allowed:
|
||||
if self.dragon_auto_lc_timer is None:
|
||||
# we only set timer when in preLaneChange state, dragon_auto_lc_delay delay
|
||||
if self.lane_change_state == LaneChangeState.preLaneChange:
|
||||
self.dragon_auto_lc_timer = cur_time + sm['dragonConf'].dpAutoLcDelay
|
||||
elif cur_time >= self.dragon_auto_lc_timer:
|
||||
# if timer is up, we set torque_applied to True to fake user input
|
||||
torque_applied = True
|
||||
self.dp_did_auto_lc = True
|
||||
|
||||
# we reset the timers when torque is applied regardless
|
||||
if torque_applied:
|
||||
self.dragon_auto_lc_timer = None
|
||||
|
||||
# State transitions
|
||||
# off
|
||||
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
|
||||
@@ -149,11 +184,17 @@ class PathPlanner():
|
||||
elif self.lane_change_ll_prob > 0.99:
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
|
||||
# dp when finishing, we reset timer to none.
|
||||
self.dragon_auto_lc_timer = None
|
||||
|
||||
if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]:
|
||||
self.lane_change_timer = 0.0
|
||||
else:
|
||||
self.lane_change_timer += DT_MDL
|
||||
|
||||
if self.prev_one_blinker and not one_blinker:
|
||||
self.dp_did_auto_lc = False
|
||||
|
||||
self.prev_one_blinker = one_blinker
|
||||
|
||||
desire = DESIRES[self.lane_change_direction][self.lane_change_state]
|
||||
@@ -202,7 +243,7 @@ class PathPlanner():
|
||||
plan_solution_valid = self.solution_invalid_cnt < 2
|
||||
|
||||
plan_send = messaging.new_message('pathPlan')
|
||||
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model'])
|
||||
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model', 'dragonConf'])
|
||||
plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
|
||||
plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly]
|
||||
plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly]
|
||||
@@ -219,6 +260,7 @@ class PathPlanner():
|
||||
plan_send.pathPlan.desire = desire
|
||||
plan_send.pathPlan.laneChangeState = self.lane_change_state
|
||||
plan_send.pathPlan.laneChangeDirection = self.lane_change_direction
|
||||
plan_send.pathPlan.dpALCAllowed = self.dragon_auto_lc_allowed
|
||||
|
||||
pm.send('pathPlan', plan_send)
|
||||
|
||||
|
||||
@@ -10,7 +10,7 @@ def apply_deadzone(error, deadzone):
|
||||
error = 0.
|
||||
return error
|
||||
|
||||
class PIController():
|
||||
class PIController:
|
||||
def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
|
||||
self._k_p = k_p # proportional gain
|
||||
self._k_i = k_i # integral gain
|
||||
@@ -86,3 +86,101 @@ class PIController():
|
||||
|
||||
self.control = clip(control, self.neg_limit, self.pos_limit)
|
||||
return self.control
|
||||
|
||||
|
||||
class PIDController:
|
||||
def __init__(self, k_p, k_i, k_d, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None):
|
||||
self.enable_long_derivative = False
|
||||
self._k_p = k_p # proportional gain
|
||||
self._k_i = k_i # integral gain
|
||||
self._k_d = k_d # derivative gain
|
||||
self.k_f = k_f # feedforward gain
|
||||
|
||||
self.max_accel_d = 0.22352 # 0.5 mph/s
|
||||
|
||||
self.pos_limit = pos_limit
|
||||
self.neg_limit = neg_limit
|
||||
|
||||
self.sat_count_rate = 1.0 / rate
|
||||
self.i_unwind_rate = 0.3 / rate
|
||||
self.rate = 1.0 / rate
|
||||
self.sat_limit = sat_limit
|
||||
self.convert = convert
|
||||
|
||||
self.reset()
|
||||
|
||||
@property
|
||||
def k_p(self):
|
||||
return interp(self.speed, self._k_p[0], self._k_p[1])
|
||||
|
||||
@property
|
||||
def k_i(self):
|
||||
return interp(self.speed, self._k_i[0], self._k_i[1])
|
||||
|
||||
@property
|
||||
def k_d(self):
|
||||
return interp(self.speed, self._k_d[0], self._k_d[1])
|
||||
|
||||
def _check_saturation(self, control, check_saturation, error):
|
||||
saturated = (control < self.neg_limit) or (control > self.pos_limit)
|
||||
|
||||
if saturated and check_saturation and abs(error) > 0.1:
|
||||
self.sat_count += self.sat_count_rate
|
||||
else:
|
||||
self.sat_count -= self.sat_count_rate
|
||||
|
||||
self.sat_count = clip(self.sat_count, 0.0, 1.0)
|
||||
|
||||
return self.sat_count > self.sat_limit
|
||||
|
||||
def reset(self):
|
||||
self.p = 0.0
|
||||
self.id = 0.0
|
||||
self.f = 0.0
|
||||
self.sat_count = 0.0
|
||||
self.saturated = False
|
||||
self.control = 0
|
||||
self.last_setpoint = 0.0
|
||||
self.last_error = 0.0
|
||||
|
||||
def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False):
|
||||
self.speed = speed
|
||||
|
||||
error = float(apply_deadzone(setpoint - measurement, deadzone))
|
||||
|
||||
self.p = error * self.k_p
|
||||
self.f = feedforward * self.k_f
|
||||
|
||||
if override:
|
||||
self.id -= self.i_unwind_rate * float(np.sign(self.id))
|
||||
else:
|
||||
i = self.id + error * self.k_i * self.rate
|
||||
control = self.p + self.f + i
|
||||
|
||||
if self.convert is not None:
|
||||
control = self.convert(control, speed=self.speed)
|
||||
|
||||
# Update when changing i will move the control away from the limits
|
||||
# or when i will move towards the sign of the error
|
||||
if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or \
|
||||
(error <= 0 and (control >= self.neg_limit or i > 0.0))) and \
|
||||
not freeze_integrator:
|
||||
self.id = i
|
||||
|
||||
if self.enable_long_derivative:
|
||||
if abs(setpoint - self.last_setpoint) / self.rate < self.max_accel_d: # if setpoint isn't changing much
|
||||
d = self.k_d * (error - self.last_error)
|
||||
if (self.id > 0 and self.id + d >= 0) or (self.id < 0 and self.id + d <= 0): # if changing integral doesn't make it cross zero
|
||||
self.id += d
|
||||
|
||||
control = self.p + self.f + self.id
|
||||
if self.convert is not None:
|
||||
control = self.convert(control, speed=self.speed)
|
||||
|
||||
self.saturated = self._check_saturation(control, check_saturation, error)
|
||||
|
||||
self.last_setpoint = float(setpoint)
|
||||
self.last_error = float(error)
|
||||
|
||||
self.control = clip(control, self.neg_limit, self.pos_limit)
|
||||
return self.control
|
||||
@@ -15,7 +15,10 @@ from selfdrive.controls.lib.fcw import FCWChecker
|
||||
from selfdrive.controls.lib.long_mpc import LongitudinalMpc
|
||||
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
|
||||
|
||||
MAX_SPEED = 255.0
|
||||
|
||||
LON_MPC_STEP = 0.2 # first step is 0.2s
|
||||
MAX_SPEED_ERROR = 2.0
|
||||
AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted
|
||||
|
||||
# lookup tables VS speed to determine min and max accels in cruise
|
||||
@@ -33,6 +36,46 @@ _A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.]
|
||||
_A_TOTAL_MAX_V = [1.7, 3.2]
|
||||
_A_TOTAL_MAX_BP = [20., 40.]
|
||||
|
||||
# 75th percentile
|
||||
SPEED_PERCENTILE_IDX = 7
|
||||
|
||||
# dp
|
||||
DP_OFF = 0
|
||||
DP_ECO = 1
|
||||
DP_NORMAL = 2
|
||||
DP_SPORT = 3
|
||||
# accel profile by @arne182
|
||||
_DP_CRUISE_MIN_V = [-2.0, -1.5, -1.0, -0.7, -0.5]
|
||||
_DP_CRUISE_MIN_V_ECO = [-1.0, -0.7, -0.6, -0.5, -0.3]
|
||||
_DP_CRUISE_MIN_V_SPORT = [-3.0, -2.6, -2.3, -2.0, -1.0]
|
||||
_DP_CRUISE_MIN_V_FOLLOWING = [-4.0, -4.0, -3.5, -2.5, -2.0]
|
||||
_DP_CRUISE_MIN_BP = [0.0, 5.0, 10.0, 20.0, 55.0]
|
||||
|
||||
_DP_CRUISE_MAX_V = [2.0, 2.0, 1.5, .5, .3]
|
||||
_DP_CRUISE_MAX_V_ECO = [0.8, 0.9, 1.0, 0.4, 0.2]
|
||||
_DP_CRUISE_MAX_V_SPORT = [3.0, 3.5, 3.0, 2.0, 2.0]
|
||||
_DP_CRUISE_MAX_V_FOLLOWING = [1.6, 1.4, 1.4, .7, .3]
|
||||
_DP_CRUISE_MAX_BP = [0., 5., 10., 20., 55.]
|
||||
|
||||
# Lookup table for turns
|
||||
_DP_TOTAL_MAX_V = [3.3, 3.0, 3.9]
|
||||
_DP_TOTAL_MAX_BP = [0., 25., 55.]
|
||||
|
||||
def dp_calc_cruise_accel_limits(v_ego, following, dp_profile):
|
||||
if following:
|
||||
a_cruise_min = interp(v_ego, _DP_CRUISE_MIN_BP, _DP_CRUISE_MIN_V_FOLLOWING)
|
||||
a_cruise_max = interp(v_ego, _DP_CRUISE_MAX_BP, _DP_CRUISE_MAX_V_FOLLOWING)
|
||||
else:
|
||||
if dp_profile == DP_ECO:
|
||||
a_cruise_min = interp(v_ego, _DP_CRUISE_MIN_BP, _DP_CRUISE_MIN_V_ECO)
|
||||
a_cruise_max = interp(v_ego, _DP_CRUISE_MAX_BP, _DP_CRUISE_MAX_V_ECO)
|
||||
elif dp_profile == DP_SPORT:
|
||||
a_cruise_min = interp(v_ego, _DP_CRUISE_MIN_BP, _DP_CRUISE_MIN_V_SPORT)
|
||||
a_cruise_max = interp(v_ego, _DP_CRUISE_MAX_BP, _DP_CRUISE_MAX_V_SPORT)
|
||||
else:
|
||||
a_cruise_min = interp(v_ego, _DP_CRUISE_MIN_BP, _DP_CRUISE_MIN_V)
|
||||
a_cruise_max = interp(v_ego, _DP_CRUISE_MAX_BP, _DP_CRUISE_MAX_V)
|
||||
return np.vstack([a_cruise_min, a_cruise_max])
|
||||
|
||||
def calc_cruise_accel_limits(v_ego, following):
|
||||
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
|
||||
@@ -80,9 +123,20 @@ class Planner():
|
||||
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 choose_solution(self, v_cruise_setpoint, enabled):
|
||||
if enabled:
|
||||
solutions = {'cruise': self.v_cruise}
|
||||
# dp - slow on curve from 0.7.6.1
|
||||
if self.dp_slow_on_curve:
|
||||
solutions = {'model': self.v_model, 'cruise': self.v_cruise}
|
||||
else:
|
||||
solutions = {'cruise': self.v_cruise}
|
||||
if self.mpc1.prev_lead_status:
|
||||
solutions['mpc1'] = self.mpc1.v_mpc
|
||||
if self.mpc2.prev_lead_status:
|
||||
@@ -101,6 +155,10 @@ class Planner():
|
||||
elif slowest == 'cruise':
|
||||
self.v_acc = self.v_cruise
|
||||
self.a_acc = self.a_cruise
|
||||
# dp - slow on curve from 0.7.6.1
|
||||
elif self.dp_slow_on_curve and 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])
|
||||
|
||||
@@ -122,9 +180,36 @@ class Planner():
|
||||
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
|
||||
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
|
||||
|
||||
# dp
|
||||
self.dp_profile = sm['dragonConf'].dpAccelProfile
|
||||
self.dp_slow_on_curve = sm['dragonConf'].dpSlowOnCurve
|
||||
|
||||
# dp - slow on curve from 0.7.6.1
|
||||
if self.dp_slow_on_curve and len(sm['model'].path.poly):
|
||||
path = list(sm['model'].path.poly)
|
||||
|
||||
# Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function
|
||||
# y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b
|
||||
# k = y'' / (1 + y'^2)^1.5
|
||||
# TODO: compute max speed without using a list of points and without numpy
|
||||
y_p = 3 * path[0] * self.path_x**2 + 2 * path[1] * self.path_x + path[2]
|
||||
y_pp = 6 * path[0] * self.path_x + 2 * path[1]
|
||||
curv = y_pp / (1. + y_p**2)**1.5
|
||||
|
||||
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
|
||||
v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
|
||||
model_speed = np.min(v_curvature)
|
||||
model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph
|
||||
else:
|
||||
model_speed = MAX_SPEED
|
||||
|
||||
# Calculate speed for normal cruise control
|
||||
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)]
|
||||
pedal_pressed = sm['carState'].gasPressed or sm['carState'].brakePressed
|
||||
if enabled and not self.first_loop and not pedal_pressed:
|
||||
if self.dp_profile == DP_OFF:
|
||||
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
|
||||
else:
|
||||
accel_limits = [float(x) for x in dp_calc_cruise_accel_limits(v_ego, following, self.dp_profile)]
|
||||
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning
|
||||
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
|
||||
|
||||
@@ -138,6 +223,12 @@ class Planner():
|
||||
accel_limits_turns[1], accel_limits_turns[0],
|
||||
jerk_limits[1], jerk_limits[0],
|
||||
LON_MPC_STEP)
|
||||
# dp - slow on curve from 0.7.6.1
|
||||
if self.dp_slow_on_curve:
|
||||
self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start,
|
||||
model_speed, 2*accel_limits[1],
|
||||
accel_limits[0], 2*jerk_limits[1], jerk_limits[0],
|
||||
LON_MPC_STEP)
|
||||
|
||||
# cruise speed can't be negative even is user is distracted
|
||||
self.v_cruise = max(self.v_cruise, 0.)
|
||||
|
||||
@@ -16,6 +16,7 @@ import numpy as np
|
||||
from numpy.linalg import solve
|
||||
from typing import Tuple
|
||||
from cereal import car
|
||||
from common.params import Params
|
||||
|
||||
|
||||
class VehicleModel:
|
||||
@@ -34,13 +35,17 @@ class VehicleModel:
|
||||
|
||||
self.cF_orig = CP.tireStiffnessFront
|
||||
self.cR_orig = CP.tireStiffnessRear
|
||||
# dp
|
||||
self.sR_orig = CP.steerRatio
|
||||
self.dp_sr_learner = Params().get('dp_sr_learner') == b'1'
|
||||
|
||||
self.update_params(1.0, CP.steerRatio)
|
||||
|
||||
def update_params(self, stiffness_factor: float, steer_ratio: float) -> None:
|
||||
"""Update the vehicle model with a new stiffness factor and steer ratio"""
|
||||
self.cF = stiffness_factor * self.cF_orig
|
||||
self.cR = stiffness_factor * self.cR_orig
|
||||
self.sR = steer_ratio
|
||||
self.sR = steer_ratio if self.dp_sr_learner else self.sR_orig
|
||||
|
||||
def steady_state_sol(self, sa: float, u: float) -> np.ndarray:
|
||||
"""Returns the steady state solution.
|
||||
|
||||
@@ -23,7 +23,7 @@ def plannerd_thread(sm=None, pm=None):
|
||||
VM = VehicleModel(CP)
|
||||
|
||||
if sm is None:
|
||||
sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters'],
|
||||
sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'dragonConf'],
|
||||
poll=['radarState', 'model'])
|
||||
|
||||
if pm is None:
|
||||
@@ -34,6 +34,10 @@ def plannerd_thread(sm=None, pm=None):
|
||||
sm['liveParameters'].steerRatio = CP.steerRatio
|
||||
sm['liveParameters'].stiffnessFactor = 1.0
|
||||
|
||||
# dp
|
||||
sm['dragonConf'].dpSlowOnCurve = False
|
||||
sm['dragonConf'].dpAccelProfile = 0
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
|
||||
@@ -164,7 +164,7 @@ class RadarD():
|
||||
radarState.radarErrors = list(rr.errors)
|
||||
radarState.controlsStateMonoTime = sm.logMonoTime['controlsState']
|
||||
|
||||
if enable_lead:
|
||||
if True:
|
||||
radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True)
|
||||
radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False)
|
||||
return dat
|
||||
|
||||
Reference in New Issue
Block a user