def __init__(self, CP, canbus):
# initialize can parser
self.car_fingerprint = CP.carFingerprint
self.cruise_buttons = CruiseButtons.UNPRESS
self.left_blinker_on = False
self.prev_left_blinker_on = False
self.right_blinker_on = False
self.prev_right_blinker_on = False
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Gm", "gm")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
# vEgo kalman filter
dt = 0.01
self.v_ego_kf = KF1D(x0=np.matrix([[0.], [0.]]),
A=np.matrix([[1., dt], [0., 1.]]),
C=np.matrix([1., 0.]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.
def __init__(self):
# labels for buttons
self.btns_init = [["alca", "ALC", ["MadMax", "Normal", "Calm"]],
[
PCCModes.BUTTON_NAME,
PCCModes.BUTTON_ABREVIATION,
PCCModes.labels()
], ["dsp", "DSP", ["OP", "MIN", "OFF", "GYRO"]],
["", "", [""]], ["msg", "MSG", [""]],
["sound", "SND", [""]]]
self.UE = UIEvents(self)
self.cstm_btns = UIButtons(self, "Tesla Model S", "tesla", False,
False)
self.custom_alert_counter = 0
for i in range(6):
print(self.cstm_btns.btns[i].btn_name,
len(self.cstm_btns.btns[i].btn_name))
print(self.cstm_btns.btns[i].btn_label,
len(self.cstm_btns.btns[i].btn_label))
print(self.cstm_btns.btns[i].btn_label2,
len(self.cstm_btns.btns[i].btn_label2))
print(self.cstm_btns.btns[i].btn_status)
class CarState(object):
def __init__(self, CP):
self.gasMode = int(kegman.conf['lastGasMode'])
self.gasLabels = ["dynamic", "sport", "eco"]
self.alcaLabels = ["MadMax", "Normal", "Wifey", "off"]
self.alcaMode = int(kegman.conf['lastALCAMode'])
steerRatio = CP.steerRatio
self.prev_distance_button = 0
self.distance_button = 0
self.prev_lka_button = 0
self.lka_button = 0
self.lkMode = True
self.lane_departure_toggle_on = True
# ALCA PARAMS
self.blind_spot_on = bool(0)
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 32., 55.]
self.CL_MAX_ANGLE_DELTA = [
0.77 * 16.2 / steerRatio, 0.86 * 16.2 / steerRatio,
0.535 * 16.2 / steerRatio
]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 2.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 4.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.040
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = [1., 1.1, 1.2]
self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.2, 0.2, 0.2]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.CP = CP
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Chrysler", "chrysler")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
self.left_blinker_on = 0
self.right_blinker_on = 0
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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
#BB init ui buttons
def init_ui_buttons(self):
btns = []
btns.append(UIButton("sound", "SND", 0, "", 0))
btns.append(
UIButton("alca", "ALC", 0, self.alcaLabels[self.alcaMode], 1))
btns.append(UIButton("mad", "MAD", 0, "", 2))
btns.append(UIButton("", "", 0, "", 3))
btns.append(UIButton("gas", "GAS", 1, self.gasLabels[self.gasMode], 4))
btns.append(UIButton("lka", "LKA", 1, "", 5))
return btns
#BB update ui buttons
def update_ui_buttons(self, id, btn_status):
if self.cstm_btns.btns[id].btn_status > 0:
if (id == 1) and (btn_status == 0
) and self.cstm_btns.btns[id].btn_name == "alca":
if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[
self.alcaMode]:
self.alcaMode = (self.alcaMode + 1) % 4
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
else:
self.alcaMode = 0
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[
self.alcaMode]
self.cstm_btns.hasChanges = True
if self.alcaMode == 3:
self.cstm_btns.set_button_status("alca", 0)
elif (id == 4) and (
btn_status
== 0) and self.cstm_btns.btns[id].btn_name == "gas":
if self.cstm_btns.btns[id].btn_label2 == self.gasLabels[
self.gasMode]:
self.gasMode = (self.gasMode + 1) % 3
kegman.save({'lastGasMode': int(self.gasMode)
}) # write last GasMode setting to file
else:
self.gasMode = 0
kegman.save({'lastGasMode': int(self.gasMode)
}) # write last GasMode setting to file
self.cstm_btns.btns[id].btn_label2 = self.gasLabels[
self.gasMode]
self.cstm_btns.hasChanges = True
else:
self.cstm_btns.btns[
id].btn_status = btn_status * self.cstm_btns.btns[
id].btn_status
else:
self.cstm_btns.btns[id].btn_status = btn_status
if (id == 1) and self.cstm_btns.btns[id].btn_name == "alca":
self.alcaMode = (self.alcaMode + 1) % 4
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[
self.alcaMode]
self.cstm_btns.hasChanges = True
def update(self, cp, cp_cam):
# copy can_valid
self.can_valid = cp.can_valid
# update prevs, update must run once per loop
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.frame_23b = int(cp.vl["WHEEL_BUTTONS"]['COUNTER'])
self.frame = int(cp.vl["EPS_STATUS"]['COUNTER'])
self.door_all_closed = not any([
cp.vl["DOORS"]['DOOR_OPEN_FL'], cp.vl["DOORS"]['DOOR_OPEN_FR'],
cp.vl["DOORS"]['DOOR_OPEN_RL'], cp.vl["DOORS"]['DOOR_OPEN_RR']
])
self.seatbelt = (
cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_UNLATCHED'] == 0)
self.brake_pressed = cp.vl["BRAKE_2"][
'BRAKE_PRESSED_2'] == 5 # human-only
self.pedal_gas = cp.vl["ACCEL_PEDAL_MSG"]['ACCEL_PEDAL']
self.car_gas = self.pedal_gas
self.esp_disabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1)
self.v_wheel_fl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FL']
self.v_wheel_rr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RR']
self.v_wheel_rl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RL']
self.v_wheel_fr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FR']
v_wheel = (cp.vl['SPEED_1']['SPEED_LEFT'] +
cp.vl['SPEED_1']['SPEED_RIGHT']) / 2.
# Kalman filter
if abs(
v_wheel - self.v_ego
) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
self.standstill = not v_wheel > 0.001
self.angle_steers = cp.vl["STEERING"]['STEER_ANGLE']
self.angle_steers_rate = cp.vl["STEERING"]['STEERING_RATE']
self.gear_shifter = parse_gear_shifter(cp.vl['GEAR']['PRNDL'])
self.main_on = cp.vl["ACC_2"]['ACC_STATUS_2'] == 7 # ACC is green.
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
self.steer_torque_driver = cp.vl["EPS_STATUS"]["TORQUE_DRIVER"]
self.steer_torque_motor = cp.vl["EPS_STATUS"]["TORQUE_MOTOR"]
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
steer_state = cp.vl["EPS_STATUS"]["LKAS_STATE"]
self.steer_error = steer_state == 4 or (
steer_state == 0 and self.v_ego > self.CP.minSteerSpeed)
self.user_brake = 0
self.brake_lights = self.brake_pressed
self.v_cruise_pcm = cp.vl["DASHBOARD"]['ACC_SPEED_CONFIG_KPH']
self.pcm_acc_status = self.main_on
self.generic_toggle = bool(cp.vl["STEERING_LEVERS"]['HIGH_BEAM_FLASH'])
self.lkas_counter = cp_cam.vl["LKAS_COMMAND"]['COUNTER']
self.lkas_car_model = cp_cam.vl["LKAS_HUD"]['CAR_MODEL']
self.lkas_status_ok = cp_cam.vl["LKAS_HEARTBIT"]['LKAS_STATUS_OK']
if self.cstm_btns.get_button_status("lka") == 0:
self.lane_departure_toggle_on = False
else:
if self.alcaMode == 3 and (self.left_blinker_on
or self.right_blinker_on):
self.lane_departure_toggle_on = False
else:
self.lane_departure_toggle_on = True
def __init__(self, CP):
self.Angle = [0, 5, 10, 15, 20, 25, 30, 35, 60, 100, 180, 270, 500]
self.Angle_Speed = [
255, 160, 100, 80, 70, 60, 55, 50, 40, 33, 27, 17, 12
]
#labels for ALCA modes
self.alcaLabels = ["MadMax", "Normal", "Wifey"]
self.alcaMode = 0
#if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 15., 32., 44.] #[10., 44.]
self.CL_MAX_ANGLE_DELTA = [2.0, 1.75, 0.96, 0.4]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.3, 1.3]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.042
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = [1.3, 1.2, 1.2]
self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7, 0.30, 0.20]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
context = zmq.Context()
#gps_ext_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, poller)
self.gps_location = messaging.sub_sock(
context, service_list['gpsLocationExternal'].port)
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
self.left_blinker_on = 0
self.right_blinker_on = 0
self.distance = 999
self.inaccuracy = 1.01
self.speedlimit = 25
self.approachradius = 100
self.includeradius = 22
self.blind_spot_on = bool(0)
self.distance_toggle_prev = 2
self.read_distance_lines_prev = 3
self.lane_departure_toggle_on_prev = True
self.acc_slow_on_prev = False
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Toyota", "toyota")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = 100 #set to 100 for 1 second display; carcontroller will take down to zero
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
class CarState(object):
def __init__(self, CP):
self.Angle = [0, 5, 10, 15, 20, 25, 30, 35, 60, 100, 180, 270, 500]
self.Angle_Speed = [
255, 160, 100, 80, 70, 60, 55, 50, 40, 33, 27, 17, 12
]
#labels for ALCA modes
self.alcaLabels = ["MadMax", "Normal", "Wifey"]
self.alcaMode = 0
#if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 15., 32., 44.] #[10., 44.]
self.CL_MAX_ANGLE_DELTA = [2.0, 1.75, 0.96, 0.4]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.3, 1.3]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.042
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = [1.3, 1.2, 1.2]
self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7, 0.30, 0.20]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
context = zmq.Context()
#gps_ext_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, poller)
self.gps_location = messaging.sub_sock(
context, service_list['gpsLocationExternal'].port)
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
self.left_blinker_on = 0
self.right_blinker_on = 0
self.distance = 999
self.inaccuracy = 1.01
self.speedlimit = 25
self.approachradius = 100
self.includeradius = 22
self.blind_spot_on = bool(0)
self.distance_toggle_prev = 2
self.read_distance_lines_prev = 3
self.lane_departure_toggle_on_prev = True
self.acc_slow_on_prev = False
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Toyota", "toyota")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = 100 #set to 100 for 1 second display; carcontroller will take down to zero
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
#BB init ui buttons
def init_ui_buttons(self):
btns = []
btns.append(UIButton("sound", "SND", 0, "", 0))
btns.append(
UIButton("alca", "ALC", 1, self.alcaLabels[self.alcaMode], 1))
btns.append(UIButton("slow", "SLO", 1, "", 2))
btns.append(UIButton("lka", "LKA", 1, "", 3))
btns.append(UIButton("tr", "TR", 0, "", 4))
btns.append(UIButton("gas", "GAS", 0, "", 5))
return btns
#BB update ui buttons
def update_ui_buttons(self, id, btn_status):
if self.cstm_btns.btns[id].btn_status > 0:
if (id == 1) and (btn_status == 0
) and self.cstm_btns.btns[id].btn_name == "alca":
if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[
self.alcaMode]:
self.alcaMode = (self.alcaMode + 1) % 3
else:
self.alcaMode = 0
self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[
self.alcaMode]
self.cstm_btns.hasChanges = True
else:
self.cstm_btns.btns[
id].btn_status = btn_status * self.cstm_btns.btns[
id].btn_status
else:
self.cstm_btns.btns[id].btn_status = btn_status
def update(self, cp, cp_cam):
# copy can_valid
self.can_valid = cp.can_valid
self.cam_can_valid = cp_cam.can_valid
msg = messaging.recv_one_or_none(self.gps_location)
if msg is not None:
gps_pkt = msg.gpsLocationExternal
self.inaccuracy = gps_pkt.accuracy
self.prev_distance = self.distance
self.distance, self.includeradius, self.approachradius, self.speedlimit = gps_distance(
gps_pkt.latitude, gps_pkt.longitude, gps_pkt.altitude,
gps_pkt.accuracy)
#self.distance = 6371010*acos(sin(radians(gps_pkt.latitude))*sin(radians(48.12893908))+cos(radians(gps_pkt.latitude))*cos(radians(48.12893908))*cos(radians(gps_pkt.longitude-9.797879048)))
# update prevs, update must run once per loop
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.door_all_closed = not any([
cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']
])
self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED']
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
if self.CP.enableGasInterceptor:
self.pedal_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS']
else:
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
self.car_gas = self.pedal_gas
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
self.v_wheel = float(
np.mean([
self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl,
self.v_wheel_rr
]))
# Kalman filter
if abs(
self.v_wheel - self.v_ego
) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_x = np.matrix([[self.v_wheel], [0.0]])
self.v_ego_raw = self.v_wheel
v_ego_x = self.v_ego_kf.update(self.v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
#self.standstill = not self.v_wheel > 0.001
self.standstill = False
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl[
"STEER_ANGLE_SENSOR"]['STEER_FRACTION']
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values)
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
self.blind_spot_side = cp.vl["DEBUG"]['BLINDSPOTSIDE']
if (cp.vl["DEBUG"]['BLINDSPOTD1'] >
10) or (cp.vl["DEBUG"]['BLINDSPOTD1'] > 10):
self.blind_spot_on = bool(1)
else:
self.blind_spot_on = bool(0)
# we could use the override bit from dbc, but it's triggered at too high torque values
self.steer_override = abs(
cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']) > 100
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [
1, 5, 9, 17, 25
]
self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
self.brake_error = 0
self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"][
'STEER_TORQUE_DRIVER']
self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"][
'STEER_TORQUE_EPS']
if bool(cp.vl["JOEL_ID"]
['LANE_WARNING']) <> self.lane_departure_toggle_on_prev:
self.lane_departure_toggle_on = bool(
cp.vl["JOEL_ID"]['LANE_WARNING'])
if self.lane_departure_toggle_on:
self.cstm_btns.set_button_status("lka", 1)
else:
self.cstm_btns.set_button_status("lka", 0)
self.lane_departure_toggle_on_prev = self.lane_departure_toggle_on
else:
if self.cstm_btns.get_button_status("lka") == 0:
self.lane_departure_toggle_on = False
else:
self.lane_departure_toggle_on = True
self.distance_toggle = cp.vl["JOEL_ID"]['ACC_DISTANCE']
self.read_distance_lines = cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES']
if self.distance_toggle <> self.distance_toggle_prev:
if self.read_distance_lines == self.distance_toggle:
self.distance_toggle_prev = self.distance_toggle
else:
self.cstm_btns.set_button_status("tr", 1)
if self.read_distance_lines <> self.read_distance_lines_prev:
self.cstm_btns.set_button_status("tr", 0)
if self.read_distance_lines == 1:
self.UE.custom_alert_message(2,
"Following distance set to 0.9s",
200, 3)
if self.read_distance_lines == 2:
self.UE.custom_alert_message(2,
"Following distance set to 1.8s",
200, 3)
if self.read_distance_lines == 3:
self.UE.custom_alert_message(2,
"Following distance set to 2.7s",
200, 3)
self.read_distance_lines_prev = self.read_distance_lines
if bool(cp.vl["JOEL_ID"]['ACC_SLOW']) <> self.acc_slow_on_prev:
self.acc_slow_on = bool(cp.vl["JOEL_ID"]['ACC_SLOW'])
if self.acc_slow_on:
self.cstm_btns.set_button_status("slow", 1)
else:
self.cstm_btns.set_button_status("slow", 0)
self.acc_slow_on_prev = self.acc_slow_on
else:
if self.cstm_btns.get_button_status("slow") == 0:
self.acc_slow_on = False
else:
self.acc_slow_on = True
# we could use the override bit from dbc, but it's triggered at too high torque values
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
self.user_brake = 0
if self.acc_slow_on:
self.v_cruise_pcm = max(7,
cp.vl["PCM_CRUISE_2"]['SET_SPEED'] - 34.0)
else:
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
self.v_cruise_pcm = int(
min(self.v_cruise_pcm,
interp(abs(self.angle_steers), self.Angle, self.Angle_Speed)))
#print "distane"
#print self.distance
if self.distance < self.approachradius + self.includeradius:
print "speed"
print self.prev_distance - self.distance
#if speed is 5% higher than the speedlimit
if self.prev_distance - self.distance > self.speedlimit * 0.00263889:
if self.v_cruise_pcm > self.speedlimit:
self.v_cruise_pcm = self.speedlimit
if self.distance < self.includeradius:
print "inside"
if self.v_cruise_pcm > self.speedlimit:
self.v_cruise_pcm = self.speedlimit
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
self.gas_pressed = not cp.vl["PCM_CRUISE"]['GAS_RELEASED']
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"][
'LOW_SPEED_LOCKOUT'] == 2
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC']
or self.brake_pressed)
if self.CP.carFingerprint == CAR.PRIUS:
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
else:
self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
class CarState(object):
def __init__(self, CP, canbus):
self.CP = CP
# initialize can parser
self.gasMode = int(kegman.conf['lastGasMode'])
self.gasLabels = ["dynamic", "sport", "eco"]
self.alcaLabels = ["MadMax", "Normal", "Wifey", "off"]
steerRatio = CP.steerRatio
self.alcaMode = int(
kegman.conf['lastALCAMode']) # default to last ALCAmode on startup
self.car_fingerprint = CP.carFingerprint
self.cruise_buttons = CruiseButtons.UNPRESS
self.prev_distance_button = 0
self.distance_button = 0
self.left_blinker_on = False
self.prev_left_blinker_on = False
self.right_blinker_on = False
self.prev_right_blinker_on = False
self.follow_level = int(kegman.conf['lastTrMode'])
self.prev_lka_button = 0
self.lka_button = 0
self.lkMode = True
# ALCA PARAMS
self.blind_spot_on = bool(0)
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 32., 44.]
self.CL_MAX_ANGLE_DELTA = [2.0, 1., 0.5]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 2.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 4.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 55.]
self.CL_MAXD_A = [
.358 * 15.7 / steerRatio, 0.084 * 15.7 / steerRatio,
0.040 * 15.7 / steerRatio
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = [1., 1.1, 1.2]
self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7, 0.30, 0.30]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.CP = CP
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Gm", "gm")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
# vEgo kalman filter
dt = 0.01
self.v_ego_kf = KF1D(x0=np.matrix([[0.], [0.]]),
A=np.matrix([[1., dt], [0., 1.]]),
C=np.matrix([1., 0.]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.
#BB init ui buttons
def init_ui_buttons(self):
btns = []
btns.append(UIButton("sound", "SND", 0, "", 0))
btns.append(
UIButton("alca", "ALC", 0, self.alcaLabels[self.alcaMode], 1))
btns.append(UIButton("stop", "", 1, "SNG", 2))
btns.append(UIButton("", "", 0, "", 3))
btns.append(UIButton("gas", "GAS", 1, self.gasLabels[self.gasMode], 4))
btns.append(UIButton("lka", "LKA", 1, "", 5))
return btns
#BB update ui buttons
def update_ui_buttons(self, id, btn_status):
if self.cstm_btns.btns[id].btn_status > 0:
if (id == 1) and (btn_status == 0
) and self.cstm_btns.btns[id].btn_name == "alca":
if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[
self.alcaMode]:
self.alcaMode = (self.alcaMode + 1) % 4
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
else:
self.alcaMode = 0
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[
self.alcaMode]
self.cstm_btns.hasChanges = True
if self.alcaMode == 3:
self.cstm_btns.set_button_status("alca", 0)
elif (id == 4) and (
btn_status
== 0) and self.cstm_btns.btns[id].btn_name == "gas":
if self.cstm_btns.btns[id].btn_label2 == self.gasLabels[
self.gasMode]:
self.gasMode = (self.gasMode + 1) % 3
kegman.save({'lastGasMode': int(self.gasMode)
}) # write last GasMode setting to file
else:
self.gasMode = 0
kegman.save({'lastGasMode': int(self.gasMode)
}) # write last GasMode setting to file
self.cstm_btns.btns[id].btn_label2 = self.gasLabels[
self.gasMode]
self.cstm_btns.hasChanges = True
else:
self.cstm_btns.btns[
id].btn_status = btn_status * self.cstm_btns.btns[
id].btn_status
else:
self.cstm_btns.btns[id].btn_status = btn_status
if (id == 1) and self.cstm_btns.btns[id].btn_name == "alca":
self.alcaMode = (self.alcaMode + 1) % 4
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[
self.alcaMode]
self.cstm_btns.hasChanges = True
def update(self, pt_cp):
self.can_valid = pt_cp.can_valid
self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons']
self.prev_distance_button = self.distance_button
self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"]
self.v_wheel_fl = pt_cp.vl["EBCMWheelSpdFront"][
'FLWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"][
'FRWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"][
'RLWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"][
'RRWheelSpd'] * CV.KPH_TO_MS
v_wheel = float(
np.mean([
self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl,
self.v_wheel_rr
]))
if abs(
v_wheel - self.v_ego
) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
self.prev_lka_button = self.lka_button
self.lka_button = pt_cp.vl["ASCMSteeringButton"]['LKAButton']
self.standstill = self.v_ego_raw < 0.01
self.angle_steers = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
self.gear_shifter = parse_gear_shifter(pt_cp.vl["ECMPRDNL"]['PRNDL'])
self.user_brake = pt_cp.vl["EBCMBrakePedalPosition"][
'BrakePedalPosition']
self.pedal_gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal']
self.user_gas_pressed = self.pedal_gas > 0
self.steer_torque_driver = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
# 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed
self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus']
self.steer_not_allowed = not is_eps_status_ok(self.lkas_status,
self.car_fingerprint)
# 1 - open, 0 - closed
self.door_all_closed = (
pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 0
and pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 0
and pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 0
and pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 0)
# 1 - latched
self.seatbelt = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 1
self.steer_error = False
self.brake_error = False
self.can_valid = True
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.left_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1
self.right_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2
if self.cstm_btns.get_button_status("lka") == 0:
self.lane_departure_toggle_on = False
else:
if self.alcaMode == 3 and (self.left_blinker_on
or self.right_blinker_on):
self.lane_departure_toggle_on = False
else:
self.lane_departure_toggle_on = True
if self.car_fingerprint in SUPERCRUISE_CARS:
self.park_brake = False
self.main_on = False
self.acc_active = pt_cp.vl["ASCMActiveCruiseControlStatus"][
'ACCCmdActive']
self.esp_disabled = False
self.regen_pressed = False
self.pcm_acc_status = int(self.acc_active)
else:
self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed']
self.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn']
self.acc_active = False
self.esp_disabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState']
if self.car_fingerprint == CAR.VOLT:
self.regen_pressed = bool(
pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
else:
self.regen_pressed = False
# Brake pedal's potentiometer returns near-zero reading
# even when pedal is not pressed.
if self.user_brake < 10:
self.user_brake = 0
# Regen braking is braking
self.brake_pressed = self.user_brake > 10 or self.regen_pressed
self.gear_shifter_valid = self.gear_shifter == car.CarState.GearShifter.drive
def get_follow_level(self):
return self.follow_level
def __init__(self, CP):
self.Angle = [0, 5, 10, 15,20,25,30,35,60,100,180,270,500]
self.Angle_Speed = [255,160,100,80,70,60,55,50,40,33,27,17,12]
#labels for ALCA modes
self.alcaLabels = ["MadMax","Normal","Wifey"]
self.alcaMode = 0
#if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 32., 44.]#[10., 44.]
self.CL_MAX_ANGLE_DELTA = [2.0, 0.96, 0.4]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16. , 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5. , 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.3, 1.3]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40.,10., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = [1.3,1.2,1.2]
self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7 ,0.30, 0.20]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.CP = CP
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self,"Hyundai","hyundai")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = 100 #set to 100 for 1 second display; carcontroller will take down to zero
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
self.left_blinker_on = 0
self.left_blinker_flash = 0
self.right_blinker_on = 0
self.right_blinker_flash = 0
def __init__(self, CP):
super().__init__(CP)
self.params = Params()
self.CL_MIN_V = 8.9
self.CL_MAX_A = 20.0
# labels for buttons
self.btns_init = [
["alca", "ALC", [""]],
[
ACCMode.BUTTON_NAME, ACCMode.BUTTON_ABREVIATION,
ACCMode.labels()
],
["dsp", "DSP", ["OP", "MIN", "OFF", "GYRO"]],
["", "", [""]],
["msg", "MSG", [""]],
["sound", "SND", [""]],
]
### START OF MAIN CONFIG OPTIONS ###
### Do NOT modify here, modify in /data/bb_openpilot.cfg and reboot
self.forcePedalOverCC = True
self.usesApillarHarness = False
self.enableHSO = True
self.enableALCA = True
self.enableDasEmulation = True
self.enableRadarEmulation = True
self.enableDriverMonitor = True
self.enableShowCar = True
self.enableShowLogo = True
self.hasNoctuaFan = False
self.limitBatteryMinMax = False
self.limitBattery_Min = 60
self.limitBattery_Max = 70
self.doAutoUpdate = True
self.blockUploadWhileTethering = False
self.tetherIP = "127.0.0."
self.useTeslaGPS = False
self.useTeslaMapData = False
self.hasTeslaIcIntegration = False
self.useTeslaRadar = False
self.useWithoutHarness = False
self.radarVIN = " "
self.enableLdw = True
self.radarOffset = 0.0
self.radarPosition = 0
self.radarEpasType = 0
self.fix1916 = False
self.forceFingerprintTesla = False
self.spinnerText = ""
self.hsoNumbPeriod = 1.5
self.ldwNumbPeriod = 1.5
self.tapBlinkerExtension = 2
self.ahbOffDuration = 5
self.roadCameraID = ""
self.driverCameraID = ""
self.roadCameraFx = 0.73
self.driverCameraFx = 0.75
self.roadCameraFlip = 0
self.driverCameraFlip = 0
self.monitorForcedRes = ""
# read config file
read_config_file(self)
### END OF MAIN CONFIG OPTIONS ###
self.apEnabled = True
self.apFollowTimeInS = 2.5 # time in seconds to follow
self.keepEonOff = False
self.alcaEnabled = True
self.mapAwareSpeed = False
# Tesla Model
self.teslaModelDetected = 1
self.teslaModel = self.params.get("TeslaModel")
if self.teslaModel:
self.teslaModel = self.teslaModel.decode()
if self.teslaModel is None:
self.teslaModel = "S"
self.teslaModelDetected = 0
# for map integration
self.csaRoadCurvC3 = 0.0
self.csaRoadCurvC2 = 0.0
self.csaRoadCurvRange = 0.0
self.csaRoadCurvUsingTspline = 0
self.csaOfframpCurvC3 = 0.0
self.csaOfframpCurvC2 = 0.0
self.csaOfframpCurvRange = 0.0
self.csaOfframpCurvUsingTspline = 0
self.roadCurvHealth = 0
self.roadCurvRange = 0.0
self.roadCurvC0 = 0.0
self.roadCurvC1 = 0.0
self.roadCurvC2 = 0.0
self.roadCurvC3 = 0.0
self.meanFleetSplineSpeedMPS = 0.0
self.UI_splineID = 0
self.meanFleetSplineAccelMPS2 = 0.0
self.medianFleetSpeedMPS = 0.0
self.topQrtlFleetSplineSpeedMPS = 0.0
self.splineLocConfidence = 0
self.baseMapSpeedLimitMPS = 0.0
self.bottomQrtlFleetSpeedMPS = 0.0
self.rampType = 0
self.mapBasedSuggestedSpeed = 0.0
self.splineBasedSuggestedSpeed = 0.0
self.map_suggested_speed = 0.0
self.gpsLongitude = 0.0
self.gpsLatitude = 0.0
self.gpsAccuracy = 0.0
self.gpsElevation = 0.0
self.gpsHeading = 0.0
self.gpsVehicleSpeed = 0.0
self.speed_limit_ms = 0.0
self.userSpeedLimitOffsetKph = 0.0
self.DAS_fusedSpeedLimit = 0
self.brake_only = CP.enableCruise
self.last_cruise_stalk_pull_time = 0
self.CP = CP
self.user_gas = 0.0
self.user_gas_pressed = False
self.pedal_interceptor_state = self.prev_pedal_interceptor_state = 0
self.pedal_interceptor_value = 0.0
self.pedal_interceptor_value2 = 0.0
self.pedal_idx = self.prev_pedal_idx = 0
self.brake_switch_prev = 0
self.brake_switch_ts = 0
self.cruise_buttons = 0
self.turn_signal_state_left = (
0 # 0 = off, 1 = on (blinking), 2 = failed, 3 = default
)
self.turn_signal_state_right = (
0 # 0 = off, 1 = on (blinking), 2 = failed, 3 = default
)
self.prev_turn_signal_blinking = False
self.turn_signal_blinking = False
self.prev_turn_signal_stalk_state = 0
self.turn_signal_stalk_state = (
0 # 0 = off, 1 = indicate left (stalk down), 2 = indicate right (stalk up)
)
self.steer_warning = 0
self.stopped = 0
# variables used for the fake DAS creation
self.DAS_info_frm = -1
self.DAS_info_msg = 0
self.DAS_status_frm = 0
self.DAS_status_idx = 0
self.DAS_status2_frm = 0
self.DAS_status2_idx = 0
self.DAS_bodyControls_frm = 0
self.DAS_bodyControls_idx = 0
self.DAS_lanes_frm = 0
self.DAS_lanes_idx = 0
self.DAS_objects_frm = 0
self.DAS_objects_idx = 0
self.DAS_pscControl_frm = 0
self.DAS_pscControl_idx = 0
self.DAS_warningMatrix0_idx = 0
self.DAS_warningMatrix1_idx = 0
self.DAS_warningMatrix3_idx = 0
self.DAS_telemetryPeriodic1_idx = 0
self.DAS_telemetryPeriodic2_idx = 0
self.DAS_telemetryEvent1_idx = 0
self.DAS_telemetryEvent2_idx = 0
self.DAS_control_idx = 0
# BB notification messages for DAS
self.DAS_canErrors = 0
self.DAS_plannerErrors = 0
self.DAS_doorOpen = 0
self.DAS_notInDrive = 0
self.summonButton = 0
# BB variables for pedal CC
self.pedal_speed_kph = 0.0
# BB UIEvents
self.UE = UIEvents(self)
# BB PCC
self.regenLight = 0
self.torqueLevel = 0.0
# BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Tesla Model S", "tesla",
self.enableShowLogo, self.enableShowCar)
# BB custom message counter
self.custom_alert_counter = (
-1
) # set to 100 for 1 second display; carcontroller will take down to zero
# BB steering_wheel_stalk last position, used by ACC and ALCA
self.steering_wheel_stalk = None
# BB carConfig data used to change IC info
self.real_carConfig = None
self.real_dasHw = 0
# BB visiond last type
self.last_visiond = self.cstm_btns.btns[3].btn_label2
# 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
self.imperial_speed_units = True
# The current max allowed cruise speed. Actual cruise speed sent to the car
# may be lower, depending on traffic.
self.v_cruise_pcm = 0.0
# Actual cruise speed currently active on the car.
self.v_cruise_actual = 0.0
# ALCA params
self.ALCA_enabled = False
self.ALCA_total_steps = 100
self.ALCA_direction = 0
self.ALCA_error = False
self.angle_offset = 0.0
self.init_angle_offset = False
# AHB params
self.ahbHighBeamStalkPosition = 0
self.ahbEnabled = 0
self.ahbLoBeamOn = 0
self.ahbHiBeamOn = 0
self.ahbNightMode = 0
def __init__(self, CP):
# labels for buttons
self.btns_init = [["alca", "ALC", ["MadMax", "Normal", "Calm"]],
[ACCMode.BUTTON_NAME, ACCMode.BUTTON_ABREVIATION, ACCMode.labels()],
["steer", "STR", [""]],
["brake", "BRK", [""]],
["msg", "MSG", [""]],
["sound", "SND", [""]]]
if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 44.]
self.CL_MAX_ANGLE_DELTA = [2.2, .3]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16. , 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5. , 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 1.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40.,10., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = 1.
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7 ,0.50, 0.40]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.brake_only = CP.enableCruise
self.last_cruise_stalk_pull_time = 0
self.CP = CP
self.user_gas = 0.
self.user_gas_pressed = False
self.pedal_interceptor_state = 0
self.pedal_interceptor_value = 0.
self.pedal_interceptor_value2 = 0.
self.pedal_interceptor_missed_counter = 0
self.brake_switch_prev = 0
self.brake_switch_ts = 0
self.cruise_buttons = 0
self.blinker_on = 0
self.left_blinker_on = 0
self.right_blinker_on = 0
self.steer_warning = 0
self.stopped = 0
self.frame_humanSteered = 0 # Last frame human steered
# variables used for the fake DAS creation
self.DAS_info_frm = -1
self.DAS_info_msg = 0
self.DAS_status_frm = 0
self.DAS_status_idx = 0
self.DAS_status2_frm = 0
self.DAS_status2_idx = 0
self.DAS_bodyControls_frm = 0
self.DAS_bodyControls_idx = 0
self.DAS_lanes_frm = 0
self.DAS_lanes_idx = 0
self.DAS_objects_frm = 0
self.DAS_objects_idx = 0
self.DAS_pscControl_frm = 0
self.DAS_pscControl_idx = 0
self.DAS_warningMatrix0_idx = 0
self.DAS_warningMatrix1_idx = 0
self.DAS_warningMatrix3_idx = 0
self.DAS_telemetryPeriodic1_idx = 0
self.DAS_telemetryPeriodic2_idx = 0
self.DAS_telemetryEvent1_idx = 0
self.DAS_telemetryEvent2_idx = 0
self.DAS_control_idx = 0
#BB variables for pedal CC
self.pedal_speed_kph = 0.
# Pedal mode is ready, i.e. hardware is present and normal cruise is off.
self.pedal_interceptor_available = False
self.prev_pedal_interceptor_available = False
#BB UIEvents
self.UE = UIEvents(self)
#BB PCC
self.regenLight = 0
self.torqueLevel = 0.
#BB variable for custom buttons
self.cstm_btns = UIButtons(self,"Tesla Model S","tesla")
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
#BB steering_wheel_stalk last position, used by ACC and ALCA
self.steering_wheel_stalk = None
#BB carConfig data used to change IC info
self.real_carConfig = None
self.real_dasHw = 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
self.imperial_speed_units = True
# The current max allowed cruise speed. Actual cruise speed sent to the car
# may be lower, depending on traffic.
self.v_cruise_pcm = 0.0
# Actual cruise speed currently active on the car.
self.v_cruise_actual = 0.0
class CarState(object):
def __init__(self, CP):
#labels for buttons
self.btns_init = [["","",["","",""]], \
["","",[""]], \
["alca","ALC",["MadMax", "Normal", "Wifey"]], \
["cam","CAM",[""]], \
["alwon", "MAD",[""]], \
["sound", "SND", [""]]]
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 44.]
self.CL_MAX_ANGLE_DELTA = [2.4, .3]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.7, .75]
self.CL_LANE_PASS_BP = [10., 44.]
self.CL_LANE_PASS_TIME = [60., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.042
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = 1.
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7, 0.4, 0.20]
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.CP = CP
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
self.left_blinker_on = 0
self.left_blinker_flash = 0
self.right_blinker_on = 0
self.right_blinker_flash = 0
self.has_scc = False
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Hyundai", "hyundai")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
def update(self, cp, cp_cam, cp_cam2):
# copy can_valid
self.can_valid = cp.can_valid
if (cp.vl["SCC11"]['TauGapSet'] > 0):
self.has_scc = True
# update prevs, update must run once per Loop
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.door_all_closed = True
self.seatbelt = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw']
self.brake_pressed = cp.vl["TCS13"][
'DriverBraking'] if not self.cstm_btns.get_button_status(
"alwon") else 0 # I'm Bad
self.esp_disabled = cp.vl["TCS15"]['ESC_Off_Step']
self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw']
self.main_on = True
if self.has_scc:
self.acc_active = (cp.vl["SCC12"]['ACCMode'] != 0) if not self.cstm_btns.get_button_status("alwon") else \
(cp.vl["SCC11"]["MainMode_ACC"] != 0) # I'm Dangerous!
self.acc_active_real = (cp.vl["SCC12"]['ACCMode'] != 0)
else:
self.acc_active = (cp.vl["LVR12"]['CF_Lvr_CruiseSet'] != 0) if not self.cstm_btns.get_button_status("alwon") else \
cp.vl['EMS16']['CRUISE_LAMP_M']
self.acc_active_real = self.acc_active
self.pcm_acc_status = int(self.acc_active)
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel_fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS
self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
self.v_wheel = 1.035 * (self.v_wheel_fl + self.v_wheel_fr +
self.v_wheel_rl + self.v_wheel_rr) / 4.
self.low_speed_lockout = self.v_wheel < 1.0
# Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default
if abs(
self.v_wheel - self.v_ego
) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_x = np.matrix([[self.v_wheel], [0.0]])
self.v_ego_raw = self.v_wheel
v_ego_x = self.v_ego_kf.update(self.v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
is_set_speed_in_mph = int(cp.vl["CLU11"]['CF_Clu_SPEED_UNIT'])
speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS
self.cruise_set_speed = (cp.vl["SCC11"]['VSetDis'] *
speed_conv) if self.has_scc else (
cp.vl["LVR12"]["CF_Lvr_CruiseSet"] *
speed_conv)
self.standstill = not self.v_wheel > 0.1
self.angle_steers = cp.vl["SAS11"]['SAS_Angle']
self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed']
self.yaw_rate = cp.vl["ESP12"]['YAW_RATE']
self.main_on = True
self.left_blinker_on = True if (
cp.vl["CGW1"]['CF_Gway_TSigLHSw']
== True) or (cp.vl["CGW1"]['CF_Gway_TurnSigLh'] == True) else False
self.left_blinker_flash = cp.vl["CGW1"]['CF_Gway_TurnSigLh']
self.right_blinker_on = True if (
cp.vl["CGW1"]['CF_Gway_TSigRHSw']
== True) or (cp.vl["CGW1"]['CF_Gway_TurnSigRh'] == True) else False
self.right_blinker_flash = cp.vl["CGW1"]['CF_Gway_TurnSigRh']
self.steer_override = abs(
cp.vl["MDPS12"]['CR_Mdps_StrColTq']
) > STEER_THRESHOLD or self.left_blinker_on or self.right_blinker_on
self.driver_steer_override = abs(
cp.vl["MDPS12"]['CR_Mdps_StrColTq']) > STEER_THRESHOLD
self.steer_state = cp.vl["MDPS12"][
'CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE
self.steer_error = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail']
self.brake_error = 0
self.steer_torque_driver = cp.vl["MDPS12"]['CR_Mdps_StrColTq']
self.steer_torque_motor = cp.vl["MDPS12"]['CR_Mdps_OutTq']
self.stopped = cp.vl["SCC11"][
'SCCInfoDisplay'] == 4. if self.has_scc else False
self.mdps11_strang = cp.vl["MDPS11"]["CR_Mdps_StrAng"]
self.mdps11_stat = cp.vl["MDPS11"]["CF_Mdps_Stat"]
self.user_brake = 0
self.brake_lights = bool(self.brake_pressed)
if (cp.vl["TCS13"]["DriverOverride"] == 0) and (
cp.vl["TCS13"]['ACC_REQ'] == 1 if self.has_scc else True):
self.pedal_gas = 0
else:
self.pedal_gas = cp.vl["EMS12"]['TPS']
self.car_gas = cp.vl["EMS12"]['TPS']
# Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
if gear == 5:
self.gear_shifter = "drive"
elif gear == 6:
self.gear_shifter = "neutral"
elif gear == 0:
self.gear_shifter = "park"
elif gear == 7:
self.gear_shifter = "reverse"
else:
self.gear_shifter = "unknown"
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection, as this seems to be standard over all cars, but is not the preferred method.
if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1:
self.gear_shifter_cluster = "drive"
elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1:
self.gear_shifter_cluster = "neutral"
elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1:
self.gear_shifter_cluster = "park"
elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
self.gear_shifter_cluster = "reverse"
else:
self.gear_shifter_cluster = "unknown"
# Gear Selecton via TCU12
gear2 = cp.vl["TCU12"]["CUR_GR"]
if gear2 == 0:
self.gear_tcu = "park"
elif gear2 == 14:
self.gear_tcu = "reverse"
elif gear2 > 0 and gear2 < 9: # unaware of anything over 8 currently
self.gear_tcu = "drive"
else:
self.gear_tcu = "unknown"
# save the entire LKAS11, CLU11 and MDPS12
if cp_cam.can_valid == True:
self.lkas11 = cp_cam.vl["LKAS11"]
self.camcan = 2
elif cp_cam2.can_valid == True:
self.lkas11 = cp_cam2.vl["LKAS11"]
self.camcan = 1
else:
self.camcan = 0
self.clu11 = cp.vl["CLU11"]
self.mdps12 = cp.vl["MDPS12"]
# Lane Change Assist Messages
self.lca_left = cp.vl["LCA11"]["CF_Lca_IndLeft"]
self.lca_right = cp.vl["LCA11"]["CF_Lca_IndRight"]
def __init__(self, CP):
#labels for buttons
self.btns_init = [["alca","ALC",["MadMax","Normal","Wifey"]], \
["sound","SND",[""]], \
["","",[""]], \
["","",[""]], \
["", "",[""]], \
["", "", [""]]]
#if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 44.]
self.CL_MAX_ANGLE_DELTA = [1.8, .3]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 1.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.11, 0.072
] # [.358, 0.084, 0.042]#delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = 1.
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7, 0.50, 0.40] #tesla parameters
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 0.1
#END OF ALCA PARAMS
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
self.left_blinker_on = 0
self.right_blinker_on = 0
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Toyota", "toyota")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
#init variable for safety shutoff
self.enabled_time = 0
self.desired_angle = 0.
self.current_angle = 0.
class CarState(object):
def __init__(self, CP):
self.brakefactor = float(kegman.conf['brakefactor'])
self.trfix = False
self.indi_toggle = False
steerRatio = CP.steerRatio
self.Angles = np.zeros(250)
self.Angles_later = np.zeros(250)
self.Angle_counter = 0
self.Angle = [0, 5, 10, 15, 20, 25, 30, 35, 60, 100, 180, 270, 500]
self.Angle_Speed = [
255, 160, 100, 80, 70, 60, 55, 50, 40, 33, 27, 17, 12
]
#labels for gas mode
self.gasMode = int(kegman.conf['lastGasMode'])
self.sloMode = int(kegman.conf['lastSloMode'])
self.sloLabels = ["offset", "normal"]
self.gasLabels = ["dynamic", "sport", "eco"]
#labelslabels for ALCA modes
self.alcaLabels = ["MadMax", "Normal", "Wifey", "off"]
self.alcaMode = int(
kegman.conf['lastALCAMode']) # default to last ALCAmode on startup
#if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 15., 32., 55.] #[10., 44.]
self.CL_MAX_ANGLE_DELTA = [
2.0 * 15.5 / steerRatio, 1.75 * 15.5 / steerRatio,
1.25 * 15.5 / steerRatio, 0.5 * 15.5 / steerRatio
]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 50.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 50.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 50.]
self.CL_LANE_DETECT_FACTOR = [1.0, 0.5]
self.CL_LANE_PASS_BP = [10., 20., 50.]
self.CL_LANE_PASS_TIME = [40., 10., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 50.]
self.CL_MAXD_A = [
.358, 0.084, 0.042
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 50.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = [1.3, 1.1, 1.05]
self.CL_CORRECTION_FACTOR_BP = [10., 32., 50.]
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 50.]
self.CL_TIMEA_T = [0.7, 0.30, 0.20]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
context = zmq.Context()
self.poller = zmq.Poller()
self.lastlat_Control = None
#gps_ext_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, poller)
self.gps_location = messaging.sub_sock(
context,
service_list['gpsLocationExternal'].port,
conflate=True,
poller=self.poller)
self.lat_Control = messaging.sub_sock(context,
service_list['latControl'].port,
conflate=True,
poller=self.poller)
self.live_MapData = messaging.sub_sock(
context,
service_list['liveMapData'].port,
conflate=True,
poller=self.poller)
self.traffic_data_sock = messaging.pub_sock(
context, service_list['liveTrafficData'].port)
self.spdval1 = 0
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
self.left_blinker_on = 0
self.right_blinker_on = 0
self.lkas_barriers = 0
self.left_line = 1
self.right_line = 1
self.distance = 999
self.inaccuracy = 1.01
self.speedlimit = 25
self.approachradius = 100
self.includeradius = 22
self.blind_spot_on = bool(0)
self.econ_on = 0
self.sport_on = 0
self.distance_toggle_prev = 2
self.read_distance_lines_prev = 3
self.lane_departure_toggle_on_prev = True
self.acc_slow_on_prev = False
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Toyota", "toyota")
if self.CP.carFingerprint == CAR.PRIUS:
self.alcaMode = 3
self.cstm_btns.set_button_status("alca", 0)
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = 100 #set to 100 for 1 second display; carcontroller will take down to zero
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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
#BB init ui buttons
def init_ui_buttons(self):
btns = []
btns.append(UIButton("sound", "SND", 0, "", 0))
btns.append(
UIButton("alca", "ALC", 0, self.alcaLabels[self.alcaMode], 1))
btns.append(
UIButton("slow", "SLO", self.sloMode, self.sloLabels[self.sloMode],
2))
btns.append(UIButton("lka", "LKA", 1, "", 3))
btns.append(UIButton("tr", "TR", 0, "", 4))
btns.append(UIButton("gas", "GAS", 1, self.gasLabels[self.gasMode], 5))
return btns
#BB update ui buttons
def update_ui_buttons(self, id, btn_status):
if self.cstm_btns.btns[id].btn_status > 0:
if (id == 1) and (btn_status == 0
) and self.cstm_btns.btns[id].btn_name == "alca":
if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[
self.alcaMode]:
self.alcaMode = (self.alcaMode + 1) % 4
if self.CP.carFingerprint == CAR.PRIUS:
self.alcaMode = 3
self.cstm_btns.set_button_status("alca", 0)
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last ALCAMode setting to file
else:
self.alcaMode = 0
if self.CP.carFingerprint == CAR.PRIUS:
self.alcaMode = 3
self.cstm_btns.set_button_status("alca", 0)
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last ALCAMode setting to file
self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[
self.alcaMode]
self.cstm_btns.hasChanges = True
if self.CP.carFingerprint == CAR.PRIUS:
self.alcaMode = 3
if self.alcaMode == 3:
self.cstm_btns.set_button_status("alca", 0)
elif (id == 2) and (
btn_status
== 0) and self.cstm_btns.btns[id].btn_name == "slow":
if self.cstm_btns.btns[id].btn_label2 == self.sloLabels[
self.sloMode]:
self.sloMode = (self.sloMode + 1) % 2
kegman.save({'lastSloMode': int(self.sloMode)
}) # write last SloMode setting to file
else:
self.sloMode = 0
kegman.save({'lastSloMode': int(self.sloMode)
}) # write last SloMode setting to file
self.cstm_btns.btns[id].btn_label2 = self.sloLabels[
self.sloMode]
self.cstm_btns.hasChanges = True
if self.sloMode == 0:
self.cstm_btns.set_button_status(
"slow", 0) # this might not be needed
elif (id == 5) and (
btn_status
== 0) and self.cstm_btns.btns[id].btn_name == "gas":
if self.cstm_btns.btns[id].btn_label2 == self.gasLabels[
self.gasMode]:
self.gasMode = (self.gasMode + 1) % 3
kegman.save({'lastGasMode': int(self.gasMode)
}) # write last GasMode setting to file
else:
self.gasMode = 0
kegman.save({'lastGasMode': int(self.gasMode)
}) # write last GasMode setting to file
self.cstm_btns.btns[id].btn_label2 = self.gasLabels[
self.gasMode]
self.cstm_btns.hasChanges = True
else:
self.cstm_btns.btns[
id].btn_status = btn_status * self.cstm_btns.btns[
id].btn_status
else:
self.cstm_btns.btns[id].btn_status = btn_status
if (id == 1) and self.cstm_btns.btns[id].btn_name == "alca":
self.alcaMode = (self.alcaMode + 1) % 4
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last ALCAMode setting to file
self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[
self.alcaMode]
self.cstm_btns.hasChanges = True
elif (id == 2) and self.cstm_btns.btns[id].btn_name == "slow":
self.sloMode = (self.sloMode + 1) % 2
kegman.save({'lastSloMode': int(self.sloMode)
}) # write last SloMode setting to file
self.cstm_btns.btns[id].btn_label2 = self.sloLabels[
self.sloMode]
self.cstm_btns.hasChanges = True
def update(self, cp, cp_cam):
# copy can_valid
self.can_valid = cp.can_valid
self.cam_can_valid = cp_cam.can_valid
msg = None
#lastspeedlimit = None
lastlive_MapData = None
for socket, event in self.poller.poll(0):
if socket is self.gps_location:
msg = messaging.recv_one(socket)
elif socket is self.lat_Control:
self.lastlat_Control = messaging.recv_one(socket).latControl
elif socket is self.live_MapData:
lastlive_MapData = messaging.recv_one(socket).liveMapData
if lastlive_MapData is not None:
if lastlive_MapData.speedLimitValid:
self.lastspeedlimit = lastlive_MapData.speedLimit
self.lastspeedlimitvalid = True
else:
self.lastspeedlimitvalid = False
if self.CP.carFingerprint == CAR.PRIUS:
self.alcaMode = 3
if msg is not None:
gps_pkt = msg.gpsLocationExternal
self.inaccuracy = gps_pkt.accuracy
self.prev_distance = self.distance
self.distance, self.includeradius, self.approachradius, self.speedlimit = gps_distance(
gps_pkt.latitude, gps_pkt.longitude, gps_pkt.altitude,
gps_pkt.accuracy)
#self.distance = 6371010*acos(sin(radians(gps_pkt.latitude))*sin(radians(48.12893908))+cos(radians(gps_pkt.latitude))*cos(radians(48.12893908))*cos(radians(gps_pkt.longitude-9.797879048)))
# update prevs, update must run once per loop
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.door_all_closed = not any([
cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']
])
self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED']
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
if self.CP.enableGasInterceptor:
self.pedal_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS']
else:
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
self.car_gas = self.pedal_gas
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
v_wheel = float(
np.mean([
self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl,
self.v_wheel_rr
]))
# Kalman filter
if abs(
v_wheel - self.v_ego
) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
if self.lastlat_Control and self.v_ego > 11:
angle_later = self.lastlat_Control.anglelater
else:
angle_later = 0
self.a_ego = float(v_ego_x[1])
self.standstill = not v_wheel > 0.001
if self.CP.carFingerprint == CAR.OLD_CAR:
self.angle_steers = -(
cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] +
cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] / 3)
else:
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"][
'STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
try:
self.econ_on = cp.vl["GEAR_PACKET"]['ECON_ON']
except:
self.econ_on = 0
try:
self.sport_on = cp.vl["GEAR_PACKET"]['SPORT_ON']
except:
self.sport_on = 0
if self.econ_on == 0 and self.sport_on == 0:
if self.gasMode == 1:
self.sport_on = 1
elif self.gasMode == 2:
self.econ_on = 1
self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values)
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
#self.lkas_barriers = cp_cam.vl["LKAS_HUD"]['BARRIERS']
#self.left_line = cp_cam.vl["LKAS_HUD"]['LEFT_LINE']
#self.right_line = cp_cam.vl["LKAS_HUD"]['RIGHT_LINE']
#print self.lkas_barriers
#print self.right_line
#print self.left_line
self.blind_spot_side = cp.vl["DEBUG"]['BLINDSPOTSIDE']
if (cp.vl["DEBUG"]['BLINDSPOTD1'] >
10) or (cp.vl["DEBUG"]['BLINDSPOTD1'] > 10):
self.blind_spot_on = bool(1)
else:
self.blind_spot_on = bool(0)
# we could use the override bit from dbc, but it's triggered at too high torque values
self.steer_override = abs(
cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']) > 100
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
if self.CP.enableGasInterceptor:
self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [
1, 3, 5, 9, 17, 25
]
else:
self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [
1, 3, 5, 9, 17, 25
]
self.steer_unavailable = cp.vl["EPS_STATUS"]['LKA_STATE'] in [
3, 17
] # don't disengage, just warning
self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
self.brake_error = 0
self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"][
'STEER_TORQUE_DRIVER']
self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"][
'STEER_TORQUE_EPS']
if bool(cp.vl["JOEL_ID"]
['LANE_WARNING']) <> self.lane_departure_toggle_on_prev:
self.lane_departure_toggle_on = bool(
cp.vl["JOEL_ID"]['LANE_WARNING'])
if self.lane_departure_toggle_on:
self.cstm_btns.set_button_status("lka", 1)
else:
self.cstm_btns.set_button_status("lka", 0)
self.lane_departure_toggle_on_prev = self.lane_departure_toggle_on
else:
if self.cstm_btns.get_button_status("lka") == 0:
self.lane_departure_toggle_on = False
else:
if self.alcaMode == 3 and (self.left_blinker_on
or self.right_blinker_on):
self.lane_departure_toggle_on = False
else:
self.lane_departure_toggle_on = True
self.distance_toggle = cp.vl["JOEL_ID"]['ACC_DISTANCE']
if cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES'] == 2:
self.trfix = True
if self.trfix:
self.read_distance_lines = cp.vl["PCM_CRUISE_SM"]['DISTANCE_LINES']
else:
self.read_distance_lines = 2
if self.distance_toggle <> self.distance_toggle_prev:
if self.read_distance_lines == self.distance_toggle:
self.distance_toggle_prev = self.distance_toggle
else:
self.cstm_btns.set_button_status("tr", 1)
if self.read_distance_lines <> self.read_distance_lines_prev:
self.cstm_btns.set_button_status("tr", 0)
if self.read_distance_lines == 1:
self.UE.custom_alert_message(2,
"Following distance set to 0.9s",
200, 3)
if self.read_distance_lines == 2:
self.UE.custom_alert_message(2, "Smooth following distance",
200, 3)
if self.read_distance_lines == 3:
self.UE.custom_alert_message(2,
"Following distance set to 2.7s",
200, 3)
self.read_distance_lines_prev = self.read_distance_lines
if bool(cp.vl["JOEL_ID"]['ACC_SLOW']) <> self.acc_slow_on_prev:
self.acc_slow_on = bool(cp.vl["JOEL_ID"]['ACC_SLOW'])
if self.acc_slow_on:
self.cstm_btns.set_button_status("slow", 1)
else:
self.cstm_btns.set_button_status("slow", 0)
self.acc_slow_on_prev = self.acc_slow_on
else:
if self.cstm_btns.get_button_status("slow") == 0:
self.acc_slow_on = False
else:
self.acc_slow_on = True
# we could use the override bit from dbc, but it's triggered at too high torque values
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
self.user_brake = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSURE']
if self.CP.carFingerprint == CAR.LEXUS_IS:
self.pcm_acc_status = cp.vl["PCM_CRUISE_3"]['CRUISE_STATE']
self.v_cruise_pcm = cp.vl["PCM_CRUISE_3"]['SET_SPEED']
self.low_speed_lockout = 0
self.main_on = cp.vl["PCM_CRUISE_3"]['MAIN_ON']
elif self.CP.carFingerprint == CAR.LEXUS_ISH:
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']
self.v_cruise_pcm = cp.vl["PCM_CRUISE_ISH"]['SET_SPEED']
self.low_speed_lockout = False
self.main_on = cp.vl["PCM_CRUISE_ISH"]['MAIN_ON']
else:
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"][
'LOW_SPEED_LOCKOUT'] == 2
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
if self.acc_slow_on and self.CP.carFingerprint != CAR.OLD_CAR:
self.v_cruise_pcm = max(7, int(self.v_cruise_pcm) - 34.0)
if self.acc_slow_on:
if not self.left_blinker_on and not self.right_blinker_on:
self.Angles[self.Angle_counter] = abs(self.angle_steers)
self.Angles_later[self.Angle_counter] = abs(angle_later)
self.v_cruise_pcm = int(
min(
self.v_cruise_pcm,
self.brakefactor * interp(np.max(
self.Angles), self.Angle, self.Angle_Speed)))
self.v_cruise_pcm = int(
min(
self.v_cruise_pcm,
self.brakefactor * interp(np.max(
self.Angles_later), self.Angle, self.Angle_Speed)))
else:
self.Angles[self.Angle_counter] = 0
self.Angles_later[self.Angle_counter] = 0
self.Angle_counter = (self.Angle_counter + 1) % 250
#print "distane"
#print self.distance
if self.distance < self.approachradius + self.includeradius:
#print "speed"
#print self.prev_distance - self.distance
#if speed is 5% higher than the speedlimit
if self.prev_distance - self.distance > self.speedlimit * 0.00263889:
if self.v_cruise_pcm > self.speedlimit:
self.v_cruise_pcm = self.speedlimit
if self.distance < self.includeradius:
#print "inside"
if self.v_cruise_pcm > self.speedlimit:
self.v_cruise_pcm = self.speedlimit
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
self.gas_pressed = not cp.vl["PCM_CRUISE"]['GAS_RELEASED']
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC']
or self.brake_pressed)
if self.CP.carFingerprint == CAR.PRIUS:
self.indi_toggle = True
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
elif self.CP.carFingerprint == CAR.LEXUS_ISH:
self.generic_toggle = bool(
cp.vl["LIGHT_STALK_ISH"]['AUTO_HIGH_BEAM'])
else:
self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
self.tsgn1 = cp_cam.vl["RSA1"]['TSGN1']
self.spdval1 = cp_cam.vl["RSA1"]['SPDVAL1']
self.splsgn1 = cp_cam.vl["RSA1"]['SPLSGN1']
self.tsgn2 = cp_cam.vl["RSA1"]['TSGN2']
self.spdval2 = cp_cam.vl["RSA1"]['SPDVAL2']
self.splsgn2 = cp_cam.vl["RSA1"]['SPLSGN2']
self.tsgn3 = cp_cam.vl["RSA2"]['TSGN3']
self.splsgn3 = cp_cam.vl["RSA2"]['SPLSGN3']
self.tsgn4 = cp_cam.vl["RSA2"]['TSGN4']
self.splsgn4 = cp_cam.vl["RSA2"]['SPLSGN4']
self.noovertake = self.tsgn1 == 65 or self.tsgn2 == 65 or self.tsgn3 == 65 or self.tsgn4 == 65 or self.tsgn1 == 66 or self.tsgn2 == 66 or self.tsgn3 == 66 or self.tsgn4 == 66
if self.spdval1 > 0 or self.spdval2 > 0:
dat = messaging.new_message()
dat.init('liveTrafficData')
if self.spdval1 > 0:
dat.liveTrafficData.speedLimitValid = True
if self.tsgn1 == 36:
dat.liveTrafficData.speedLimit = self.spdval1 * 1.60934
elif self.tsgn1 == 1:
dat.liveTrafficData.speedLimit = self.spdval1
else:
dat.liveTrafficData.speedLimit = 0
else:
dat.liveTrafficData.speedLimitValid = False
if self.spdval2 > 0:
dat.liveTrafficData.speedAdvisoryValid = True
dat.liveTrafficData.speedAdvisory = self.spdval2
else:
dat.liveTrafficData.speedAdvisoryValid = False
self.traffic_data_sock.send(dat.to_bytes())
def __init__(self, CP):
self.lkMode = True
self.gasMode = int(kegman.conf['lastGasMode'])
self.gasLabels = ["dynamic", "sport", "eco"]
self.Angle = [0, 5, 10, 15, 20, 25, 30, 35, 60, 100, 180, 270, 500]
self.Angle_Speed = [
255, 160, 100, 80, 70, 60, 55, 50, 40, 30, 20, 10, 5
]
self.blind_spot_on = bool(0)
#labels for ALCA modes
self.alcaLabels = ["MadMax", "Normal", "Wifey", "off"]
steerRatio = CP.steerRatio
self.trLabels = ["0.9", "dyn", "2.7"]
self.alcaMode = int(kegman.conf['lastALCAMode']
) # default to last ALCA Mode on startup
if self.alcaMode > 3:
self.alcaMode = 3
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
self.trMode = int(kegman.conf['lastTrMode']
) # default to last distance interval on startup
if self.trMode > 2:
self.trMode = 2
kegman.save({'lastTrMode': int(self.trMode)
}) # write last distance bar setting to file
#if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 32., 55.]
self.CL_MAX_ANGLE_DELTA = [
2.0 * 15.4 / steerRatio, 1. * 15.4 / steerRatio,
0.5 * 15.4 / steerRatio
]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 2.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 4.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.040
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = [1., 1.1, 1.2]
self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7, 0.30, 0.30]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.read_distance_lines_prev = 3
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
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.stopped = 0
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Honda", "honda")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
# 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
class CarState(object):
def __init__(self, CP):
self.lkMode = True
self.gasMode = int(kegman.conf['lastGasMode'])
self.gasLabels = ["dynamic", "sport", "eco"]
self.Angle = [0, 5, 10, 15, 20, 25, 30, 35, 60, 100, 180, 270, 500]
self.Angle_Speed = [
255, 160, 100, 80, 70, 60, 55, 50, 40, 30, 20, 10, 5
]
self.blind_spot_on = bool(0)
#labels for ALCA modes
self.alcaLabels = ["MadMax", "Normal", "Wifey", "off"]
steerRatio = CP.steerRatio
self.trLabels = ["0.9", "dyn", "2.7"]
self.alcaMode = int(kegman.conf['lastALCAMode']
) # default to last ALCA Mode on startup
if self.alcaMode > 3:
self.alcaMode = 3
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
self.trMode = int(kegman.conf['lastTrMode']
) # default to last distance interval on startup
if self.trMode > 2:
self.trMode = 2
kegman.save({'lastTrMode': int(self.trMode)
}) # write last distance bar setting to file
#if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 32., 55.]
self.CL_MAX_ANGLE_DELTA = [
2.0 * 15.4 / steerRatio, 1. * 15.4 / steerRatio,
0.5 * 15.4 / steerRatio
]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 2.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 4.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.040
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = [1., 1.1, 1.2]
self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7, 0.30, 0.30]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.read_distance_lines_prev = 3
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
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.stopped = 0
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Honda", "honda")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
# 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
#BB init ui buttons
def init_ui_buttons(self):
btns = []
btns.append(
UIButton("alca", "ALC", 1, self.alcaLabels[self.alcaMode], 0))
btns.append(UIButton("lka", "LKA", 1, "", 1))
btns.append(UIButton("", "", 0, "", 2))
btns.append(UIButton("sound", "SND", 0, "", 3))
btns.append(UIButton("tr", "TR", 0, self.trLabels[self.trMode], 4))
btns.append(UIButton("gas", "Gas", 1, self.gasLabels[self.gasMode], 5))
return btns
#BB update ui buttons
def update_ui_buttons(self, id, btn_status):
if self.cstm_btns.btns[id].btn_status > 0:
if (id == 0) and (btn_status == 0
) and self.cstm_btns.btns[id].btn_name == "alca":
if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[
self.alcaMode]:
self.alcaMode = (self.alcaMode + 1) % 4
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
else:
self.alcaMode = 0
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[
self.alcaMode]
self.cstm_btns.hasChanges = True
if self.alcaMode == 3:
self.cstm_btns.set_button_status("alca", 0)
elif (id == 4) and (btn_status == 0
) and self.cstm_btns.btns[id].btn_name == "tr":
if self.cstm_btns.btns[id].btn_label2 == self.trLabels[
self.trMode]:
self.trMode = (self.trMode + 1) % 3
kegman.save({'lastTrMode': int(self.trMode)
}) # write last distance bar setting to file
else:
self.trMode = 0
kegman.save({'lastTrMode': int(self.trMode)
}) # write last distance bar setting to file
self.cstm_btns.btns[id].btn_label2 = self.trLabels[self.trMode]
self.cstm_btns.hasChanges = True
elif (id == 5) and (
btn_status
== 0) and self.cstm_btns.btns[id].btn_name == "gas":
if self.cstm_btns.btns[id].btn_label2 == self.gasLabels[
self.gasMode]:
self.gasMode = (self.gasMode + 1) % 3
kegman.save({'lastGasMode': int(self.gasMode)
}) # write last GasMode setting to file
else:
self.gasMode = 0
kegman.save({'lastGasMode': int(self.gasMode)
}) # write last GasMode setting to file
self.cstm_btns.btns[id].btn_label2 = self.gasLabels[
self.gasMode]
self.cstm_btns.hasChanges = True
else:
self.cstm_btns.btns[
id].btn_status = btn_status * self.cstm_btns.btns[
id].btn_status
else:
self.cstm_btns.btns[id].btn_status = btn_status
if (id == 0) and self.cstm_btns.btns[id].btn_name == "alca":
self.alcaMode = (self.alcaMode + 1) % 4
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[
self.alcaMode]
self.cstm_btns.hasChanges = True
def update(self, cp, cp_cam):
# copy can_valid on buses 0 and 2
self.can_valid = cp.can_valid
self.cam_can_valid = cp_cam.can_valid
# 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): # 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):
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
self.door_all_closed = not cp.vl["SCM_FEEDBACK"][
'DRIVERS_DOOR_OPEN']
self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']
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']
# 2 = temporary; 3 = TBD; 4 = significant steering wheel torque; 5 = (permanent); 6 = temporary; 7 = (permanent)
# TODO: Use values from DBC to parse this field
self.steer_error = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [
0, 2, 3, 4, 6
]
self.steer_not_allowed = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [
0, 4
] # 4 can be caused by bump OR steering nudge from driver
self.steer_warning = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [
0, 3, 4
] # 3 is low speed lockout, not worth a warning
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']
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_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):
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
self.main_on = cp.vl["SCM_FEEDBACK"]['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):
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_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.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):
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']
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.v_cruise_pcm = int(
min(self.v_cruise_pcm,
interp(self.angle_steers, self.Angle, self.Angle_Speed)))
self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS']
self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']
if self.cruise_setting == 3:
if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0:
self.trMode = (self.trMode + 1) % 3
kegman.save({'lastTrMode': int(self.trMode)
}) # write last distance bar setting to file
self.cstm_btns.btns[4].btn_label2 = self.trLabels[self.trMode]
self.read_distance_lines = self.trMode + 1
if self.read_distance_lines <> self.read_distance_lines_prev:
if self.read_distance_lines == 1:
self.UE.custom_alert_message(2,
"Following distance set to 0.9s",
200, 3)
if self.read_distance_lines == 2:
self.UE.custom_alert_message(2, "Dynamic Following distance",
200, 3)
if self.read_distance_lines == 3:
self.UE.custom_alert_message(2,
"Following distance set to 2.7s",
200, 3)
self.read_distance_lines_prev = self.read_distance_lines
# 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']
if self.cstm_btns.get_button_status("lka") == 0:
self.lane_departure_toggle_on = False
else:
if self.alcaMode == 3 and (self.left_blinker_on
or self.right_blinker_on):
self.lane_departure_toggle_on = False
else:
self.lane_departure_toggle_on = True
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
# TODO: this should be ok for all cars. Verify it.
if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019,
CAR.RIDGELINE):
if self.user_brake > 0.05:
self.brake_pressed = 1
def __init__(self, CP):
self.gasMode = int(kegman.conf['lastGasMode'])
self.gasLabels = ["dynamic", "sport", "eco"]
self.alcaLabels = ["MadMax", "Normal", "Wifey", "off"]
self.alcaMode = int(kegman.conf['lastALCAMode'])
steerRatio = CP.steerRatio
self.prev_distance_button = 0
self.distance_button = 0
self.prev_lka_button = 0
self.lka_button = 0
self.lkMode = True
self.lane_departure_toggle_on = True
# ALCA PARAMS
self.blind_spot_on = bool(0)
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 32., 55.]
self.CL_MAX_ANGLE_DELTA = [
0.77 * 16.2 / steerRatio, 0.86 * 16.2 / steerRatio,
0.535 * 16.2 / steerRatio
]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 2.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 4.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.040
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = [1., 1.1, 1.2]
self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.2, 0.2, 0.2]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.CP = CP
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Chrysler", "chrysler")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
self.left_blinker_on = 0
self.right_blinker_on = 0
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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
class CarState(object):
def __init__(self, CP):
# labels for buttons
self.btns_init = [["alca", "ALC", ["MadMax", "Normal", "Calm"]],
[ACCMode.BUTTON_NAME, ACCMode.BUTTON_ABREVIATION, ACCMode.labels()],
["steer", "STR", [""]],
["brake", "BRK", [""]],
["msg", "MSG", [""]],
["sound", "SND", [""]]]
if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 44.]
self.CL_MAX_ANGLE_DELTA = [2.2, .3]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16. , 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5. , 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 1.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40.,10., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = 1.
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7 ,0.50, 0.40]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.brake_only = CP.enableCruise
self.last_cruise_stalk_pull_time = 0
self.CP = CP
self.user_gas = 0.
self.user_gas_pressed = False
self.pedal_interceptor_state = 0
self.pedal_interceptor_value = 0.
self.pedal_interceptor_value2 = 0.
self.pedal_interceptor_missed_counter = 0
self.brake_switch_prev = 0
self.brake_switch_ts = 0
self.cruise_buttons = 0
self.blinker_on = 0
self.left_blinker_on = 0
self.right_blinker_on = 0
self.steer_warning = 0
self.stopped = 0
self.frame_humanSteered = 0 # Last frame human steered
# variables used for the fake DAS creation
self.DAS_info_frm = -1
self.DAS_info_msg = 0
self.DAS_status_frm = 0
self.DAS_status_idx = 0
self.DAS_status2_frm = 0
self.DAS_status2_idx = 0
self.DAS_bodyControls_frm = 0
self.DAS_bodyControls_idx = 0
self.DAS_lanes_frm = 0
self.DAS_lanes_idx = 0
self.DAS_objects_frm = 0
self.DAS_objects_idx = 0
self.DAS_pscControl_frm = 0
self.DAS_pscControl_idx = 0
self.DAS_warningMatrix0_idx = 0
self.DAS_warningMatrix1_idx = 0
self.DAS_warningMatrix3_idx = 0
self.DAS_telemetryPeriodic1_idx = 0
self.DAS_telemetryPeriodic2_idx = 0
self.DAS_telemetryEvent1_idx = 0
self.DAS_telemetryEvent2_idx = 0
self.DAS_control_idx = 0
#BB variables for pedal CC
self.pedal_speed_kph = 0.
# Pedal mode is ready, i.e. hardware is present and normal cruise is off.
self.pedal_interceptor_available = False
self.prev_pedal_interceptor_available = False
#BB UIEvents
self.UE = UIEvents(self)
#BB PCC
self.regenLight = 0
self.torqueLevel = 0.
#BB variable for custom buttons
self.cstm_btns = UIButtons(self,"Tesla Model S","tesla")
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
#BB steering_wheel_stalk last position, used by ACC and ALCA
self.steering_wheel_stalk = None
#BB carConfig data used to change IC info
self.real_carConfig = None
self.real_dasHw = 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
self.imperial_speed_units = True
# The current max allowed cruise speed. Actual cruise speed sent to the car
# may be lower, depending on traffic.
self.v_cruise_pcm = 0.0
# Actual cruise speed currently active on the car.
self.v_cruise_actual = 0.0
def config_ui_buttons(self, pedalPresent):
if pedalPresent:
self.btns_init[1] = [PCCModes.BUTTON_NAME, PCCModes.BUTTON_ABREVIATION, PCCModes.labels()]
else:
# we don't have pedal interceptor
self.btns_init[1] = [ACCMode.BUTTON_NAME, ACCMode.BUTTON_ABREVIATION, ACCMode.labels()]
btn = self.cstm_btns.btns[1]
btn.btn_name = self.btns_init[1][0]
btn.btn_label = self.btns_init[1][1]
btn.btn_label2 = self.btns_init[1][2][0]
btn.btn_status = 1
self.cstm_btns.update_ui_buttons(1, 1)
def update(self, cp, epas_cp):
# copy can_valid
self.can_valid = cp.can_valid
# 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_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.steering_wheel_stalk = cp.vl["STW_ACTN_RQ"]
self.real_carConfig = cp.vl["GTW_carConfig"]
self.real_dasHw = cp.vl["GTW_carConfig"]['GTW_dasHw']
self.cruise_buttons = cp.vl["STW_ACTN_RQ"]['SpdCtrlLvr_Stat']
# ******************* parse out can *******************
self.door_all_closed = not any([cp.vl["GTW_carState"]['DOOR_STATE_FL'], cp.vl["GTW_carState"]['DOOR_STATE_FR'],
cp.vl["GTW_carState"]['DOOR_STATE_RL'], cp.vl["GTW_carState"]['DOOR_STATE_RR']]) #JCT
self.seatbelt = cp.vl["GTW_status"]['GTW_driverPresent']
# 2 = temporary 3= TBD 4 = temporary, hit a bump 5 (permanent) 6 = temporary 7 (permanent)
# TODO: Use values from DBC to parse this field
self.steer_error = epas_cp.vl["EPAS_sysStatus"]['EPAS_steeringFault'] == 1
self.steer_not_allowed = epas_cp.vl["EPAS_sysStatus"]['EPAS_eacStatus'] not in [2,1] # 2 "EAC_ACTIVE" 1 "EAC_AVAILABLE" 3 "EAC_FAULT" 0 "EAC_INHIBITED"
self.steer_warning = self.steer_not_allowed
self.brake_error = 0 #NOT WORKINGcp.vl[309]['ESP_brakeLamp'] #JCT
# JCT More ESP errors available, these needs to be added once car steers on its own to disable / alert driver
self.esp_disabled = 0 #NEED TO CORRECT DBC cp.vl[309]['ESP_espOffLamp'] or cp.vl[309]['ESP_tcOffLamp'] or cp.vl[309]['ESP_tcLampFlash'] or cp.vl[309]['ESP_espFaultLamp'] #JCT
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel_fl = 0 #JCT
self.v_wheel_fr = 0 #JCT
self.v_wheel_rl = 0 #JCT
self.v_wheel_rr = 0 #JCT
self.v_wheel = 0 #JCT
self.v_weight = 0 #JCT
speed = (cp.vl["DI_torque2"]['DI_vehicleSpeed'])*CV.MPH_TO_KPH/3.6 #JCT MPH_TO_MS. Tesla is in MPH, v_ego is expected in M/S
speed = speed * 1.01 # To match car's displayed speed
self.v_ego_x = np.matrix([[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])
#BB use this set for pedal work as the user_gas_xx is used in other places
self.pedal_interceptor_state = epas_cp.vl["GAS_SENSOR"]['STATE']
self.pedal_interceptor_value = epas_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS']
self.pedal_interceptor_value2 = epas_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']
can_gear_shifter = cp.vl["DI_torque2"]['DI_gear']
self.gear = 0 # JCT
# self.angle_steers = -(cp.vl["STW_ANGLHP_STAT"]['StW_AnglHP']) #JCT polarity reversed from Honda/Acura
self.angle_steers = -(epas_cp.vl["EPAS_sysStatus"]['EPAS_internalSAS']) #BB see if this works better than STW_ANGLHP_STAT for angle
self.angle_steers_rate = 0 #JCT
self.blinker_on = (cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 1) or (cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 2)
self.left_blinker_on = cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 1
self.right_blinker_on = cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 2
#if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY):
# self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
# self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
# self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
#else:
self.park_brake = 0 # TODO
self.brake_hold = 0 # TODO
self.main_on = 1 #cp.vl["SCM_BUTTONS"]['MAIN_ON']
self.cruise_speed_offset = calc_cruise_offset(cp.vl["DI_state"]['DI_cruiseSet'], self.v_ego)
self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.CP.carFingerprint)
self.pedal_gas = 0. # cp.vl["DI_torque1"]['DI_pedalPos'] / 102 #BB: to make it between 0..1
self.car_gas = self.pedal_gas
self.steer_override = abs(epas_cp.vl["EPAS_sysStatus"]['EPAS_handsOnLevel']) > 0
self.steer_torque_driver = 0 #JCT
# brake switch has shown some single time step noise, so only considered when
# switch is on for at least 2 consecutive CAN samples
# Todo / refactor: This shouldn't have to do with epas == 3..
# was wrongly set to epas_cp.vl["EPAS_sysStatus"]['EPAS_eacErrorCode'] == 3 and epas_cp.vl["EPAS_sysStatus"]['EPAS_eacStatus'] == 0
self.brake_switch = cp.vl["DI_torque2"]['DI_brakePedal']
self.brake_pressed = cp.vl["DI_torque2"]['DI_brakePedal']
self.standstill = cp.vl["DI_torque2"]['DI_vehicleSpeed'] == 0
self.torqueMotor = cp.vl["DI_torque1"]['DI_torqueMotor']
self.pcm_acc_status = cp.vl["DI_state"]['DI_cruiseState']
self.imperial_speed_units = cp.vl["DI_state"]['DI_speedUnits'] == 0
self.regenLight = cp.vl["DI_state"]['DI_regenLight'] == 1
self.prev_pedal_interceptor_available = self.pedal_interceptor_available
pedal_has_value = bool(self.pedal_interceptor_value) or bool(self.pedal_interceptor_value2)
pedal_interceptor_present = self.pedal_interceptor_state in [0, 5] and pedal_has_value
# Add loggic if we just miss some CAN messages so we don't immediately disable pedal
if pedal_interceptor_present:
self.pedal_interceptor_missed_counter = 0
else:
self.pedal_interceptor_missed_counter += 1
pedal_interceptor_present = self.pedal_interceptor_missed_counter < 10
# Mark pedal unavailable while traditional cruise is on.
self.pedal_interceptor_available = pedal_interceptor_present and not bool(self.pcm_acc_status)
if self.pedal_interceptor_available != self.prev_pedal_interceptor_available:
self.config_ui_buttons(self.pedal_interceptor_available)
if self.imperial_speed_units:
self.v_cruise_actual = (cp.vl["DI_state"]['DI_cruiseSet'])*CV.MPH_TO_KPH # Reported in MPH, expected in KPH??
else:
self.v_cruise_actual = cp.vl["DI_state"]['DI_cruiseSet']
self.hud_lead = 0 #JCT
self.cruise_speed_offset = calc_cruise_offset(self.v_cruise_pcm, self.v_ego)
def __init__(self, CP):
#labels for buttons
self.btns_init = [["","",["","",""]], \
["","",[""]], \
["alca","ALC",["MadMax", "Normal", "Wifey"]], \
["cam","CAM",[""]], \
["alwon", "MAD",[""]], \
["sound", "SND", [""]]]
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 44.]
self.CL_MAX_ANGLE_DELTA = [2.4, .3]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.7, .75]
self.CL_LANE_PASS_BP = [10., 44.]
self.CL_LANE_PASS_TIME = [60., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.042
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = 1.
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7, 0.4, 0.20]
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.CP = CP
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
self.left_blinker_on = 0
self.left_blinker_flash = 0
self.right_blinker_on = 0
self.right_blinker_flash = 0
self.has_scc = False
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Hyundai", "hyundai")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
self.params = Params()
self.CL_MIN_V = 8.9
self.CL_MAX_A = 20.0
# labels for buttons
self.btns_init = [
["alca", "ALC", [""]],
[
ACCMode.BUTTON_NAME, ACCMode.BUTTON_ABREVIATION,
ACCMode.labels()
],
["dsp", "DSP", ["OP", "MIN", "OFF", "GYRO"]],
["", "", [""]],
["msg", "MSG", [""]],
["sound", "SND", [""]],
]
### START OF MAIN CONFIG OPTIONS ###
### Do NOT modify here, modify in /data/bb_openpilot.cfg and reboot
self.forcePedalOverCC = True
self.usesApillarHarness = False
self.enableHSO = True
self.enableALCA = True
self.enableDasEmulation = True
self.enableRadarEmulation = True
self.enableDriverMonitor = True
self.enableShowCar = True
self.enableShowLogo = True
self.hasNoctuaFan = False
self.limitBatteryMinMax = False
self.limitBattery_Min = 60
self.limitBattery_Max = 70
self.doAutoUpdate = True
self.blockUploadWhileTethering = False
self.tetherIP = "127.0.0."
self.useTeslaGPS = False
self.useTeslaMapData = False
self.hasTeslaIcIntegration = False
self.useTeslaRadar = False
self.useWithoutHarness = False
self.radarVIN = " "
self.enableLdw = True
self.radarOffset = 0.0
self.radarPosition = 0
self.radarEpasType = 0
self.fix1916 = False
self.forceFingerprintTesla = False
self.spinnerText = ""
self.hsoNumbPeriod = 1.5
self.ldwNumbPeriod = 1.5
self.tapBlinkerExtension = 2
self.ahbOffDuration = 5
self.roadCameraID = ""
self.driverCameraID = ""
self.roadCameraFx = 0.73
self.driverCameraFx = 0.75
self.roadCameraFlip = 0
self.driverCameraFlip = 0
self.monitorForcedRes = ""
# read config file
read_config_file(self)
### END OF MAIN CONFIG OPTIONS ###
self.apEnabled = True
self.apFollowTimeInS = 2.5 # time in seconds to follow
self.keepEonOff = False
self.alcaEnabled = True
self.mapAwareSpeed = False
# Tesla Model
self.teslaModelDetected = 1
self.teslaModel = self.params.get("TeslaModel")
if self.teslaModel:
self.teslaModel = self.teslaModel.decode()
if self.teslaModel is None:
self.teslaModel = "S"
self.teslaModelDetected = 0
# for map integration
self.csaRoadCurvC3 = 0.0
self.csaRoadCurvC2 = 0.0
self.csaRoadCurvRange = 0.0
self.csaRoadCurvUsingTspline = 0
self.csaOfframpCurvC3 = 0.0
self.csaOfframpCurvC2 = 0.0
self.csaOfframpCurvRange = 0.0
self.csaOfframpCurvUsingTspline = 0
self.roadCurvHealth = 0
self.roadCurvRange = 0.0
self.roadCurvC0 = 0.0
self.roadCurvC1 = 0.0
self.roadCurvC2 = 0.0
self.roadCurvC3 = 0.0
self.meanFleetSplineSpeedMPS = 0.0
self.UI_splineID = 0
self.meanFleetSplineAccelMPS2 = 0.0
self.medianFleetSpeedMPS = 0.0
self.topQrtlFleetSplineSpeedMPS = 0.0
self.splineLocConfidence = 0
self.baseMapSpeedLimitMPS = 0.0
self.bottomQrtlFleetSpeedMPS = 0.0
self.rampType = 0
self.mapBasedSuggestedSpeed = 0.0
self.splineBasedSuggestedSpeed = 0.0
self.map_suggested_speed = 0.0
self.gpsLongitude = 0.0
self.gpsLatitude = 0.0
self.gpsAccuracy = 0.0
self.gpsElevation = 0.0
self.gpsHeading = 0.0
self.gpsVehicleSpeed = 0.0
self.speed_limit_ms = 0.0
self.userSpeedLimitOffsetKph = 0.0
self.DAS_fusedSpeedLimit = 0
self.brake_only = CP.enableCruise
self.last_cruise_stalk_pull_time = 0
self.CP = CP
self.user_gas = 0.0
self.user_gas_pressed = False
self.pedal_interceptor_state = self.prev_pedal_interceptor_state = 0
self.pedal_interceptor_value = 0.0
self.pedal_interceptor_value2 = 0.0
self.pedal_idx = self.prev_pedal_idx = 0
self.brake_switch_prev = 0
self.brake_switch_ts = 0
self.cruise_buttons = 0
self.turn_signal_state_left = (
0 # 0 = off, 1 = on (blinking), 2 = failed, 3 = default
)
self.turn_signal_state_right = (
0 # 0 = off, 1 = on (blinking), 2 = failed, 3 = default
)
self.prev_turn_signal_blinking = False
self.turn_signal_blinking = False
self.prev_turn_signal_stalk_state = 0
self.turn_signal_stalk_state = (
0 # 0 = off, 1 = indicate left (stalk down), 2 = indicate right (stalk up)
)
self.steer_warning = 0
self.stopped = 0
# variables used for the fake DAS creation
self.DAS_info_frm = -1
self.DAS_info_msg = 0
self.DAS_status_frm = 0
self.DAS_status_idx = 0
self.DAS_status2_frm = 0
self.DAS_status2_idx = 0
self.DAS_bodyControls_frm = 0
self.DAS_bodyControls_idx = 0
self.DAS_lanes_frm = 0
self.DAS_lanes_idx = 0
self.DAS_objects_frm = 0
self.DAS_objects_idx = 0
self.DAS_pscControl_frm = 0
self.DAS_pscControl_idx = 0
self.DAS_warningMatrix0_idx = 0
self.DAS_warningMatrix1_idx = 0
self.DAS_warningMatrix3_idx = 0
self.DAS_telemetryPeriodic1_idx = 0
self.DAS_telemetryPeriodic2_idx = 0
self.DAS_telemetryEvent1_idx = 0
self.DAS_telemetryEvent2_idx = 0
self.DAS_control_idx = 0
# BB notification messages for DAS
self.DAS_canErrors = 0
self.DAS_plannerErrors = 0
self.DAS_doorOpen = 0
self.DAS_notInDrive = 0
self.summonButton = 0
# BB variables for pedal CC
self.pedal_speed_kph = 0.0
# BB UIEvents
self.UE = UIEvents(self)
# BB PCC
self.regenLight = 0
self.torqueLevel = 0.0
# BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Tesla Model S", "tesla",
self.enableShowLogo, self.enableShowCar)
# BB custom message counter
self.custom_alert_counter = (
-1
) # set to 100 for 1 second display; carcontroller will take down to zero
# BB steering_wheel_stalk last position, used by ACC and ALCA
self.steering_wheel_stalk = None
# BB carConfig data used to change IC info
self.real_carConfig = None
self.real_dasHw = 0
# BB visiond last type
self.last_visiond = self.cstm_btns.btns[3].btn_label2
# 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
self.imperial_speed_units = True
# The current max allowed cruise speed. Actual cruise speed sent to the car
# may be lower, depending on traffic.
self.v_cruise_pcm = 0.0
# Actual cruise speed currently active on the car.
self.v_cruise_actual = 0.0
# ALCA params
self.ALCA_enabled = False
self.ALCA_total_steps = 100
self.ALCA_direction = 0
self.ALCA_error = False
self.angle_offset = 0.0
self.init_angle_offset = False
# AHB params
self.ahbHighBeamStalkPosition = 0
self.ahbEnabled = 0
self.ahbLoBeamOn = 0
self.ahbHiBeamOn = 0
self.ahbNightMode = 0
@staticmethod
def get_can_parser(CP):
signals, checks = get_can_signals(CP)
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
@staticmethod
def get_can_parser2(CP, mydbc):
signals, checks = get_can_signals(CP)
return CANParser(mydbc, signals, checks, 0)
@staticmethod
def get_epas_parser(CP, epascan):
signals, checks = get_epas_can_signals(CP)
return CANParser(DBC[CP.carFingerprint]["pt"] + "_epas", signals,
checks, epascan)
@staticmethod
def get_pedal_parser(CP, pedalcan):
signals, checks = get_pedal_can_signals(CP)
return CANParser(DBC[CP.carFingerprint]["pt"] + "_pedal", signals,
checks, pedalcan)
def config_ui_buttons(self, pcc_available, pcc_blocked_by_acc_mode):
if pcc_available:
self.btns_init[1] = [
PCCModes.BUTTON_NAME,
PCCModes.BUTTON_ABREVIATION,
PCCModes.labels(),
]
else:
self.btns_init[1] = [
ACCMode.BUTTON_NAME,
ACCMode.BUTTON_ABREVIATION,
ACCMode.labels(),
]
btn = self.cstm_btns.btns[1]
btn.btn_name = self.btns_init[1][0]
btn.btn_label = self.btns_init[1][1]
btn.btn_label2 = self.btns_init[1][2][0]
btn.btn_status = 1
if (not pcc_available) and pcc_blocked_by_acc_mode:
btn.btn_label2 = self.btns_init[1][2][1]
self.cstm_btns.update_ui_buttons(1, 1)
def _convert_to_DAS_fusedSpeedLimit(self, speed_limit_uom,
speed_limit_type):
if speed_limit_uom > 0:
if speed_limit_type == 0x1E: # Autobahn with no speed limit
return 0x1F # no speed limit sign
return int(speed_limit_uom / 5 +
0.5) # sign ID in 5 kph/mph increments (7 shows as 5)
else:
if speed_limit_type == 0x1F: # SNA (parking lot, no public road, etc.)
return 0 # no sign
return 1 # show 5 kph/mph for unknown limit where we should have one
def compute_speed(self):
# if one of them is zero, select max of the two
if self.meanFleetSplineSpeedMPS == 0 or self.medianFleetSpeedMPS == 0:
self.splineBasedSuggestedSpeed = max(self.meanFleetSplineSpeedMPS,
self.medianFleetSpeedMPS)
else:
self.splineBasedSuggestedSpeed = (
self.splineLocConfidence * self.meanFleetSplineSpeedMPS +
(100 - self.splineLocConfidence) *
self.medianFleetSpeedMPS) / 100.0
# if confidence over 60%, then weight between bottom speed and top speed
# if less than 40% then use map data
if self.splineLocConfidence > 60:
self.mapBasedSuggestedSpeed = (
self.splineLocConfidence * self.meanFleetSplineSpeedMPS +
(100 - self.splineLocConfidence) *
self.bottomQrtlFleetSpeedMPS) / 100.0
else:
self.mapBasedSuggestedSpeed = self.speed_limit_ms
if self.rampType > 0:
# we are on a ramp, use the spline info if available
if self.splineBasedSuggestedSpeed > 0:
self.map_suggested_speed = self.splineBasedSuggestedSpeed
else:
self.map_suggested_speed = self.mapBasedSuggestedSpeed
else:
# we are on a normal road, use max of the two
self.map_suggested_speed = max(self.mapBasedSuggestedSpeed,
self.splineBasedSuggestedSpeed)
def update_ui_buttons(self, btn_id, btn_status):
# we only focus on btn_id=3, which is for visiond
if ((btn_id == 3) and (self.cstm_btns.btns[btn_id].btn_status > 0) and
(self.last_visiond != self.cstm_btns.btns[btn_id].btn_label2)):
self.last_visiond = self.cstm_btns.btns[btn_id].btn_label2
# we switched between wiggly and normal
args = [
"/data/openpilot/selfdrive/car/modules/ch_visiond.sh",
self.cstm_btns.btns[btn_id].btn_label2,
]
subprocess.Popen(
args,
shell=False,
stdin=None,
stdout=None,
stderr=None,
env=dict(os.environ),
close_fds=True,
)
def update(self, cp, epas_cp, pedal_cp):
self.steering_wheel_stalk = cp.vl["STW_ACTN_RQ"]
self.real_carConfig = cp.vl["GTW_carConfig"]
self.real_dasHw = cp.vl["GTW_carConfig"]["GTW_dasHw"]
self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = cp.vl["STW_ACTN_RQ"]["SpdCtrlLvr_Stat"]
# ******************* parse out can *******************
self.door_all_closed = not any([
cp.vl["GTW_carState"]["DOOR_STATE_FL"],
cp.vl["GTW_carState"]["DOOR_STATE_FR"],
cp.vl["GTW_carState"]["DOOR_STATE_RL"],
cp.vl["GTW_carState"]["DOOR_STATE_RR"],
]) # JCT
if self.usesApillarHarness:
self.seatbelt = epas_cp.vl["SDM1"]["SDM_bcklDrivStatus"]
else:
self.seatbelt = cp.vl["SDM1"]["SDM_bcklDrivStatus"]
# self.seatbelt = cp.vl["SDM1"]['SDM_bcklDrivStatus'] and cp.vl["GTW_status"]['GTW_driverPresent']
if (cp.vl["GTW_carConfig"]["GTW_performanceConfig"]) and (
cp.vl["GTW_carConfig"]["GTW_performanceConfig"] > 0):
prev_teslaModel = self.teslaModel
self.teslaModel = "S"
if cp.vl["GTW_carConfig"]["GTW_performanceConfig"] > 1:
self.teslaModel = self.teslaModel + "P"
if cp.vl["GTW_carConfig"]["GTW_fourWheelDrive"] == 1:
self.teslaModel = self.teslaModel + "D"
if (self.teslaModelDetected
== 0) or (prev_teslaModel != self.teslaModel):
self.params.put("TeslaModel", self.teslaModel)
self.teslaModelDetected = 1
# Nav Map Data
self.csaRoadCurvC3 = cp.vl["UI_csaRoadCurvature"]["UI_csaRoadCurvC3"]
self.csaRoadCurvC2 = cp.vl["UI_csaRoadCurvature"]["UI_csaRoadCurvC2"]
self.csaRoadCurvRange = cp.vl["UI_csaRoadCurvature"][
"UI_csaRoadCurvRange"]
self.csaRoadCurvUsingTspline = cp.vl["UI_csaRoadCurvature"][
"UI_csaRoadCurvUsingTspline"]
self.csaOfframpCurvC3 = cp.vl["UI_csaOfframpCurvature"][
"UI_csaOfframpCurvC3"]
self.csaOfframpCurvC2 = cp.vl["UI_csaOfframpCurvature"][
"UI_csaOfframpCurvC2"]
self.csaOfframpCurvRange = cp.vl["UI_csaOfframpCurvature"][
"UI_csaOfframpCurvRange"]
self.csaOfframpCurvUsingTspline = cp.vl["UI_csaOfframpCurvature"][
"UI_csaOfframpCurvUsingTspline"]
self.roadCurvHealth = cp.vl["UI_roadCurvature"]["UI_roadCurvHealth"]
self.roadCurvRange = cp.vl["UI_roadCurvature"]["UI_roadCurvRange"]
self.roadCurvC0 = cp.vl["UI_roadCurvature"]["UI_roadCurvC0"]
self.roadCurvC1 = cp.vl["UI_roadCurvature"]["UI_roadCurvC1"]
self.roadCurvC2 = cp.vl["UI_roadCurvature"]["UI_roadCurvC2"]
self.roadCurvC3 = cp.vl["UI_roadCurvature"]["UI_roadCurvC3"]
self.gpsLongitude = cp.vl["MCU_locationStatus"]["MCU_longitude"]
self.gpsLatitude = cp.vl["MCU_locationStatus"]["MCU_latitude"]
self.gpsAccuracy = cp.vl["MCU_locationStatus"]["MCU_gpsAccuracy"]
self.gpsElevation = cp.vl["MCU_locationStatus2"]["MCU_elevation"]
self.gpsHeading = cp.vl["UI_gpsVehicleSpeed"]["UI_gpsVehicleHeading"]
self.gpsVehicleSpeed = (
cp.vl["UI_gpsVehicleSpeed"]["UI_gpsVehicleSpeed"] * CV.KPH_TO_MS)
if self.hasTeslaIcIntegration:
# BB: AutoSteer enabled does not work unless we do old style port mapping on MCU
# self.apEnabled = (cp.vl["MCU_chassisControl"]["MCU_latControlEnable"] == 1)
self.summonButton = int(
cp.vl["UI_driverAssistControl"]["UI_autoSummonEnable"])
self.apFollowTimeInS = (
1 + cp.vl["MCU_chassisControl"]["MCU_fcwSensitivity"] * 0.5)
self.keepEonOff = cp.vl["MCU_chassisControl"]["MCU_ldwEnable"] == 1
self.alcaEnabled = cp.vl["MCU_chassisControl"][
"MCU_pedalSafetyEnable"] == 1
self.mapAwareSpeed = (cp.vl["MCU_chassisControl"]["MCU_aebEnable"]
== 1 and self.useTeslaMapData)
# AHB info
self.ahbHighBeamStalkPosition = cp.vl["STW_ACTN_RQ"][
"HiBmLvr_Stat"]
self.ahbEnabled = cp.vl["MCU_chassisControl"]["MCU_ahlbEnable"]
if self.usesApillarHarness:
self.ahbLoBeamOn = epas_cp.vl["BODY_R1"]["LoBm_On_Rq"]
self.ahbHiBeamOn = epas_cp.vl["BODY_R1"]["HiBm_On"]
self.ahbNightMode = epas_cp.vl["BODY_R1"]["LgtSens_Night"]
else:
self.ahbLoBeamOn = cp.vl["BODY_R1"]["LoBm_On_Rq"]
self.ahbHiBeamOn = cp.vl["BODY_R1"]["HiBm_On"]
self.ahbNightMode = cp.vl["BODY_R1"]["LgtSens_Night"]
usu = cp.vl["UI_gpsVehicleSpeed"]["UI_userSpeedOffsetUnits"]
if usu == 1:
self.userSpeedLimitOffsetKph = cp.vl["UI_gpsVehicleSpeed"][
"UI_userSpeedOffset"]
else:
self.userSpeedLimitOffsetKph = (
cp.vl["UI_gpsVehicleSpeed"]["UI_userSpeedOffset"] *
CV.MPH_TO_KPH)
msu = cp.vl["UI_gpsVehicleSpeed"]["UI_mapSpeedLimitUnits"]
map_speed_uom_to_ms = CV.KPH_TO_MS if msu == 1 else CV.MPH_TO_MS
map_speed_ms_to_uom = CV.MS_TO_KPH if msu == 1 else CV.MS_TO_MPH
speed_limit_type = int(
cp.vl["UI_driverAssistMapData"]["UI_mapSpeedLimit"])
rdSignMsg = cp.vl["UI_driverAssistRoadSign"]["UI_roadSign"]
if rdSignMsg == 4: # ROAD_SIGN_SPEED_SPLINE
self.meanFleetSplineSpeedMPS = cp.vl["UI_driverAssistRoadSign"][
"UI_meanFleetSplineSpeedMPS"]
self.meanFleetSplineAccelMPS2 = cp.vl["UI_driverAssistRoadSign"][
"UI_meanFleetSplineAccelMPS2"]
self.medianFleetSpeedMPS = cp.vl["UI_driverAssistRoadSign"][
"UI_medianFleetSpeedMPS"]
self.splineLocConfidence = cp.vl["UI_driverAssistRoadSign"][
"UI_splineLocConfidence"]
self.UI_splineID = cp.vl["UI_driverAssistRoadSign"]["UI_splineID"]
self.rampType = cp.vl["UI_driverAssistRoadSign"]["UI_rampType"]
elif rdSignMsg == 3: # ROAD_SIGN_SPEED_LIMIT
self.topQrtlFleetSplineSpeedMPS = cp.vl["UI_driverAssistRoadSign"][
"UI_topQrtlFleetSpeedMPS"]
self.splineLocConfidence = cp.vl["UI_driverAssistRoadSign"][
"UI_splineLocConfidence"]
self.baseMapSpeedLimitMPS = cp.vl["UI_driverAssistRoadSign"][
"UI_baseMapSpeedLimitMPS"]
# we round the speed limit in the map's units of measurement to fix noisy data (there are no signs with a limit of 79.2 kph)
self.baseMapSpeedLimitMPS = (
int(self.baseMapSpeedLimitMPS * map_speed_ms_to_uom + 0.99) /
map_speed_ms_to_uom)
self.bottomQrtlFleetSpeedMPS = cp.vl["UI_driverAssistRoadSign"][
"UI_bottomQrtlFleetSpeedMPS"]
if self.baseMapSpeedLimitMPS > 0 and (
speed_limit_type != 0x1F or self.baseMapSpeedLimitMPS <= 5.56):
self.speed_limit_ms = (
self.baseMapSpeedLimitMPS
) # this one is earlier than the actual sign but can also be unreliable, so we ignore it on SNA at higher speeds
else:
self.speed_limit_ms = (
cp.vl["UI_gpsVehicleSpeed"]["UI_mppSpeedLimit"] *
map_speed_uom_to_ms)
self.DAS_fusedSpeedLimit = self._convert_to_DAS_fusedSpeedLimit(
self.speed_limit_ms * map_speed_ms_to_uom, speed_limit_type)
self.compute_speed()
# 2 = temporary 3= TBD 4 = temporary, hit a bump 5 (permanent) 6 = temporary 7 (permanent)
# TODO: Use values from DBC to parse this field
self.steer_error = epas_cp.vl["EPAS_sysStatus"][
"EPAS_steeringFault"] == 1
self.steer_not_allowed = epas_cp.vl["EPAS_sysStatus"][
"EPAS_eacStatus"] not in [
2,
1,
] # 2 "EAC_ACTIVE" 1 "EAC_AVAILABLE" 3 "EAC_FAULT" 0 "EAC_INHIBITED"
self.steer_warning = self.steer_not_allowed
self.brake_error = 0 # NOT WORKINGcp.vl[309]['ESP_brakeLamp'] #JCT
# JCT More ESP errors available, these needs to be added once car steers on its own to disable / alert driver
self.esp_disabled = 0 # NEED TO CORRECT DBC cp.vl[309]['ESP_espOffLamp'] or cp.vl[309]['ESP_tcOffLamp'] or cp.vl[309]['ESP_tcLampFlash'] or cp.vl[309]['ESP_espFaultLamp'] #JCT
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel_fl = 0 # JCT
self.v_wheel_fr = 0 # JCT
self.v_wheel_rl = 0 # JCT
self.v_wheel_rr = 0 # JCT
self.v_wheel = 0 # JCT
self.v_weight = 0 # JCT
self.imperial_speed_units = cp.vl["DI_state"]["DI_speedUnits"] == 0
speed_ms = cp.vl["DI_state"]["DI_analogSpeed"] * (
CV.MPH_TO_MS if self.imperial_speed_units else CV.KPH_TO_MS
) # car's displayed speed in m/s
if (abs(speed_ms - self.v_ego) > 2.0
): # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[speed_ms], [0.0]]
self.v_ego_raw = speed_ms
v_ego_x = self.v_ego_kf.update(speed_ms)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
# BB use this set for pedal work as the user_gas_xx is used in other places
self.prev_pedal_interceptor_state = self.pedal_interceptor_state
self.pedal_interceptor_state = pedal_cp.vl["GAS_SENSOR"]["STATE"]
self.pedal_interceptor_value = pedal_cp.vl["GAS_SENSOR"][
"INTERCEPTOR_GAS"]
self.pedal_interceptor_value2 = pedal_cp.vl["GAS_SENSOR"][
"INTERCEPTOR_GAS2"]
self.prev_pedal_idx = self.pedal_idx
self.pedal_idx = pedal_cp.vl["GAS_SENSOR"]["IDX"]
can_gear_shifter = cp.vl["DI_torque2"]["DI_gear"]
# self.angle_steers = -(cp.vl["STW_ANGLHP_STAT"]['StW_AnglHP']) #JCT polarity reversed from Honda/Acura
self.angle_steers = -(
epas_cp.vl["EPAS_sysStatus"]["EPAS_internalSAS"]
) # BB see if this works better than STW_ANGLHP_STAT for angle
self.angle_steers_rate = 0 # JCT
self.turn_signal_state_left = cp.vl["GTW_carState"][
"BC_indicatorLStatus"]
self.turn_signal_state_right = cp.vl["GTW_carState"][
"BC_indicatorRStatus"]
self.prev_turn_signal_blinking = self.turn_signal_blinking
self.turn_signal_blinking = (self.turn_signal_state_left == 1
or self.turn_signal_state_right == 1)
self.prev_turn_signal_stalk_state = self.turn_signal_stalk_state
self.turn_signal_stalk_state = (
0 if cp.vl["STW_ACTN_RQ"]["TurnIndLvr_Stat"] == 3 else int(
cp.vl["STW_ACTN_RQ"]["TurnIndLvr_Stat"]))
self.brake_hold = 0 # TODO
self.main_on = 1 # cp.vl["SCM_BUTTONS"]['MAIN_ON']
self.DI_cruiseSet = cp.vl["DI_state"]["DI_cruiseSet"]
if self.imperial_speed_units:
self.DI_cruiseSet = self.DI_cruiseSet * CV.MPH_TO_KPH
self.gear_shifter = parse_gear_shifter(can_gear_shifter,
self.CP.carFingerprint)
self.park_brake = self.gear_shifter == "park" # TODO
self.pedal_gas = (
0.0 # cp.vl["DI_torque1"]['DI_pedalPos'] / 102 #BB: to make it between 0..1
)
self.car_gas = self.pedal_gas
self.steer_override = abs(
epas_cp.vl["EPAS_sysStatus"]["EPAS_handsOnLevel"]) > 0
self.steer_torque_driver = 0 # JCT
# brake switch has shown some single time step noise, so only considered when
# switch is on for at least 2 consecutive CAN samples
# Todo / refactor: This shouldn't have to do with epas == 3..
# was wrongly set to epas_cp.vl["EPAS_sysStatus"]['EPAS_eacErrorCode'] == 3 and epas_cp.vl["EPAS_sysStatus"]['EPAS_eacStatus'] == 0
self.brake_switch = cp.vl["DI_torque2"]["DI_brakePedal"]
self.brake_pressed = cp.vl["DI_torque2"]["DI_brakePedal"]
self.standstill = cp.vl["DI_torque2"]["DI_vehicleSpeed"] == 0
self.torqueMotor = cp.vl["DI_torque1"]["DI_torqueMotor"]
self.pcm_acc_status = cp.vl["DI_state"]["DI_cruiseState"]
self.regenLight = cp.vl["DI_state"]["DI_regenLight"] == 1
self.v_cruise_actual = self.DI_cruiseSet
class CarState(object):
def __init__(self, CP):
self.gasMode = int(kegman.conf['lastGasMode'])
self.gasLabels = ["dynamic", "sport", "eco"]
self.alcaLabels = ["MadMax", "Normal", "Wifey", "off"]
self.alcaMode = int(kegman.conf['lastALCAMode'])
steerRatio = CP.steerRatio
self.prev_distance_button = 0
self.distance_button = 0
self.prev_lka_button = 0
self.lka_button = 0
self.lkMode = True
self.lane_departure_toggle_on = True
# ALCA PARAMS
self.blind_spot_on = bool(0)
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 32., 55.]
self.CL_MAX_ANGLE_DELTA = [
0.77 * 16.2 / steerRatio, 0.86 * 16.2 / steerRatio,
0.535 * 16.2 / steerRatio
]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 2.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 4.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.040
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = [1., 1.1, 1.2]
self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.2, 0.2, 0.2]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
# initialize can parser
self.CP = CP
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Subaru", "subaru")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
self.car_fingerprint = CP.carFingerprint
self.left_blinker_on = False
self.prev_left_blinker_on = False
self.right_blinker_on = False
self.prev_right_blinker_on = False
self.steer_torque_driver = 0
self.steer_not_allowed = False
self.main_on = False
# vEgo kalman filter
dt = 0.01
self.v_ego_kf = KF1D(x0=[[0.], [0.]],
A=[[1., dt], [0., 1.]],
C=[1., 0.],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.
#BB init ui buttons
def init_ui_buttons(self):
btns = []
btns.append(UIButton("sound", "SND", 0, "", 0))
btns.append(
UIButton("alca", "ALC", 0, self.alcaLabels[self.alcaMode], 1))
btns.append(UIButton("mad", "MAD", 0, "", 2))
btns.append(UIButton("", "", 0, "", 3))
btns.append(UIButton("gas", "GAS", 1, self.gasLabels[self.gasMode], 4))
btns.append(UIButton("lka", "LKA", 1, "", 5))
return btns
#BB update ui buttons
def update_ui_buttons(self, id, btn_status):
if self.cstm_btns.btns[id].btn_status > 0:
if (id == 1) and (btn_status == 0
) and self.cstm_btns.btns[id].btn_name == "alca":
if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[
self.alcaMode]:
self.alcaMode = (self.alcaMode + 1) % 4
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
else:
self.alcaMode = 0
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[
self.alcaMode]
self.cstm_btns.hasChanges = True
if self.alcaMode == 3:
self.cstm_btns.set_button_status("alca", 0)
elif (id == 4) and (
btn_status
== 0) and self.cstm_btns.btns[id].btn_name == "gas":
if self.cstm_btns.btns[id].btn_label2 == self.gasLabels[
self.gasMode]:
self.gasMode = (self.gasMode + 1) % 3
kegman.save({'lastGasMode': int(self.gasMode)
}) # write last GasMode setting to file
else:
self.gasMode = 0
kegman.save({'lastGasMode': int(self.gasMode)
}) # write last GasMode setting to file
self.cstm_btns.btns[id].btn_label2 = self.gasLabels[
self.gasMode]
self.cstm_btns.hasChanges = True
else:
self.cstm_btns.btns[
id].btn_status = btn_status * self.cstm_btns.btns[
id].btn_status
else:
self.cstm_btns.btns[id].btn_status = btn_status
if (id == 1) and self.cstm_btns.btns[id].btn_name == "alca":
self.alcaMode = (self.alcaMode + 1) % 4
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[
self.alcaMode]
self.cstm_btns.hasChanges = True
def update(self, cp, cp_cam):
self.can_valid = cp.can_valid
self.cam_can_valid = cp_cam.can_valid
self.pedal_gas = cp.vl["Throttle"]['Throttle_Pedal']
self.brake_pressure = cp.vl["Brake_Pedal"]['Brake_Pedal']
self.user_gas_pressed = self.pedal_gas > 0
self.brake_pressed = self.brake_pressure > 0
self.brake_lights = bool(self.brake_pressed)
self.v_wheel_fl = cp.vl["Wheel_Speeds"]['FL'] * CV.KPH_TO_MS
self.v_wheel_fr = cp.vl["Wheel_Speeds"]['FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS
self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]['Cruise_Set_Speed']
# 1 = imperial, 6 = metric
if cp.vl["Dash_State"]['Units'] == 1:
self.v_cruise_pcm *= CV.MPH_TO_KPH
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl +
self.v_wheel_rr) / 4.
# Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default
if abs(
v_wheel - self.v_ego
) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
self.standstill = self.v_ego_raw < 0.01
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.left_blinker_on = cp.vl["Dashlights"]['LEFT_BLINKER'] == 1
self.right_blinker_on = cp.vl["Dashlights"]['RIGHT_BLINKER'] == 1
self.seatbelt_unlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1
self.steer_torque_driver = cp.vl["Steering_Torque"][
'Steer_Torque_Sensor']
self.steer_torque_motor = cp.vl["Steering_Torque"][
'Steer_Torque_Output']
self.acc_active = cp.vl["CruiseControl"]['Cruise_Activated']
self.main_on = cp.vl["CruiseControl"]['Cruise_On']
self.steer_override = abs(
self.steer_torque_driver) > STEER_THRESHOLD[self.car_fingerprint]
self.angle_steers = cp.vl["Steering_Torque"]['Steering_Angle']
self.door_open = any([
cp.vl["BodyInfo"]['DOOR_OPEN_RR'],
cp.vl["BodyInfo"]['DOOR_OPEN_RL'],
cp.vl["BodyInfo"]['DOOR_OPEN_FR'],
cp.vl["BodyInfo"]['DOOR_OPEN_FL']
])
self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
if self.car_fingerprint not in (CAR.OUTBACK, CAR.LEGACY):
self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"][
"Cruise_Set_Speed"] * CV.MPH_TO_KPH
self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
else:
self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"]
self.steer_not_allowed = cp.vl["Steering_Torque"]["LKA_Lockout"]
if self.cstm_btns.get_button_status("lka") == 0:
self.lane_departure_toggle_on = False
else:
if self.alcaMode == 3 and (self.left_blinker_on
or self.right_blinker_on):
self.lane_departure_toggle_on = False
else:
self.lane_departure_toggle_on = True
def __init__(self, CP):
#if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 44.]
self.CL_MAX_ANGLE_DELTA = [1.8, .3]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 1.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.042
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = 1.
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7, 0.30, 0.20]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
self.user_gas, self.user_gas_pressed = 0., 0
self.brake_switch_prev = 0
self.brake_switch_ts = 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.stopped = 0
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Honda", "honda")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
# 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
class CarState(object):
def __init__(self, CP):
self.speed_control_enabled = 0
self.CL_MIN_V = 8.9
self.CL_MAX_A = 20.
# labels for buttons
self.btns_init = [["alca", "ALC", ["MadMax", "Normal", "Calm"]],
[
ACCMode.BUTTON_NAME, ACCMode.BUTTON_ABREVIATION,
ACCMode.labels()
], ["dsp", "DSP", ["OP", "MIN", "OFF", "GYRO"]],
["", "", [""]], ["msg", "MSG", [""]],
["sound", "SND", [""]]]
### START OF MAIN CONFIG OPTIONS ###
### Do NOT modify here, modify in /data/bb_openpilot.cfg and reboot
self.forcePedalOverCC = True
self.enableHSO = True
self.enableALCA = True
self.enableDasEmulation = True
self.enableRadarEmulation = True
self.enableSpeedVariableDesAngle = False
self.enableRollAngleCorrection = False
self.enableFeedForwardAngleCorrection = True
self.enableDriverMonitor = True
self.enableShowCar = True
self.enableShowLogo = True
self.hasNoctuaFan = False
self.limitBatteryMinMax = False
self.limitBattery_Min = 60
self.limitBattery_Max = 70
self.doAutoUpdate = True
self.blockUploadWhileTethering = False
self.tetherIP = "127.0.0."
self.useTeslaGPS = False
self.useTeslaMapData = False
self.hasTeslaIcIntegration = False
self.useTeslaRadar = False
self.useWithoutHarness = False
self.radarVIN = " "
self.enableLdw = True
self.radarOffset = 0.
self.radarPosition = 0
self.radarEpasType = 0
self.fix1916 = False
self.forceFingerprintTesla = False
self.eonToFront = 0.9
#read config file
read_config_file(self)
### END OF MAIN CONFIG OPTIONS ###
self.apEnabled = True
self.apFollowTimeInS = 2.5 #time in seconds to follow
self.keepEonOff = False
self.alcaEnabled = True
self.mapAwareSpeed = False
# Tesla Model
self.teslaModelDetected = 1
self.teslaModel = read_db('/data/params', 'TeslaModel')
if self.teslaModel is None:
self.teslaModel = "S"
self.teslaModelDetected = 0
# for map integration
self.csaRoadCurvC3 = 0.
self.csaRoadCurvC2 = 0.
self.csaRoadCurvRange = 0.
self.csaRoadCurvUsingTspline = 0
self.csaOfframpCurvC3 = 0.
self.csaOfframpCurvC2 = 0.
self.csaOfframpCurvRange = 0.
self.csaOfframpCurvUsingTspline = 0
self.roadCurvHealth = 0
self.roadCurvRange = 0.
self.roadCurvC0 = 0.
self.roadCurvC1 = 0.
self.roadCurvC2 = 0.
self.roadCurvC3 = 0.
self.speedLimitUnits = 0
self.speedLimit = 0
self.meanFleetSplineSpeedMPS = 0.
self.meanFleetSplineAccelMPS2 = 0.
self.medianFleetSpeedMPS = 0.
self.topQrtlFleetSplineSpeedMPS = 0.
self.splineLocConfidence = 0
self.baseMapSpeedLimitMPS = 0.
self.bottomQrtlFleetSpeedMPS = 0.
self.rampType = 0
self.mapBasedSuggestedSpeed = 0.
self.splineBasedSuggestedSpeed = 0.
self.maxdrivespeed = 0.
self.gpsLongitude = 0.
self.gpsLatitude = 0.
self.gpsAccuracy = 0.
self.gpsElevation = 0.
self.gpsHeading = 0.
self.gpsVehicleSpeed = 0.
self.userSpeedLimitKph = 0.
self.userSpeedLimitOffsetKph = 0.
self.brake_only = CP.enableCruise
self.last_cruise_stalk_pull_time = 0
self.CP = CP
self.user_gas = 0.
self.user_gas_pressed = False
self.pedal_interceptor_state = 0
self.pedal_interceptor_value = 0.
self.pedal_interceptor_value2 = 0.
self.pedal_interceptor_missed_counter = 0
self.brake_switch_prev = 0
self.brake_switch_ts = 0
self.cruise_buttons = 0
self.blinker_on = 0
self.left_blinker_on = 0
self.right_blinker_on = 0
self.steer_warning = 0
self.stopped = 0
# variables used for the fake DAS creation
self.DAS_info_frm = -1
self.DAS_info_msg = 0
self.DAS_status_frm = 0
self.DAS_status_idx = 0
self.DAS_status2_frm = 0
self.DAS_status2_idx = 0
self.DAS_bodyControls_frm = 0
self.DAS_bodyControls_idx = 0
self.DAS_lanes_frm = 0
self.DAS_lanes_idx = 0
self.DAS_objects_frm = 0
self.DAS_objects_idx = 0
self.DAS_pscControl_frm = 0
self.DAS_pscControl_idx = 0
self.DAS_warningMatrix0_idx = 0
self.DAS_warningMatrix1_idx = 0
self.DAS_warningMatrix3_idx = 0
self.DAS_telemetryPeriodic1_idx = 0
self.DAS_telemetryPeriodic2_idx = 0
self.DAS_telemetryEvent1_idx = 0
self.DAS_telemetryEvent2_idx = 0
self.DAS_control_idx = 0
#BB notification messages for DAS
self.DAS_canErrors = 0
self.DAS_plannerErrors = 0
self.DAS_doorOpen = 0
self.DAS_notInDrive = 0
#BB variables for pedal CC
self.pedal_speed_kph = 0.
# Pedal mode is ready, i.e. hardware is present and normal cruise is off.
self.pedal_interceptor_available = False
self.prev_pedal_interceptor_available = False
#BB UIEvents
self.UE = UIEvents(self)
#BB PCC
self.regenLight = 0
self.torqueLevel = 0.
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Tesla Model S", "tesla",
self.enableShowLogo, self.enableShowCar)
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
#BB steering_wheel_stalk last position, used by ACC and ALCA
self.steering_wheel_stalk = None
#BB carConfig data used to change IC info
self.real_carConfig = None
self.real_dasHw = 0
#BB visiond last type
self.last_visiond = self.cstm_btns.btns[3].btn_label2
# 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
self.imperial_speed_units = True
# The current max allowed cruise speed. Actual cruise speed sent to the car
# may be lower, depending on traffic.
self.v_cruise_pcm = 0.0
# Actual cruise speed currently active on the car.
self.v_cruise_actual = 0.0
#ALCA params
self.ALCA_enabled = False
self.ALCA_total_steps = 100
self.ALCA_direction = 0
self.ALCA_error = False
self.angle_offset = 0.
self.init_angle_offset = False
def config_ui_buttons(self, pedalPresent):
if pedalPresent:
self.btns_init[1] = [
PCCModes.BUTTON_NAME, PCCModes.BUTTON_ABREVIATION,
PCCModes.labels()
]
else:
# we don't have pedal interceptor
self.btns_init[1] = [
ACCMode.BUTTON_NAME, ACCMode.BUTTON_ABREVIATION,
ACCMode.labels()
]
btn = self.cstm_btns.btns[1]
btn.btn_name = self.btns_init[1][0]
btn.btn_label = self.btns_init[1][1]
btn.btn_label2 = self.btns_init[1][2][0]
btn.btn_status = 1
self.cstm_btns.update_ui_buttons(1, 1)
def compute_speed(self):
# if one of them is zero, select max of the two
if self.meanFleetSplineSpeedMPS == 0 or self.medianFleetSpeedMPS == 0:
self.splineBasedSuggestedSpeed = max(self.meanFleetSplineSpeedMPS,
self.medianFleetSpeedMPS)
else:
self.splineBasedSuggestedSpeed = (
self.splineLocConfidence * self.meanFleetSplineSpeedMPS +
(100 - self.splineLocConfidence) *
self.medianFleetSpeedMPS) / 100.
# if confidence over 60%, then weight between bottom speed and top speed
# if less than 40% then use map data
if self.splineLocConfidence > 60:
self.mapBasedSuggestedSpeed = (
self.splineLocConfidence * self.meanFleetSplineSpeedMPS +
(100 - self.splineLocConfidence) *
self.bottomQrtlFleetSpeedMPS) / 100.
else:
self.mapBasedSuggestedSpeed = self.baseMapSpeedLimitMPS
if self.rampType > 0:
#we are on a ramp, use the spline info if available
if self.splineBasedSuggestedSpeed > 0:
self.maxdrivespeed = self.splineBasedSuggestedSpeed
else:
self.maxdrivespeed = self.mapBasedSuggestedSpeed
else:
#we are on a normal road, use max of the two
self.maxdrivespeed = max(self.mapBasedSuggestedSpeed,
self.splineBasedSuggestedSpeed)
def update_ui_buttons(self, btn_id, btn_status):
# we only focus on btn_id=3, which is for visiond
if (btn_id == 3) and (self.cstm_btns.btns[btn_id].btn_status > 0) and (
self.last_visiond != self.cstm_btns.btns[btn_id].btn_label2):
self.last_visiond = self.cstm_btns.btns[btn_id].btn_label2
# we switched between wiggly and normal
args = [
"/data/openpilot/selfdrive/car/modules/ch_visiond.sh",
self.cstm_btns.btns[btn_id].btn_label2
]
subprocess.Popen(args,
shell=False,
stdin=None,
stdout=None,
stderr=None,
env=dict(os.environ),
close_fds=True)
def update(self, cp, epas_cp, pedal_cp):
# 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_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.steering_wheel_stalk = cp.vl["STW_ACTN_RQ"]
self.real_carConfig = cp.vl["GTW_carConfig"]
self.real_dasHw = cp.vl["GTW_carConfig"]['GTW_dasHw']
self.cruise_buttons = cp.vl["STW_ACTN_RQ"]['SpdCtrlLvr_Stat']
# ******************* parse out can *******************
self.door_all_closed = not any([
cp.vl["GTW_carState"]['DOOR_STATE_FL'],
cp.vl["GTW_carState"]['DOOR_STATE_FR'],
cp.vl["GTW_carState"]['DOOR_STATE_RL'],
cp.vl["GTW_carState"]['DOOR_STATE_RR']
]) #JCT
self.seatbelt = cp.vl["SDM1"]['SDM_bcklDrivStatus']
#self.seatbelt = cp.vl["SDM1"]['SDM_bcklDrivStatus'] and cp.vl["GTW_status"]['GTW_driverPresent']
if (cp.vl["GTW_carConfig"]['GTW_performanceConfig']) and (
cp.vl["GTW_carConfig"]['GTW_performanceConfig'] > 0):
prev_teslaModel = self.teslaModel
self.teslaModel = "S"
if (cp.vl["GTW_carConfig"]['GTW_performanceConfig'] > 1):
self.teslaModel = self.teslaModel + "P"
if (cp.vl["GTW_carConfig"]['GTW_fourWheelDrive'] == 1):
self.teslaModel = self.teslaModel + "D"
if (self.teslaModelDetected
== 0) or (prev_teslaModel != self.teslaModel):
write_db('/data/params', 'TeslaModel', self.teslaModel)
self.teslaModelDetected = 1
#Nav Map Data
self.csaRoadCurvC3 = cp.vl['UI_csaRoadCurvature']["UI_csaRoadCurvC3"]
self.csaRoadCurvC2 = cp.vl['UI_csaRoadCurvature']["UI_csaRoadCurvC2"]
self.csaRoadCurvRange = cp.vl['UI_csaRoadCurvature'][
"UI_csaRoadCurvRange"]
self.csaRoadCurvUsingTspline = cp.vl['UI_csaRoadCurvature'][
"UI_csaRoadCurvUsingTspline"]
self.csaOfframpCurvC3 = cp.vl['UI_csaOfframpCurvature'][
"UI_csaOfframpCurvC3"]
self.csaOfframpCurvC2 = cp.vl['UI_csaOfframpCurvature'][
"UI_csaOfframpCurvC2"]
self.csaOfframpCurvRange = cp.vl['UI_csaOfframpCurvature'][
"UI_csaOfframpCurvRange"]
self.csaOfframpCurvUsingTspline = cp.vl['UI_csaOfframpCurvature'][
"UI_csaOfframpCurvUsingTspline"]
self.roadCurvHealth = cp.vl['UI_roadCurvature']["UI_roadCurvHealth"]
self.roadCurvRange = cp.vl['UI_roadCurvature']["UI_roadCurvRange"]
self.roadCurvC0 = cp.vl['UI_roadCurvature']["UI_roadCurvC0"]
self.roadCurvC1 = cp.vl['UI_roadCurvature']["UI_roadCurvC1"]
self.roadCurvC2 = cp.vl['UI_roadCurvature']["UI_roadCurvC2"]
self.roadCurvC3 = cp.vl['UI_roadCurvature']["UI_roadCurvC3"]
self.gpsLongitude = cp.vl['MCU_locationStatus']["MCU_longitude"]
self.gpsLatitude = cp.vl['MCU_locationStatus']["MCU_latitude"]
self.gpsAccuracy = cp.vl['MCU_locationStatus']["MCU_gpsAccuracy"]
self.gpsElevation = cp.vl['MCU_locationStatus2']["MCU_elevation"]
self.gpsHeading = cp.vl['MCU_gpsVehicleSpeed']["MCU_gpsVehicleHeading"]
self.gpsVehicleSpeed = cp.vl['MCU_gpsVehicleSpeed'][
"MCU_gpsVehicleSpeed"] * CV.KPH_TO_MS
if (self.hasTeslaIcIntegration):
self.apEnabled = (
cp.vl["MCU_chassisControl"]["MCU_latControlEnable"] == 1)
self.apFollowTimeInS = 1 + cp.vl["MCU_chassisControl"][
"MCU_fcwSensitivity"] * 0.5
self.keepEonOff = cp.vl["MCU_chassisControl"]["MCU_ldwEnable"] == 1
self.alcaEnabled = cp.vl["MCU_chassisControl"][
"MCU_pedalSafetyEnable"] == 1
self.mapAwareSpeed = cp.vl["MCU_chassisControl"][
"MCU_aebEnable"] == 1
usu = cp.vl['MCU_gpsVehicleSpeed']["MCU_userSpeedOffsetUnits"]
if usu == 1:
self.userSpeedLimitOffsetKph = cp.vl['MCU_gpsVehicleSpeed'][
"MCU_userSpeedOffset"]
else:
self.userSpeedLimitOffsetKph = cp.vl['MCU_gpsVehicleSpeed'][
"MCU_userSpeedOffset"] * CV.MPH_TO_KPH
msu = cp.vl['MCU_gpsVehicleSpeed']["MCU_mapSpeedLimitUnits"]
if msu == 1:
self.userSpeedLimitKph = cp.vl['MCU_gpsVehicleSpeed'][
"MCU_mppSpeedLimit"]
else:
self.userSpeedLimitKph = cp.vl['MCU_gpsVehicleSpeed'][
"MCU_mppSpeedLimit"] * CV.MPH_TO_KPH
speed_limit_tesla_lookup = [
0, 5, 7, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75,
80, 85, 90, 95, 100, 105, 110, 115, 120, 130, 140, 150, 160, 0, 0
]
self.speedLimitUnits = cp.vl["UI_driverAssistMapData"][
"UI_mapSpeedUnits"]
self.speedLimitKph = speed_limit_tesla_lookup[int(
cp.vl["UI_driverAssistMapData"]["UI_mapSpeedLimit"])] * (
1 + 0.609 * (1 - self.speedLimitUnits))
rdSignMsg = cp.vl["UI_driverAssistRoadSign"]["UI_roadSign"]
if rdSignMsg == 4:
self.meanFleetSplineSpeedMPS = cp.vl["UI_driverAssistRoadSign"][
"UI_meanFleetSplineSpeedMPS"]
self.meanFleetSplineAccelMPS2 = cp.vl["UI_driverAssistRoadSign"][
"UI_meanFleetSplineAccelMPS2"]
self.medianFleetSpeedMPS = cp.vl["UI_driverAssistRoadSign"][
"UI_medianFleetSpeedMPS"]
self.splineLocConfidence = cp.vl["UI_driverAssistRoadSign"][
"UI_splineLocConfidence"]
self.rampType = cp.vl["UI_driverAssistRoadSign"]["UI_rampType"]
if rdSignMsg == 3:
self.topQrtlFleetSplineSpeedMPS = cp.vl["UI_driverAssistRoadSign"][
"UI_topQrtlFleetSpeedMPS"]
self.splineLocConfidence = cp.vl["UI_driverAssistRoadSign"][
"UI_splineLocConfidence"]
self.baseMapSpeedLimitMPS = cp.vl["UI_driverAssistRoadSign"][
"UI_baseMapSpeedLimitMPS"]
self.bottomQrtlFleetSpeedMPS = cp.vl["UI_driverAssistRoadSign"][
"UI_bottomQrtlFleetSpeedMPS"]
self.compute_speed()
# 2 = temporary 3= TBD 4 = temporary, hit a bump 5 (permanent) 6 = temporary 7 (permanent)
# TODO: Use values from DBC to parse this field
self.steer_error = epas_cp.vl["EPAS_sysStatus"][
'EPAS_steeringFault'] == 1
self.steer_not_allowed = epas_cp.vl["EPAS_sysStatus"][
'EPAS_eacStatus'] not in [
2, 1
] # 2 "EAC_ACTIVE" 1 "EAC_AVAILABLE" 3 "EAC_FAULT" 0 "EAC_INHIBITED"
self.steer_warning = self.steer_not_allowed
self.brake_error = 0 #NOT WORKINGcp.vl[309]['ESP_brakeLamp'] #JCT
# JCT More ESP errors available, these needs to be added once car steers on its own to disable / alert driver
self.esp_disabled = 0 #NEED TO CORRECT DBC cp.vl[309]['ESP_espOffLamp'] or cp.vl[309]['ESP_tcOffLamp'] or cp.vl[309]['ESP_tcLampFlash'] or cp.vl[309]['ESP_espFaultLamp'] #JCT
# calc best v_ego estimate, by averaging two opposite corners
self.v_wheel_fl = 0 #JCT
self.v_wheel_fr = 0 #JCT
self.v_wheel_rl = 0 #JCT
self.v_wheel_rr = 0 #JCT
self.v_wheel = 0 #JCT
self.v_weight = 0 #JCT
speed = (
cp.vl["DI_torque2"]['DI_vehicleSpeed']
) * CV.MPH_TO_KPH / 3.6 #JCT MPH_TO_MS. Tesla is in MPH, v_ego is expected in M/S
speed = speed * 1.01 # To match car's displayed speed
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])
#BB use this set for pedal work as the user_gas_xx is used in other places
self.pedal_interceptor_state = pedal_cp.vl["GAS_SENSOR"]['STATE']
self.pedal_interceptor_value = pedal_cp.vl["GAS_SENSOR"][
'INTERCEPTOR_GAS']
self.pedal_interceptor_value2 = pedal_cp.vl["GAS_SENSOR"][
'INTERCEPTOR_GAS2']
can_gear_shifter = cp.vl["DI_torque2"]['DI_gear']
self.gear = 0 # JCT
# self.angle_steers = -(cp.vl["STW_ANGLHP_STAT"]['StW_AnglHP']) #JCT polarity reversed from Honda/Acura
self.angle_steers = -(
epas_cp.vl["EPAS_sysStatus"]['EPAS_internalSAS']
) #BB see if this works better than STW_ANGLHP_STAT for angle
self.angle_steers_rate = 0 #JCT
self.blinker_on = (cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat']
== 1) or (cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat']
== 2)
self.left_blinker_on = cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 1
self.right_blinker_on = cp.vl["STW_ACTN_RQ"]['TurnIndLvr_Stat'] == 2
#if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY):
# self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
# self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
# self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
#else:
self.park_brake = 0 # TODO
self.brake_hold = 0 # TODO
self.main_on = 1 #cp.vl["SCM_BUTTONS"]['MAIN_ON']
self.imperial_speed_units = cp.vl["DI_state"]['DI_speedUnits'] == 0
self.DI_cruiseSet = cp.vl["DI_state"]['DI_cruiseSet']
if self.imperial_speed_units:
self.DI_cruiseSet = self.DI_cruiseSet * CV.MPH_TO_KPH
self.cruise_speed_offset = calc_cruise_offset(
self.DI_cruiseSet * CV.KPH_TO_MS, self.v_ego)
self.gear_shifter = parse_gear_shifter(can_gear_shifter,
self.CP.carFingerprint)
self.pedal_gas = 0. # cp.vl["DI_torque1"]['DI_pedalPos'] / 102 #BB: to make it between 0..1
self.car_gas = self.pedal_gas
self.steer_override = abs(
epas_cp.vl["EPAS_sysStatus"]['EPAS_handsOnLevel']) > 0
self.steer_torque_driver = 0 #JCT
# brake switch has shown some single time step noise, so only considered when
# switch is on for at least 2 consecutive CAN samples
# Todo / refactor: This shouldn't have to do with epas == 3..
# was wrongly set to epas_cp.vl["EPAS_sysStatus"]['EPAS_eacErrorCode'] == 3 and epas_cp.vl["EPAS_sysStatus"]['EPAS_eacStatus'] == 0
self.brake_switch = cp.vl["DI_torque2"]['DI_brakePedal']
self.brake_pressed = cp.vl["DI_torque2"]['DI_brakePedal']
self.standstill = cp.vl["DI_torque2"]['DI_vehicleSpeed'] == 0
self.torqueMotor = cp.vl["DI_torque1"]['DI_torqueMotor']
self.pcm_acc_status = cp.vl["DI_state"]['DI_cruiseState']
self.regenLight = cp.vl["DI_state"]['DI_regenLight'] == 1
self.prev_pedal_interceptor_available = self.pedal_interceptor_available
pedal_has_value = bool(self.pedal_interceptor_value) or bool(
self.pedal_interceptor_value2)
pedal_interceptor_present = self.pedal_interceptor_state in [
0, 5
] and pedal_has_value
# Add loggic if we just miss some CAN messages so we don't immediately disable pedal
if pedal_has_value:
self.pedal_interceptor_missed_counter = 0
if pedal_interceptor_present:
self.pedal_interceptor_missed_counter = 0
else:
self.pedal_interceptor_missed_counter += 1
pedal_interceptor_present = pedal_interceptor_present and (
self.pedal_interceptor_missed_counter < 10)
# Mark pedal unavailable while traditional cruise is on.
self.pedal_interceptor_available = pedal_interceptor_present and (
self.forcePedalOverCC or not bool(self.pcm_acc_status))
if self.pedal_interceptor_available != self.prev_pedal_interceptor_available:
self.config_ui_buttons(self.pedal_interceptor_available)
self.v_cruise_actual = self.DI_cruiseSet
self.hud_lead = 0 #JCT
self.cruise_speed_offset = calc_cruise_offset(self.v_cruise_pcm,
self.v_ego)