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