def __init__(self, CP): self.speed_control_enabled = 0 self.CL_MIN_V = 8.9 self.CL_MAX_A = 20. # labels for buttons self.btns_init = [["alca", "ALC", ["MadMax", "Normal", "Calm"]], [ ACCMode.BUTTON_NAME, ACCMode.BUTTON_ABREVIATION, ACCMode.labels() ], ["dsp", "DSP", ["OP", "MIN", "OFF", "GYRO"]], ["", "", [""]], ["msg", "MSG", [""]], ["sound", "SND", [""]]] ### START OF MAIN CONFIG OPTIONS ### ### Do NOT modify here, modify in /data/bb_openpilot.cfg and reboot self.forcePedalOverCC = True self.enableHSO = True self.enableALCA = True self.enableDasEmulation = True self.enableRadarEmulation = True self.enableSpeedVariableDesAngle = False self.enableRollAngleCorrection = False self.enableFeedForwardAngleCorrection = True self.enableDriverMonitor = True self.enableShowCar = True self.enableShowLogo = True self.hasNoctuaFan = False self.limitBatteryMinMax = False self.limitBattery_Min = 60 self.limitBattery_Max = 70 self.doAutoUpdate = True self.blockUploadWhileTethering = False self.tetherIP = "127.0.0." self.useTeslaGPS = False self.useTeslaMapData = False self.hasTeslaIcIntegration = False self.useTeslaRadar = False self.useWithoutHarness = False self.radarVIN = " " self.enableLdw = True self.radarOffset = 0. self.radarPosition = 0 self.radarEpasType = 0 self.fix1916 = False self.forceFingerprintTesla = False self.eonToFront = 0.9 #read config file read_config_file(self) ### END OF MAIN CONFIG OPTIONS ### self.apEnabled = True self.apFollowTimeInS = 2.5 #time in seconds to follow self.keepEonOff = False self.alcaEnabled = True self.mapAwareSpeed = False # Tesla Model self.teslaModelDetected = 1 self.teslaModel = read_db('/data/params', 'TeslaModel') if self.teslaModel is None: self.teslaModel = "S" self.teslaModelDetected = 0 # for map integration self.csaRoadCurvC3 = 0. self.csaRoadCurvC2 = 0. self.csaRoadCurvRange = 0. self.csaRoadCurvUsingTspline = 0 self.csaOfframpCurvC3 = 0. self.csaOfframpCurvC2 = 0. self.csaOfframpCurvRange = 0. self.csaOfframpCurvUsingTspline = 0 self.roadCurvHealth = 0 self.roadCurvRange = 0. self.roadCurvC0 = 0. self.roadCurvC1 = 0. self.roadCurvC2 = 0. self.roadCurvC3 = 0. self.speedLimitUnits = 0 self.speedLimit = 0 self.meanFleetSplineSpeedMPS = 0. self.meanFleetSplineAccelMPS2 = 0. self.medianFleetSpeedMPS = 0. self.topQrtlFleetSplineSpeedMPS = 0. self.splineLocConfidence = 0 self.baseMapSpeedLimitMPS = 0. self.bottomQrtlFleetSpeedMPS = 0. self.rampType = 0 self.mapBasedSuggestedSpeed = 0. self.splineBasedSuggestedSpeed = 0. self.maxdrivespeed = 0. self.gpsLongitude = 0. self.gpsLatitude = 0. self.gpsAccuracy = 0. self.gpsElevation = 0. self.gpsHeading = 0. self.gpsVehicleSpeed = 0. self.userSpeedLimitKph = 0. self.userSpeedLimitOffsetKph = 0. self.brake_only = CP.enableCruise self.last_cruise_stalk_pull_time = 0 self.CP = CP self.user_gas = 0. self.user_gas_pressed = False self.pedal_interceptor_state = 0 self.pedal_interceptor_value = 0. self.pedal_interceptor_value2 = 0. self.pedal_interceptor_missed_counter = 0 self.brake_switch_prev = 0 self.brake_switch_ts = 0 self.cruise_buttons = 0 self.blinker_on = 0 self.left_blinker_on = 0 self.right_blinker_on = 0 self.steer_warning = 0 self.stopped = 0 # variables used for the fake DAS creation self.DAS_info_frm = -1 self.DAS_info_msg = 0 self.DAS_status_frm = 0 self.DAS_status_idx = 0 self.DAS_status2_frm = 0 self.DAS_status2_idx = 0 self.DAS_bodyControls_frm = 0 self.DAS_bodyControls_idx = 0 self.DAS_lanes_frm = 0 self.DAS_lanes_idx = 0 self.DAS_objects_frm = 0 self.DAS_objects_idx = 0 self.DAS_pscControl_frm = 0 self.DAS_pscControl_idx = 0 self.DAS_warningMatrix0_idx = 0 self.DAS_warningMatrix1_idx = 0 self.DAS_warningMatrix3_idx = 0 self.DAS_telemetryPeriodic1_idx = 0 self.DAS_telemetryPeriodic2_idx = 0 self.DAS_telemetryEvent1_idx = 0 self.DAS_telemetryEvent2_idx = 0 self.DAS_control_idx = 0 #BB notification messages for DAS self.DAS_canErrors = 0 self.DAS_plannerErrors = 0 self.DAS_doorOpen = 0 self.DAS_notInDrive = 0 #BB variables for pedal CC self.pedal_speed_kph = 0. # Pedal mode is ready, i.e. hardware is present and normal cruise is off. self.pedal_interceptor_available = False self.prev_pedal_interceptor_available = False #BB UIEvents self.UE = UIEvents(self) #BB PCC self.regenLight = 0 self.torqueLevel = 0. #BB variable for custom buttons self.cstm_btns = UIButtons(self, "Tesla Model S", "tesla", self.enableShowLogo, self.enableShowCar) #BB custom message counter self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero #BB steering_wheel_stalk last position, used by ACC and ALCA self.steering_wheel_stalk = None #BB carConfig data used to change IC info self.real_carConfig = None self.real_dasHw = 0 #BB visiond last type self.last_visiond = self.cstm_btns.btns[3].btn_label2 # vEgo kalman filter dt = 0.01 # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # R = 1e3 self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], A=[[1.0, dt], [0.0, 1.0]], C=[1.0, 0.0], K=[[0.12287673], [0.29666309]]) self.v_ego = 0.0 self.imperial_speed_units = True # The current max allowed cruise speed. Actual cruise speed sent to the car # may be lower, depending on traffic. self.v_cruise_pcm = 0.0 # Actual cruise speed currently active on the car. self.v_cruise_actual = 0.0 #ALCA params self.ALCA_enabled = False self.ALCA_total_steps = 100 self.ALCA_direction = 0 self.ALCA_error = False self.angle_offset = 0. self.init_angle_offset = False
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # Scaled tire stiffness ts_factor = 8 ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "tesla" ret.carFingerprint = candidate teslaModel = read_db('/data/params','TeslaModel') if teslaModel is not None: teslaModel = teslaModel.decode() if teslaModel is None: teslaModel = "S" ret.safetyModel = car.CarParams.SafetyModel.tesla ret.safetyParam = 1 ret.carVin = "TESLAFAKEVIN12345" ret.enableCamera = True ret.enableGasInterceptor = False #keep this False for now print ("ECU Camera Simulated: ", ret.enableCamera) print ("ECU Gas Interceptor: ", ret.enableGasInterceptor) ret.enableCruise = not ret.enableGasInterceptor mass_models = 4722./2.205 + STD_CARGO_KG wheelbase_models = 2.959 # RC: I'm assuming center means center of mass, and I think Model S is pretty even between two axles centerToFront_models = wheelbase_models * 0.5 #BB was 0.48 centerToRear_models = wheelbase_models - centerToFront_models rotationalInertia_models = 2500 tireStiffnessFront_models = 85100 #BB was 85400 tireStiffnessRear_models = 90000 # will create Kp and Ki for 0, 20, 40, 60 mph ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 8.94, 17.88, 26.82 ], [0., 8.94, 17.88, 26.82]] if candidate == CAR.MODELS: stop_and_go = True ret.mass = mass_models ret.wheelbase = wheelbase_models ret.centerToFront = centerToFront_models ret.steerRatio = 11.5 # Kp and Ki for the lateral control for 0, 20, 40, 60 mph ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.20, 0.80, 0.60, 0.30], [0.16, 0.12, 0.08, 0.04]] ret.lateralTuning.pid.kf = 0.00006 # Initial test value TODO: investigate FF steer control for Model S? ret.steerActuatorDelay = 0.2 #ret.steerReactance = 1.0 #ret.steerInductance = 1.0 #ret.steerResistance = 1.0 # Kp and Ki for the longitudinal control if teslaModel == "S": ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4] ret.longitudinalTuning.kiBP = [0., 5., 35.] ret.longitudinalTuning.kiV = [0.01,0.01,0.01] elif teslaModel == "SP": ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.375, 0.325, 0.325] ret.longitudinalTuning.kiBP = [0., 5., 35.] ret.longitudinalTuning.kiV = [0.00915,0.00825,0.00725] elif teslaModel == "SD": ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.50, 0.45, 0.4] ret.longitudinalTuning.kiBP = [0., 5., 35.] ret.longitudinalTuning.kiV = [0.01,0.01,0.01] elif teslaModel == "SPD": ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.375, 0.325, 0.325] ret.longitudinalTuning.kiBP = [0., 5., 35.] ret.longitudinalTuning.kiV = [0.00915,0.00825,0.00725] else: #use S numbers if we can't match anything ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [0.375, 0.325, 0.3] ret.longitudinalTuning.kiBP = [0., 5., 35.] ret.longitudinalTuning.kiV = [0.08,0.08,0.08] else: raise ValueError("unsupported car %s" % candidate) ret.steerControlType = car.CarParams.SteerControlType.angle # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not # conflict with PCM acc ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 25.5 * CV.MPH_TO_MS centerToRear = ret.wheelbase - ret.centerToFront # TODO: get actual value, for now starting with reasonable value for Model S ret.rotationalInertia = rotationalInertia_models * \ ret.mass * ret.wheelbase**2 / (mass_models * wheelbase_models**2) # TODO: start from empirically derived lateral slip stiffness and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront = (tireStiffnessFront_models * ts_factor) * \ ret.mass / mass_models * \ (centerToRear / ret.wheelbase) / (centerToRear_models / wheelbase_models) ret.tireStiffnessRear = (tireStiffnessRear_models * ts_factor) * \ ret.mass / mass_models * \ (ret.centerToFront / ret.wheelbase) / (centerToFront_models / wheelbase_models) # no rear steering, at least on the listed cars above ret.steerRatioRear = 0. # no max steer limit VS speed ret.steerMaxBP = [0.,15.] # m/s ret.steerMaxV = [420.,420.] # max steer allowed ret.gasMaxBP = [0.] # m/s ret.gasMaxV = [0.3] #if ret.enableGasInterceptor else [0.] # max gas allowed ret.brakeMaxBP = [0., 20.] # m/s ret.brakeMaxV = [1., 1.] # max brake allowed - BB: since we are using regen, make this even ret.longitudinalTuning.deadzoneBP = [0., 9.] #BB: added from Toyota to start pedal work; need to tune ret.longitudinalTuning.deadzoneV = [0., 0.] #BB: added from Toyota to start pedal work; need to tune; changed to 0 for now ret.stoppingControl = True ret.openpilotLongitudinalControl = True ret.steerLimitAlert = False ret.startAccel = 0.5 ret.steerRateCost = 1.0 ret.radarOffCan = not CarSettings().get_value("useTeslaRadar") ret.radarTimeStep = 0.05 #20Hz return ret