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)
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
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
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
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'])
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 __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.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
