def __init__(self, CP):
self.kegman = KegmanConf(CP)
self.deadzone = float(self.kegman.conf['deadzone'])
self.pid = PIController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0,
sat_limit=CP.steerLimitTimer)
self.angle_steers_des = 0.
self.mpc_frame = 0
def __init__(self, CP):
self.kegman = KegmanConf()
self.trMode = int(self.kegman.conf['lastTrMode']
) # default to last distance interval on startup
#self.trMode = 1
self.lkMode = True
self.read_distance_lines_prev = 4
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
self.steer_status_values = defaultdict(
lambda: "UNKNOWN",
self.can_define.dv["STEER_STATUS"]["STEER_STATUS"])
self.user_gas, self.user_gas_pressed = 0., 0
self.brake_switch_prev = 0
self.brake_switch_ts = 0
self.lead_distance = 255
self.hud_lead = 0
self.cruise_buttons = 0
self.cruise_setting = 0
self.v_cruise_pcm_prev = 0
self.blinker_on = 0
self.left_blinker_on = 0
self.right_blinker_on = 0
self.cruise_mode = 0
self.stopped = 0
# vEgo kalman filter
dt = 0.01
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
# R = 1e3
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
A=[[1.0, dt], [0.0, 1.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0
def __init__(self, CP):
self.LP = LanePlanner()
self.last_cloudlog_t = 0
self.steer_rate_cost = CP.steerRateCost
self.setup_mpc()
self.solution_invalid_cnt = 0
self.path_offset_i = 0.0
self.mpc_frame = 0
self.sR_delay_counter = 0
self.steerRatio_new = 0.0
self.sR_time = 1
kegman = KegmanConf(CP)
if kegman.conf['steerRatio'] == "-1":
self.steerRatio = CP.steerRatio
else:
self.steerRatio = float(kegman.conf['steerRatio'])
if kegman.conf['steerRateCost'] == "-1":
self.steerRateCost = CP.steerRateCost
else:
self.steerRateCost = float(kegman.conf['steerRateCost'])
if kegman.conf['steerActuatorDelay'] == "-1":
self.steerActuatorDelay = CP.steerActuatorDelay
else:
self.steerActuatorDelay = float(kegman.conf['steerActuatorDelay'])
self.sR = [
float(kegman.conf['steerRatio']),
(float(kegman.conf['steerRatio']) + float(kegman.conf['sR_boost']))
]
self.sRBP = [
float(kegman.conf['sR_BP0']),
float(kegman.conf['sR_BP1'])
]
self.steerRateCost_prev = self.steerRateCost
self.setup_mpc()
self.alc_nudge_less = bool(int(kegman.conf['ALCnudgeLess']))
self.alc_min_speed = float(kegman.conf['ALCminSpeed'])
self.alc_timer = float(kegman.conf['ALCtimer'])
self.lane_change_state = LaneChangeState.off
self.lane_change_timer = 0.0
self.pre_lane_change_timer = 0.0
self.prev_one_blinker = False
def __init__(self, mpc_id):
self.mpc_id = mpc_id
self.setup_mpc()
self.v_mpc = 0.0
self.v_mpc_future = 0.0
self.a_mpc = 0.0
self.v_cruise = 0.0
self.prev_lead_status = False
self.prev_lead_x = 0.0
self.new_lead = False
self.v_rel = 0.0
self.lastTR = 2
self.last_cloudlog_t = 0.0
self.v_rel = 10
self.last_cloudlog_t = 0.0
self.bp_counter = 0
kegman = KegmanConf()
self.oneBarBP = [
float(kegman.conf['1barBP0']),
float(kegman.conf['1barBP1'])
]
self.twoBarBP = [
float(kegman.conf['2barBP0']),
float(kegman.conf['2barBP1'])
]
self.threeBarBP = [
float(kegman.conf['3barBP0']),
float(kegman.conf['3barBP1'])
]
self.oneBarProfile = [ONE_BAR_DISTANCE, float(kegman.conf['1barMax'])]
self.twoBarProfile = [TWO_BAR_DISTANCE, float(kegman.conf['2barMax'])]
self.threeBarProfile = [
THREE_BAR_DISTANCE,
float(kegman.conf['3barMax'])
]
self.oneBarHwy = [
ONE_BAR_DISTANCE, ONE_BAR_DISTANCE + float(kegman.conf['1barHwy'])
]
self.twoBarHwy = [
TWO_BAR_DISTANCE, TWO_BAR_DISTANCE + float(kegman.conf['2barHwy'])
]
self.threeBarHwy = [
THREE_BAR_DISTANCE,
THREE_BAR_DISTANCE + float(kegman.conf['3barHwy'])
]
def live_tune(self, CP):
self.mpc_frame += 1
if self.mpc_frame % 300 == 0:
# live tuning through /data/openpilot/tune.py overrides interface.py settings
self.kegman = KegmanConf(CP)
if self.kegman.conf['tuneGernby'] == "1":
self.steerKpV = [float(self.kegman.conf['Kp'])]
self.steerKiV = [float(self.kegman.conf['Ki'])]
self.steerKf = float(self.kegman.conf['Kf'])
self.pid = PIController(
(CP.lateralTuning.pid.kpBP, self.steerKpV),
(CP.lateralTuning.pid.kiBP, self.steerKiV),
k_f=self.steerKf,
pos_limit=1.0)
self.deadzone = float(self.kegman.conf['deadzone'])
self.mpc_frame = 0
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
self.kegman = KegmanConf()
self.mpc_frame = 0
from common.numpy_fast import interp
import numpy as np
from selfdrive.kegman_conf import KegmanConf
from cereal import log
kegman = KegmanConf()
CAMERA_OFFSET = float(kegman.conf['cameraOffset']) # m from center car to camera
#zorrobyte
def mean(numbers):
return float(sum(numbers)) / max(len(numbers), 1)
def compute_path_pinv(l=50):
deg = 3
x = np.arange(l*1.0)
X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T
pinv = np.linalg.pinv(X)
return pinv
def model_polyfit(points, path_pinv):
return np.dot(path_pinv, [float(x) for x in points])
def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
# This will improve behaviour when lanes suddenly widen
lane_width = min(3.5, lane_width)
l_prob = l_prob * interp(abs(l_poly[3]), [2, 2.5], [1.0, 0.0])
r_prob = r_prob * interp(abs(r_poly[3]), [2, 2.5], [1.0, 0.0])
def update(self, sm, pm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
if self.mpc_frame % 1000 == 0:
self.kegman = KegmanConf()
self.mpc_frame = 0
self.mpc_frame += 1
if len(sm['model'].path.poly) and int(self.kegman.conf['slowOnCurves']):
path = list(sm['model'].path.poly)
# Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function
# y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b
# k = y'' / (1 + y'^2)^1.5
# TODO: compute max speed without using a list of points and without numpy
y_p = 3 * path[0] * self.path_x**2 + 2 * path[1] * self.path_x + path[2]
y_pp = 6 * path[0] * self.path_x + 2 * path[1]
curv = y_pp / (1. + y_p**2)**1.5
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
model_speed = np.min(v_curvature)
model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph
else:
model_speed = MAX_SPEED
# Calculate speed for normal cruise control
if enabled:
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0],
jerk_limits[1], jerk_limits[0],
LON_MPC_STEP)
self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start,
model_speed,
2*accel_limits[1], accel_limits[0],
2*jerk_limits[1], jerk_limits[0],
LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint)
self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time,
sm['controlsState'].active,
v_ego, sm['carState'].aEgo,
lead_1.dRel, lead_1.vLead, lead_1.aLeadK,
lead_1.yRel, lead_1.vLat,
lead_1.fcw, blinkers) and not sm['carState'].brakePressed
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
radar_dead = not sm.alive['radarState']
radar_errors = list(sm['radarState'].radarErrors)
radar_fault = car.RadarData.Error.fault in radar_errors
radar_can_error = car.RadarData.Error.canError in radar_errors
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState'])
plan_send.plan.mdMonoTime = sm.logMonoTime['model']
plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
# longitudal plan
plan_send.plan.vCruise = float(self.v_cruise)
plan_send.plan.aCruise = float(self.a_cruise)
plan_send.plan.vStart = float(self.v_acc_start)
plan_send.plan.aStart = float(self.a_acc_start)
plan_send.plan.vTarget = float(self.v_acc)
plan_send.plan.aTarget = float(self.a_acc)
plan_send.plan.vTargetFuture = float(self.v_acc_future)
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(radar_can_error)
plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState']
# Send out fcw
plan_send.plan.fcw = fcw
pm.send('plan', plan_send)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
def update(self, cp, cp_cam):
# car params
v_weight_v = [
0., 1.
] # don't trust smooth speed at low values to avoid premature zero snapping
v_weight_bp = [
1., 6.
] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
# update prevs, update must run once per loop
self.prev_cruise_buttons = self.cruise_buttons
self.prev_blinker_on = self.blinker_on
self.prev_lead_distance = self.lead_distance
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
# ******************* parse out can *******************
if self.CP.carFingerprint in (
CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH,
CAR.INSIGHT): # TODO: find wheels moving bit in dbc
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
self.door_all_closed = not cp.vl["SCM_FEEDBACK"][
'DRIVERS_DOOR_OPEN']
self.lead_distance = cp.vl["RADAR_HUD"]['LEAD_DISTANCE']
elif self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CRV_HYBRID):
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
self.door_all_closed = not cp.vl["SCM_FEEDBACK"][
'DRIVERS_DOOR_OPEN']
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
self.door_all_closed = not cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN']
else:
self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING']
self.door_all_closed = not any([
cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'],
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']
])
self.seatbelt = not cp.vl["SEATBELT_STATUS"][
'SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"][
'SEATBELT_DRIVER_LATCHED']
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]
['STEER_STATUS']]
self.steer_error = steer_status not in [
'NORMAL', 'NO_TORQUE_ALERT_1', 'NO_TORQUE_ALERT_2',
'LOW_SPEED_LOCKOUT', 'TMP_FAULT'
]
# NO_TORQUE_ALERT_2 can be caused by bump OR steering nudge from driver
self.steer_not_allowed = steer_status not in [
'NORMAL', 'NO_TORQUE_ALERT_2'
]
# LOW_SPEED_LOCKOUT is not worth a warning
self.steer_warning = steer_status not in [
'NORMAL', 'LOW_SPEED_LOCKOUT', 'NO_TORQUE_ALERT_2'
]
if self.CP.radarOffCan:
self.brake_error = 0
else:
self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl[
"STANDSTILL"]['BRAKE_ERROR_2']
self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED']
# calc best v_ego estimate, by averaging two opposite corners
speed_factor = SPEED_FACTOR[self.CP.carFingerprint]
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl +
self.v_wheel_rr) / 4.
# blend in transmission speed at low speed, since it has more low speed accuracy
self.v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \
self.v_weight * v_wheel
if abs(
speed - self.v_ego
) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[speed], [0.0]]
self.v_ego_raw = speed
v_ego_x = self.v_ego_kf.update(speed)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
# this is a hack for the interceptor. This is now only used in the simulation
# TODO: Replace tests by toyota so this can go away
if self.CP.enableGasInterceptor:
self.user_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] +
cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl[
"GEARBOX"]['GEAR']
self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
#self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl[
"SCM_FEEDBACK"]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER']
self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G,
CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH,
CAR.CIVIC_BOSCH, CAR.INSIGHT,
CAR.CRV_HYBRID):
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
else:
self.park_brake = 0 # TODO
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER'])
self.gear_shifter = parse_gear_shifter(can_gear_shifter,
self.shifter_values)
self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS']
# crv doesn't include cruise control
if self.CP.carFingerprint in (CAR.CRV, CAR.ODYSSEY, CAR.ACURA_RDX,
CAR.RIDGELINE, CAR.PILOT_2019,
CAR.ODYSSEY_CHN):
self.car_gas = self.pedal_gas
else:
self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS']
self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']
self.steer_torque_motor = cp.vl["STEER_STATUS"]['STEER_TORQUE_MOTOR']
self.steer_override = abs(
self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint]
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
if self.CP.radarOffCan:
self.cruise_mode = cp.vl["ACC_HUD"]['CRUISE_CONTROL_LABEL']
self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego)
if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.ACCORDH,
CAR.INSIGHT, CAR.CRV_HYBRID):
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
(self.brake_switch and self.brake_switch_prev and \
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
self.brake_switch_prev = self.brake_switch
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
if self.CP.carFingerprint in (CAR.CIVIC_BOSCH):
self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']
else:
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
# On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
self.v_cruise_pcm = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"][
'CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED']
self.v_cruise_pcm_prev = self.v_cruise_pcm
else:
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.cruise_speed_offset = calc_cruise_offset(
cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], self.v_ego)
self.v_cruise_pcm = cp.vl["CRUISE"]['CRUISE_SPEED_PCM']
# brake switch has shown some single time step noise, so only considered when
# switch is on for at least 2 consecutive CAN samples
self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \
(self.brake_switch and self.brake_switch_prev and \
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
self.brake_switch_prev = self.brake_switch
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS']
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2018,
CAR.PILOT_2019, CAR.RIDGELINE):
if self.user_brake > 0.05:
self.brake_pressed = 1
# when user presses distance button on steering wheel
if self.cruise_setting == 3:
if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0:
self.trMode = (self.trMode + 1) % 4
self.kegman = KegmanConf()
self.kegman.conf['lastTrMode'] = str(
self.trMode) # write last distance bar setting to file
self.kegman.write_config(self.kegman.conf)
# when user presses LKAS button on steering wheel
if self.cruise_setting == 1:
if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0:
if self.lkMode:
self.lkMode = False
else:
self.lkMode = True
self.prev_cruise_setting = self.cruise_setting
self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
self.read_distance_lines = self.trMode + 1
if self.read_distance_lines != self.read_distance_lines_prev:
self.read_distance_lines_prev = self.read_distance_lines
# TODO: discover the CAN msg that has the imperial unit bit for all other cars
self.is_metric = not cp.vl["HUD_SETTING"][
'IMPERIAL_UNIT'] if self.CP.carFingerprint in (
CAR.CIVIC) else False
if self.CP.carFingerprint in HONDA_BOSCH:
self.stock_aeb = bool(
cp_cam.vl["ACC_CONTROL"]["AEB_STATUS"]
and cp_cam.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
else:
self.stock_aeb = bool(
cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"]
and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5)
if self.CP.carFingerprint in HONDA_BOSCH:
self.stock_hud = False
self.stock_fcw = False
else:
#self.stock_fcw = bool(cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0)
self.stock_fcw = False # Disable stock FCW because it's too bloody sensitive
self.stock_hud = cp_cam.vl["ACC_HUD"]
self.stock_brake = cp_cam.vl["BRAKE_COMMAND"]
def update(self, sm, pm, CP, VM):
v_ego = sm['carState'].vEgo
angle_steers = sm['carState'].steeringAngle
active = sm['controlsState'].active
angle_offset = sm['liveParameters'].angleOffset
# Run MPC
self.angle_steers_des_prev = self.angle_steers_des_mpc
VM.update_params(sm['liveParameters'].stiffnessFactor,
sm['liveParameters'].steerRatio)
curvature_factor = VM.curvature_factor(v_ego)
# Get steerRatio, steerRateCost, steerActuatorDelay from kegman.json every x seconds
self.mpc_frame += 1
if self.mpc_frame % 500 == 0:
# live tuning through /data/openpilot/tune.py overrides interface.py settings
kegman = KegmanConf()
if kegman.conf['tuneGernby'] == "1":
self.steerRateCost = float(kegman.conf['steerRateCost'])
if self.steerRateCost != self.steerRateCost_prev:
self.setup_mpc()
self.steerRateCost_prev = self.steerRateCost
self.steerActuatorDelay = float(
kegman.conf['steerActuatorDelay'])
self.sR = [
float(kegman.conf['steerRatio']),
(float(kegman.conf['steerRatio']) +
float(kegman.conf['sR_boost']))
]
self.sRBP = [
float(kegman.conf['sR_BP0']),
float(kegman.conf['sR_BP1'])
]
self.sR_time = int(float(kegman.conf['sR_time'])) * 100
self.mpc_frame = 0
if v_ego > 11.111:
# boost steerRatio by boost amount if desired steer angle is high
self.steerRatio_new = interp(abs(angle_steers), self.sRBP, self.sR)
self.sR_delay_counter += 1
if self.sR_delay_counter % self.sR_time != 0:
if self.steerRatio_new > self.steerRatio:
self.steerRatio = self.steerRatio_new
else:
self.steerRatio = self.steerRatio_new
self.sR_delay_counter = 0
else:
self.steerRatio = self.sR[0]
self.LP.parse_model(sm['model'])
# Lane change logic
lane_change_direction = LaneChangeDirection.none
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
if not active or self.lane_change_timer > 10.0:
self.lane_change_state = LaneChangeState.off
self.pre_lane_change_timer = 0.0
else:
if sm['carState'].leftBlinker:
lane_change_direction = LaneChangeDirection.left
self.pre_lane_change_timer += DT_MDL
elif sm['carState'].rightBlinker:
lane_change_direction = LaneChangeDirection.right
self.pre_lane_change_timer += DT_MDL
else:
self.pre_lane_change_timer = 0.0
if self.alc_nudge_less and self.pre_lane_change_timer > self.alc_timer:
torque_applied = True
else:
if lane_change_direction == LaneChangeDirection.left:
torque_applied = sm['carState'].steeringTorque > 0 and sm[
'carState'].steeringPressed
else:
torque_applied = sm['carState'].steeringTorque < 0 and sm[
'carState'].steeringPressed
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker:
self.lane_change_state = LaneChangeState.preLaneChange
# pre
elif self.lane_change_state == LaneChangeState.preLaneChange and not one_blinker:
self.lane_change_state = LaneChangeState.off
elif self.lane_change_state == LaneChangeState.preLaneChange and torque_applied:
self.lane_change_state = LaneChangeState.laneChangeStarting
# starting
elif self.lane_change_state == LaneChangeState.laneChangeStarting and lane_change_prob > 0.5:
self.lane_change_state = LaneChangeState.laneChangeFinishing
# finishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.2:
self.lane_change_state = LaneChangeState.preLaneChange
# Don't allow starting lane change below 45 mph
if (v_ego < self.alc_min_speed) and (
self.lane_change_state == LaneChangeState.preLaneChange):
self.lane_change_state = LaneChangeState.off
if self.lane_change_state in [
LaneChangeState.off, LaneChangeState.preLaneChange
]:
self.lane_change_timer = 0.0
else:
self.lane_change_timer += DT_MDL
self.prev_one_blinker = one_blinker
desire = DESIRES[lane_change_direction][self.lane_change_state]
# Turn off lanes during lane change
if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft:
self.LP.l_prob = 0.
self.LP.r_prob = 0.
self.libmpc.init_weights(MPC_COST_LAT.PATH / 10.0,
MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING,
self.steer_rate_cost)
else:
self.libmpc.init_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE,
MPC_COST_LAT.HEADING,
self.steer_rate_cost)
self.LP.update_d_poly(v_ego)
# TODO: Check for active, override, and saturation
# if active:
# self.path_offset_i += self.LP.d_poly[3] / (60.0 * 20.0)
# self.path_offset_i = clip(self.path_offset_i, -0.5, 0.5)
# self.LP.d_poly[3] += self.path_offset_i
# else:
# self.path_offset_i = 0.0
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego,
angle_steers - angle_offset,
curvature_factor,
self.steerRatio,
self.steerActuatorDelay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
list(self.LP.l_poly), list(self.LP.r_poly),
list(self.LP.d_poly), self.LP.l_prob,
self.LP.r_prob, curvature_factor, v_ego_mpc,
self.LP.lane_width)
# reset to current steer angle if not active or overriding
if active:
delta_desired = self.mpc_solution[0].delta[1]
rate_desired = math.degrees(self.mpc_solution[0].rate[0] *
self.steerRatio)
else:
delta_desired = math.radians(angle_steers -
angle_offset) / self.steerRatio
rate_desired = 0.0
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(
math.degrees(delta_desired * self.steerRatio) + angle_offset)
# Check for infeasable MPC solution
mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta)
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE,
MPC_COST_LAT.HEADING, self.steerRateCost)
self.cur_state[0].delta = math.radians(
angle_steers - angle_offset) / self.steerRatio
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if self.mpc_solution[
0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge
self.solution_invalid_cnt += 1
else:
self.solution_invalid_cnt = 0
plan_solution_valid = self.solution_invalid_cnt < 2
plan_send = messaging.new_message()
plan_send.init('pathPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=[
'carState', 'controlsState', 'liveParameters', 'model'
])
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]
plan_send.pathPlan.lProb = float(self.LP.l_prob)
plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly]
plan_send.pathPlan.rProb = float(self.LP.r_prob)
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
plan_send.pathPlan.rateSteers = float(rate_desired)
plan_send.pathPlan.angleOffset = float(
sm['liveParameters'].angleOffsetAverage)
plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid)
plan_send.pathPlan.posenetValid = bool(
sm['liveParameters'].posenetValid)
plan_send.pathPlan.desire = desire
plan_send.pathPlan.laneChangeState = self.lane_change_state
plan_send.pathPlan.laneChangeDirection = lane_change_direction
pm.send('pathPlan', plan_send)
if LOG_MPC:
dat = messaging.new_message()
dat.init('liveMpc')
dat.liveMpc.x = list(self.mpc_solution[0].x)
dat.liveMpc.y = list(self.mpc_solution[0].y)
dat.liveMpc.psi = list(self.mpc_solution[0].psi)
dat.liveMpc.delta = list(self.mpc_solution[0].delta)
dat.liveMpc.cost = self.mpc_solution[0].cost
pm.send('liveMpc', dat)
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
params = [
"Kp", "Ki", "Kf", "steerRatio", "steerRateCost", "steerActuatorDelay",
"deadzone", "sR_boost", "sR_BP0", "sR_BP1", "sR_time", "slowOnCurves",
"1barBP0", "1barBP1", "1barMax", "2barBP0", "2barBP1", "2barMax",
"3barBP0", "3barBP1", "3barMax", "1barHwy", "2barHwy", "3barHwy"
]
kegman = KegmanConf()
kegman.conf['tuneGernby'] = "1"
# kegman.write_config(kegman.conf)
j = 0
while True:
print("")
print(print_letters(params[j][0:9]))
print("")
print(print_letters(kegman.conf[params[j]]))
print("")
print("1,3,5,7,r to incr 0.1,0.05,0.01,0.001,0.00001")
print("a,d,g,j,v to decr 0.1,0.05,0.01,0.001,0.00001")
print("0 / L to make the value 0 / 1")
print("press SPACE / m for next /prev parameter")
print("press z to quit")
def update(self, pm, CS, lead, v_cruise_setpoint):
v_ego = CS.vEgo
# Setup current mpc state
self.cur_state[0].x_ego = 0.0
if lead is not None and lead.status:
x_lead = max(0, lead.dRel - STOPPING_DISTANCE
) # increase stopping distance to car by X [m]
v_lead = max(0.0, lead.vLead)
a_lead = lead.aLeadK
if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
v_lead = 0.0
a_lead = 0.0
self.a_lead_tau = max(lead.aLeadTau, (a_lead**2 * math.pi) /
(2 * (v_lead + 0.01)**2))
self.new_lead = False
if not self.prev_lead_status or abs(x_lead -
self.prev_lead_x) > 2.5:
self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead,
a_lead, self.a_lead_tau)
self.new_lead = True
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.prev_lead_status = False
# Fake a fast lead car, so mpc keeps running
self.cur_state[0].x_l = 50.0
self.cur_state[0].v_l = v_ego + 10.0
a_lead = 0.0
v_lead = 0.0
self.a_lead_tau = _LEAD_ACCEL_TAU
# Calculate conditions
self.v_rel = v_lead - v_ego # calculate relative velocity vs lead car
# Is the car running surface street speeds?
if v_ego < CITY_SPEED:
self.street_speed = 1
else:
self.street_speed = 0
# Live Tuning of breakpoints for braking profile change
self.bp_counter += 1
if self.bp_counter % 500 == 0:
kegman = KegmanConf()
self.oneBarBP = [
float(kegman.conf['1barBP0']),
float(kegman.conf['1barBP1'])
]
self.twoBarBP = [
float(kegman.conf['2barBP0']),
float(kegman.conf['2barBP1'])
]
self.threeBarBP = [
float(kegman.conf['3barBP0']),
float(kegman.conf['3barBP1'])
]
self.oneBarProfile = [
ONE_BAR_DISTANCE,
float(kegman.conf['1barMax'])
]
self.twoBarProfile = [
TWO_BAR_DISTANCE,
float(kegman.conf['2barMax'])
]
self.threeBarProfile = [
THREE_BAR_DISTANCE,
float(kegman.conf['3barMax'])
]
self.oneBarHwy = [
ONE_BAR_DISTANCE,
ONE_BAR_DISTANCE + float(kegman.conf['1barHwy'])
]
self.twoBarHwy = [
TWO_BAR_DISTANCE,
TWO_BAR_DISTANCE + float(kegman.conf['2barHwy'])
]
self.threeBarHwy = [
THREE_BAR_DISTANCE,
THREE_BAR_DISTANCE + float(kegman.conf['3barHwy'])
]
self.bp_counter = 0
# Calculate mpc
# Adjust distance from lead car when distance button pressed
if CS.readdistancelines == 1:
#if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
if self.street_speed:
TR = interp(-self.v_rel, self.oneBarBP, self.oneBarProfile)
else:
TR = interp(-self.v_rel, H_ONE_BAR_PROFILE_BP, self.oneBarHwy)
if CS.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, 1.0,
MPC_COST_LONG.ACCELERATION,
MPC_COST_LONG.JERK)
self.lastTR = CS.readdistancelines
elif CS.readdistancelines == 2:
#if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
if self.street_speed:
TR = interp(-self.v_rel, self.twoBarBP, self.twoBarProfile)
else:
TR = interp(-self.v_rel, H_TWO_BAR_PROFILE_BP, self.twoBarHwy)
if CS.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
MPC_COST_LONG.ACCELERATION,
MPC_COST_LONG.JERK)
self.lastTR = CS.readdistancelines
elif CS.readdistancelines == 3:
if self.street_speed:
#if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating):
TR = interp(-self.v_rel, self.threeBarBP, self.threeBarProfile)
else:
TR = interp(-self.v_rel, H_THREE_BAR_PROFILE_BP,
self.threeBarHwy)
if CS.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
MPC_COST_LONG.ACCELERATION,
MPC_COST_LONG.JERK)
self.lastTR = CS.readdistancelines
elif CS.readdistancelines == 4:
TR = FOUR_BAR_DISTANCE
if CS.readdistancelines != self.lastTR:
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
MPC_COST_LONG.ACCELERATION,
MPC_COST_LONG.JERK)
self.lastTR = CS.readdistancelines
else:
TR = TWO_BAR_DISTANCE # if readdistancelines != 1,2,3,4
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
t = sec_since_boot()
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:
self.send_mpc_solution(pm, n_its, duration)
# Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
self.v_mpc = self.mpc_solution[0].v_ego[1]
self.a_mpc = self.mpc_solution[0].a_ego[1]
self.v_mpc_future = self.mpc_solution[0].v_ego[10]
# Reset if NaN or goes through lead car
crashing = any(lead - ego < -50 for (
lead,
ego) in zip(self.mpc_solution[0].x_l, self.mpc_solution[0].x_ego))
nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego)
backwards = min(self.mpc_solution[0].v_ego) < -0.01
if ((backwards or crashing) and self.prev_lead_status) or nans:
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning(
"Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s"
% (self.mpc_id, backwards, crashing, nans))
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.cur_state[0].v_ego = v_ego
self.cur_state[0].a_ego = 0.0
self.v_mpc = v_ego
self.a_mpc = CS.aEgo
self.prev_lead_status = False