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 # 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, CP): # initialize can parser self.CP = CP self.car_fingerprint = CP.carFingerprint self.left_blinker_on = False self.prev_left_blinker_on = False self.right_blinker_on = False self.prev_right_blinker_on = False # vEgo kalman filter dt = 0.01 self.v_ego_kf = KF1D(x0=[[0.], [0.]], A=[[1., dt], [0., 1.]], C=[1., 0.], K=[[0.12287673], [0.29666309]]) self.v_ego = 0.
def __init__(self, CP): self.CP = CP self.car_fingerprint = CP.carFingerprint self.out = car.CarState.new_message() self.cruise_buttons = 0 self.left_blinker_cnt = 0 self.right_blinker_cnt = 0 self.left_blinker_prev = False self.right_blinker_prev = False # 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_CTRL], [0.0, 1.0]], C=[1.0, 0.0], K=[[0.12287673], [0.29666309]])
def __init__(self, CP): self.CP = CP 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=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
def __init__(self, CP): self.CP = CP # initialize can parser self.car_fingerprint = CP.carFingerprint self.left_blinker_on = 0 self.left_blinker_flash = 0 self.right_blinker_on = 0 self.right_blinker_flash = 0 self.lca_left = 0 self.lca_right = 0 self.no_radar = CP.sccBus == -1 self.mdps_bus = CP.mdpsBus self.sas_bus = CP.sasBus self.scc_bus = CP.sccBus self.blinker_timer = 0 self.blinker_status = 0 self.lkas_LdwsSysState = 0 self.lkas_LdwsLHWarning = 0 self.lkas_LdwsRHWarning = 0 self.clu_CruiseSwState = 0 self.cruise_set_speed = 0 self.cruise_set_speed_kph = 0 self.curise_set_first = 0 self.curise_sw_check = 0 self.prev_clu_CruiseSwState = 0 self.prev_VSetDis = 30 self.cruise_set_mode = 0 self.driverAcc_time = 0 # 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_CTRL], [0.0, 1.0]], C=[1.0, 0.0], K=[[0.12287673], [0.29666309]])
def __init__(self, CP): 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 # 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
def __init__(self, CP, canbus): # initialize can parser self.CP = CP self.car_fingerprint = CP.carFingerprint self.left_blinker_on = False self.prev_left_blinker_on = False self.right_blinker_on = False self.prev_right_blinker_on = False self.steer_torque_driver = 0 self.steer_not_allowed = False self.main_on = False # vEgo kalman filter dt = 0.01 self.v_ego_kf = KF1D(x0=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, CP): self.CP = CP self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"] self.steer_status_values = defaultdict( lambda: "UNKNOWN", self.can_define.dv["STEER_STATUS"]["STEER_STATUS"]) self.user_gas, self.user_gas_pressed = 0., 0 self.brake_switch_prev = 0 self.brake_switch_ts = 0 self.cruise_buttons = 0 self.cruise_setting = 0 self.v_cruise_pcm_prev = 0 self.blinker_on = 0 self.left_blinker_on = 0 self.right_blinker_on = 0 self.cruise_mode = 0 self.stopped = 0 ### START OF MAIN CONFIG OPTIONS ### ### Do NOT modify here, modify in /data/honda_openpilot.cfg and reboot self.useTeslaRadar = False self.radarVIN = " " self.radarOffset = 0 #read config file read_config_file(self) ### END OF MAIN CONFIG OPTIONS ### # 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.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=[[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.left_blinker_on = 0 self.left_blinker_flash = 0 self.right_blinker_on = 0 self.right_blinker_flash = 0 self.no_radar = self.CP.carFingerprint in FEATURES["non_scc"]
def __init__(self, CP): self.CP = CP # initialize can parser self.car_fingerprint = CP.carFingerprint self.left_blinker_on = 0 self.left_blinker_flash = 0 self.right_blinker_on = 0 self.right_blinker_flash = 0 self.lca_left = 0 self.lca_right = 0 self.no_radar = CP.sccBus == -1 self.mdps_bus = CP.mdpsBus self.sas_bus = CP.sasBus self.scc_bus = CP.sccBus # 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_CTRL], [0.0, 1.0]], C=[1.0, 0.0], K=[[0.12287673], [0.29666309]])
def __init__(self, CP): self.kegman = KegmanConf() self.trMode = int(self.kegman.conf['lastTrMode'] ) # default to last distance interval on startup #self.trMode = 1 self.lkMode = True self.read_distance_lines_prev = 4 self.CP = CP self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"] self.steer_status_values = defaultdict( lambda: "UNKNOWN", self.can_define.dv["STEER_STATUS"]["STEER_STATUS"]) self.user_gas, self.user_gas_pressed = 0., 0 self.brake_switch_prev = 0 self.brake_switch_ts = 0 self.lead_distance = 255 self.hud_lead = 0 self.cruise_buttons = 0 self.cruise_setting = 0 self.v_cruise_pcm_prev = 0 self.blinker_on = 0 self.left_blinker_on = 0 self.right_blinker_on = 0 self.cruise_mode = 0 self.stopped = 0 # vEgo kalman filter dt = 0.01 # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # R = 1e3 self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], A=[[1.0, dt], [0.0, 1.0]], C=[1.0, 0.0], K=[[0.12287673], [0.29666309]]) self.v_ego = 0.0
def setUp(self): dt = 0.01 x0_0 = 0.0 x1_0 = 0.0 A0_0 = 1.0 A0_1 = dt A1_0 = 0.0 A1_1 = 1.0 C0_0 = 1.0 C0_1 = 0.0 K0_0 = 0.12287673 K1_0 = 0.29666309 self.kf_old = KF1D_old(x0=np.matrix([[x0_0], [x1_0]]), A=np.matrix([[A0_0, A0_1], [A1_0, A1_1]]), C=np.matrix([C0_0, C0_1]), K=np.matrix([[K0_0], [K1_0]])) self.kf = KF1D(x0=[[x0_0], [x1_0]], A=[[A0_0, A0_1], [A1_0, A1_1]], C=[C0_0, C0_1], K=[[K0_0], [K1_0]])
def __init__(self, CP): self.CP = CP 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.blinker_on = 0 self.left_blinker_on = 0 self.right_blinker_on = 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
def __init__(self, CP): self.gasbuttonstatus = 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.angle_offset = 0. self.init_angle_offset = False self.v_cruise_pcmlast = 41 self.pcm_acc_status = False self.setspeedoffset = 34.0 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 ] if not travis: self.traffic_data_sock = messaging.pub_sock( service_list['liveTrafficData'].port) self.arne182Status_sock = messaging.pub_sock( service_list['arne182Status'].port) # 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.CP = CP self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) print("DBC PARSED: %s" % DBC[CP.carFingerprint]['pt']) self.shifter_values = 'D' #self.can_define.dv["GEAR_PACKET"]['GEAR'] self.ENGAGED = False # 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 self.pcm_acc_status = 0 self.pcm_acc_active = False self.v_cruise_pcm = 0
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.
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., 32., 44.]#[10., 44.] self.CL_MAX_ANGLE_DELTA = [2.0, 0.96, 0.4] # adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother self.CL_ADJUST_FACTOR_BP = [10., 44.] self.CL_ADJUST_FACTOR = [16. , 8.] # reenrey angle when to let go self.CL_REENTRY_ANGLE_BP = [10., 44.] self.CL_REENTRY_ANGLE = [5. , 5.] # a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line self.CL_LANE_DETECT_BP = [10., 44.] self.CL_LANE_DETECT_FACTOR = [1.3, 1.3] self.CL_LANE_PASS_BP = [10., 20., 44.] self.CL_LANE_PASS_TIME = [40.,10., 3.] # change lane delta angles and other params self.CL_MAXD_BP = [10., 32., 44.] self.CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75 self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s # do not turn if actuator wants more than x deg for going straight; this should be interp based on speed self.CL_MAX_A_BP = [10., 44.] self.CL_MAX_A = [10., 10.] # define limits for angle change every 0.1 s # we need to force correction above 10 deg but less than 20 # anything more means we are going to steep or not enough in a turn self.CL_MAX_ACTUATOR_DELTA = 2. self.CL_MIN_ACTUATOR_DELTA = 0. self.CL_CORRECTION_FACTOR = [1.3,1.2,1.2] self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.] #duration after we cross the line until we release is a factor of speed self.CL_TIMEA_BP = [10., 32., 44.] self.CL_TIMEA_T = [0.7 ,0.30, 0.20] #duration to wait (in seconds) with blinkers on before starting to turn self.CL_WAIT_BEFORE_START = 1 #END OF ALCA PARAMS self.CP = CP #BB UIEvents self.UE = UIEvents(self) #BB variable for custom buttons self.cstm_btns = UIButtons(self,"Hyundai","hyundai") #BB pid holder for ALCA self.pid = None #BB custom message counter self.custom_alert_counter = 100 #set to 100 for 1 second display; carcontroller will take down to zero # initialize can parser self.car_fingerprint = CP.carFingerprint # vEgo kalman filter dt = 0.01 # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # R = 1e3 self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]), A=np.matrix([[1.0, dt], [0.0, 1.0]]), C=np.matrix([1.0, 0.0]), K=np.matrix([[0.12287673], [0.29666309]])) self.v_ego = 0.0 self.left_blinker_on = 0 self.left_blinker_flash = 0 self.right_blinker_on = 0 self.right_blinker_flash = 0
def __init__(self, CP): 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
def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned, measured, steer_override): if self.initted: # pylint: disable=access-member-before-definition self.dPathPrev = self.dPath self.vLeadPrev = self.vLead self.vRelPrev = self.vRel # relative values, copy self.dRel = d_rel # LONG_DIST self.yRel = y_rel # -LAT_DIST self.vRel = v_rel # REL_SPEED self.measured = measured # measured or estimate # compute distance to path self.dPath = d_path # computed velocity and accelerations self.vLead = self.vRel + v_ego_t_aligned if not self.initted: self.initted = True self.aLeadTau = _LEAD_ACCEL_TAU self.cnt = 1 self.vision_cnt = 0 self.vision = False self.aRel = 0. # nidec gives no information about this self.vLat = 0. self.kf = KF1D([[self.vLead], [0.0]], _VLEAD_A, _VLEAD_C, _VLEAD_K) else: # estimate acceleration # TODO: use Kalman filter a_rel_unfilt = (self.vRel - self.vRelPrev) / ts a_rel_unfilt = clip(a_rel_unfilt, -10., 10.) self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel # TODO: use Kalman filter # neglect steer override cases as dPath is too noisy v_lat_unfilt = 0. if steer_override else (self.dPath - self.dPathPrev) / ts self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat self.kf.update(self.vLead) self.cnt += 1 self.vLeadK = float(self.kf.x[SPEED][0]) self.aLeadK = float(self.kf.x[ACCEL][0]) if self.stationary: # stationary objects can become non stationary, but not the other way around self.stationary = v_ego_t_aligned > v_ego_stationary and abs( self.vLead) < v_stationary_thr self.oncoming = self.vLead < v_oncoming_thr self.vision_score = NO_FUSION_SCORE # Learn if constant acceleration if abs(self.aLeadK) < 0.5: self.aLeadTau = _LEAD_ACCEL_TAU else: self.aLeadTau *= 0.9
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
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 reset_a_lead(self, aLeadK, aLeadTau): self.kf = KF1D([[self.vLead], [aLeadK]], _VLEAD_A, _VLEAD_C, _VLEAD_K) self.aLeadK = aLeadK self.aLeadTau = aLeadTau
def reset_a_lead(self, aLeadK, aLeadTau): self.kf = KF1D([[self.vLead], [aLeadK]], self.K_A, self.K_C, self.K_K) self.aLeadK = aLeadK self.aLeadTau = aLeadTau