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