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())
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):
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
