def controlsd_thread(gctx=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
# Pub Sockets
sendcan = messaging.pub_sock(service_list['sendcan'].port)
controlsstate = messaging.pub_sock(service_list['controlsState'].port)
carstate = messaging.pub_sock(service_list['carState'].port)
carcontrol = messaging.pub_sock(service_list['carControl'].port)
carevents = messaging.pub_sock(service_list['carEvents'].port)
carparams = messaging.pub_sock(service_list['carParams'].port)
is_metric = params.get("IsMetric") == "1"
passive = params.get("Passive") != "0"
sm = messaging.SubMaster([
'thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan',
'pathPlan'
])
logcan = messaging.sub_sock(service_list['can'].port)
# wait for health and CAN packets
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
is_panda_black = hw_type == log.HealthData.HwType.blackPanda
wait_for_can(logcan)
CI, CP = get_car(logcan, sendcan, is_panda_black)
logcan.close()
# TODO: Use the logcan socket from above, but that will currenly break the tests
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock(service_list['can'].port,
timeout=can_timeout)
car_recognized = CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
controller_available = CP.enableCamera and CI.CC is not None and not passive
read_only = not car_recognized or not controller_available
if read_only:
CP.safetyModel = car.CarParams.SafetyModel.elm327 # diagnostic only
# Write CarParams for radard and boardd safety mode
params.put("CarParams", CP.to_bytes())
params.put("LongitudinalControl",
"1" if CP.openpilotLongitudinalControl else "0")
CC = car.CarControl.new_message()
AM = AlertManager()
startup_alert = get_startup_alert(car_recognized, controller_available)
AM.add(sm.frame, startup_alert, False)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
if CP.lateralTuning.which() == 'pid':
LaC = LatControlPID(CP)
elif CP.lateralTuning.which() == 'indi':
LaC = LatControlINDI(CP)
elif CP.lateralTuning.which() == 'lqr':
LaC = LatControlLQR(CP)
driver_status = DriverStatus()
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
v_cruise_kph_last = 0
overtemp = False
free_space = False
cal_status = Calibration.INVALID
cal_perc = 0
mismatch_counter = 0
low_battery = False
events_prev = []
sm['pathPlan'].sensorValid = True
sm['pathPlan'].posenetValid = True
# detect sound card presence
sounds_available = not os.path.isfile('/EON') or (
os.path.isdir('/proc/asound/card0')
and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
# controlsd is driven by can recv, expected at 100Hz
rk = Ratekeeper(100, print_delay_threshold=None)
prof = Profiler(False) # off by default
while True:
start_time = sec_since_boot()
prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\
data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
driver_status, state, mismatch_counter, params)
prof.checkpoint("Sample")
# Create alerts
if not sm.all_alive_and_valid():
events.append(
create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['pathPlan'].mpcSolutionValid:
events.append(
create_event('plannerError',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sm['pathPlan'].sensorValid:
events.append(
create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
if not sm['pathPlan'].paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
if not sm['pathPlan'].posenetValid:
events.append(
create_event('posenetInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['plan'].radarValid:
events.append(
create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if sm['plan'].radarCanError:
events.append(
create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not CS.canValid:
events.append(
create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sounds_available:
events.append(
create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and sm[
'plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
events.append(
create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not read_only:
# update control state
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc)
prof.checkpoint("State Control")
# Publish data
CC, events_prev = data_send(sm, CS, CI, CP, VM, state, events,
actuators, v_cruise_kph, rk, carstate,
carcontrol, carevents, carparams,
controlsstate, sendcan, AM, driver_status,
LaC, LoC, read_only, start_time, v_acc,
a_acc, lac_log, events_prev)
prof.checkpoint("Sent")
rk.monitor_time()
prof.display()
def get_can_parser(CP):
signals = [
# sig_name, sig_address, default
("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0),
("GEAR", "GEAR_PACKET", 0),
("BRAKE_PRESSED", "BRAKE_MODULE", 0),
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
("DOOR_OPEN_FL", "SEATS_DOORS", 1),
("DOOR_OPEN_FR", "SEATS_DOORS", 1),
("DOOR_OPEN_RL", "SEATS_DOORS", 1),
("DOOR_OPEN_RR", "SEATS_DOORS", 1),
("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 1),
("TC_DISABLED", "ESP_CONTROL", 1),
("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0),
("STEER_RATE", "STEER_ANGLE_SENSOR", 0),
("CRUISE_ACTIVE", "PCM_CRUISE", 0),
("CRUISE_STATE", "PCM_CRUISE", 0),
("GAS_RELEASED", "PCM_CRUISE", 1),
("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0),
("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0),
("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers
("LKA_STATE", "EPS_STATUS", 0),
("AUTO_HIGH_BEAM", "LIGHT_STALK", 0),
#dp
("SPORT_ON", "GEAR_PACKET", 0),
("ECON_ON", "GEAR_PACKET", 1),
("DISTANCE_LINES", "PCM_CRUISE_SM", 0),
("RPM", "ENGINE_RPM", 0),
("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0),
]
checks = [
("GEAR_PACKET", 1),
("LIGHT_STALK", 1),
("STEERING_LEVERS", 0.15),
("SEATS_DOORS", 3),
("ESP_CONTROL", 3),
("EPS_STATUS", 25),
("BRAKE_MODULE", 40),
("WHEEL_SPEEDS", 80),
("STEER_ANGLE_SENSOR", 80),
("PCM_CRUISE", 33),
("STEER_TORQUE_SENSOR", 50),
#dp
("ENGINE_RPM", 100),
]
#dp acceleration
if CP.carFingerprint == CAR.RAV4_TSS2:
signals.append(("SPORT_ON_2", "GEAR_PACKET", 0))
if CP.carFingerprint in [
CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.CHRH, CAR.PRIUS_TSS2,
CAR.HIGHLANDERH_TSS2
]:
signals.append(("SPORT_ON", "GEAR_PACKET2", 0))
signals.append(("ECON_ON", "GEAR_PACKET2", 0))
if CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_ISH]:
signals.append(("MAIN_ON", "DSU_CRUISE", 0))
signals.append(("SET_SPEED", "DSU_CRUISE", 0))
checks.append(("DSU_CRUISE", 5))
else:
signals.append(("MAIN_ON", "PCM_CRUISE_2", 0))
signals.append(("SET_SPEED", "PCM_CRUISE_2", 0))
signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0))
checks.append(("PCM_CRUISE_2", 33))
if CP.carFingerprint == CAR.LEXUS_ISH:
signals.append(("GAS_PEDAL", "GAS_PEDAL_ALT", 0))
checks.append(("GAS_PEDAL_ALT", 33))
else:
signals.append(("GAS_PEDAL", "GAS_PEDAL", 0))
checks.append(("GAS_PEDAL", 33))
# add gas interceptor reading if we are using it
if CP.enableGasInterceptor:
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0))
checks.append(("GAS_SENSOR", 50))
if CP.enableBsm:
signals += [
("L_ADJACENT", "BSM", 0),
("L_APPROACHING", "BSM", 0),
("R_ADJACENT", "BSM", 0),
("R_APPROACHING", "BSM", 0),
]
checks += [("BSM", 1)]
if Params().get('dp_toyota_zss') == b'1':
signals += [("ZORRO_STEER", "SECONDARY_STEER_ANGLE", 0)]
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
def __init__(self, dbc_name, CP, VM):
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.accel_steady = 0
self.apply_steer_last = 0
self.steer_rate_limited = False
self.lkas11_cnt = 0
self.scc12_cnt = 0
self.resume_cnt = 0
self.last_lead_distance = 0
self.resume_wait_timer = 0
self.last_resume_frame = 0
self.lanechange_manual_timer = 0
self.emergency_manual_timer = 0
self.driver_steering_torque_above_timer = 0
self.mode_change_timer = 0
self.params = Params()
self.mode_change_switch = int(self.params.get('CruiseStatemodeSelInit'))
self.opkr_variablecruise = int(self.params.get('OpkrVariableCruise'))
self.opkr_autoresume = int(self.params.get('OpkrAutoResume'))
self.opkr_autoresumeoption = int(self.params.get('OpkrAutoResumeOption'))
self.opkr_maxanglelimit = int(self.params.get('OpkrMaxAngleLimit'))
self.steer_mode = ""
self.mdps_status = ""
self.lkas_switch = ""
self.timer1 = tm.CTime1000("time")
self.SC = Spdctrl()
self.model_speed = 0
self.model_sum = 0
self.dRele = 0
self.yRele = 0
self.vRele = 0
self.cruise_gap = 0.0
self.cruise_gap_prev = 0
self.cruise_gap_set_init = 0
self.cruise_gap_switch_timer = 0
self.lkas_button_on = True
self.longcontrol = CP.openpilotLongitudinalControl
self.scc_live = not CP.radarOffCan
if CP.lateralTuning.which() == 'pid':
self.str_log2 = 'TUNE={:0.2f}/{:0.3f}/{:0.5f}'.format(CP.lateralTuning.pid.kpV[1], CP.lateralTuning.pid.kiV[1], CP.lateralTuning.pid.kf)
elif CP.lateralTuning.which() == 'indi':
self.str_log2 = 'TUNE={:03.1f}/{:03.1f}/{:03.1f}/{:03.1f}'.format(CP.lateralTuning.indi.innerLoopGain, CP.lateralTuning.indi.outerLoopGain, CP.lateralTuning.indi.timeConstant, CP.lateralTuning.indi.actuatorEffectiveness)
elif CP.lateralTuning.which() == 'lqr':
self.str_log2 = 'TUNE={:04.0f}/{:05.3f}/{:06.4f}'.format(CP.lateralTuning.lqr.scale, CP.lateralTuning.lqr.ki, CP.lateralTuning.lqr.dcGain)
if CP.spasEnabled:
self.en_cnt = 0
self.apply_steer_ang = 0.0
self.en_spas = 3
self.mdps11_stat_last = 0
self.spas_always = False
def fingerprint(logcan, sendcan, has_relay):
fixed_fingerprint = os.environ.get('FINGERPRINT', "")
skip_fw_query = os.environ.get('SKIP_FW_QUERY', False)
if has_relay and not fixed_fingerprint and not skip_fw_query:
# Vin query only reliably works thorugh OBDII
bus = 1
cached_params = Params().get("CarParamsCache")
if cached_params is not None:
cached_params = car.CarParams.from_bytes(cached_params)
if cached_params.carName == "mock":
cached_params = None
if cached_params is not None and len(
cached_params.carFw
) > 0 and cached_params.carVin is not VIN_UNKNOWN:
cloudlog.warning("Using cached CarParams")
vin = cached_params.carVin
car_fw = list(cached_params.carFw)
else:
cloudlog.warning("Getting VIN & FW versions")
_, vin = get_vin(logcan, sendcan, bus)
car_fw = get_fw_versions(logcan, sendcan, bus)
fw_candidates = match_fw_to_car(car_fw)
else:
vin = VIN_UNKNOWN
fw_candidates, car_fw = set(), []
cloudlog.warning("VIN %s", vin)
Params().put("CarVin", vin)
finger = gen_empty_fingerprint()
candidate_cars = {i: all_known_cars()
for i in [0]} # attempt fingerprint on bus 0 only
frame = 0
frame_fingerprint = 10 # 0.1s
car_fingerprint = None
done = False
while not done:
a = get_one_can(logcan)
for can in a.can:
# need to independently try to fingerprint both bus 0 and 1 to work
# for the combo black_panda and honda_bosch. Ignore extended messages
# and VIN query response.
# Include bus 2 for toyotas to disambiguate cars using camera messages
# (ideally should be done for all cars but we can't for Honda Bosch)
if can.src in range(0, 4):
finger[can.src][can.address] = len(can.dat)
for b in candidate_cars:
if (can.src == b or (only_toyota_left(candidate_cars[b]) and can.src == 2)) and \
can.address < 0x800 and can.address not in [0x7df, 0x7e0, 0x7e8]:
candidate_cars[b] = eliminate_incompatible_cars(
can, candidate_cars[b])
# if we only have one car choice and the time since we got our first
# message has elapsed, exit
for b in candidate_cars:
# Toyota needs higher time to fingerprint, since DSU does not broadcast immediately
if only_toyota_left(candidate_cars[b]):
frame_fingerprint = 100 # 1s
if len(candidate_cars[b]) == 1:
if frame > frame_fingerprint:
# fingerprint done
car_fingerprint = candidate_cars[b][0]
# bail if no cars left or we've been waiting for more than 2s
failed = all(len(cc) == 0
for cc in candidate_cars.values()) or frame > 200
succeeded = car_fingerprint is not None
done = failed or succeeded
frame += 1
source = car.CarParams.FingerprintSource.can
# If FW query returns exactly 1 candidate, use it
if len(fw_candidates) == 1:
car_fingerprint = list(fw_candidates)[0]
source = car.CarParams.FingerprintSource.fw
if fixed_fingerprint:
car_fingerprint = fixed_fingerprint
source = car.CarParams.FingerprintSource.fixed
cloudlog.warning("fingerprinted %s", car_fingerprint)
return car_fingerprint, finger, vin, car_fw, source
if car_model not in tested_cars:
print("***** WARNING: %s not tested *****" % car_model)
print("Preparing processes")
for p in tested_procs:
manager.prepare_managed_process(p)
results = {}
for route, checks in routes.items():
if route not in non_public_routes:
get_route_logs(route)
elif "UNLOGGER_PATH" not in os.environ:
continue
shutil.rmtree('/data/params')
params = Params()
params.manager_start()
params.put("OpenpilotEnabledToggle", "1")
params.put("CommunityFeaturesToggle", "1")
params.put("Passive", "1" if route in passive_routes else "0")
print("testing ", route, " ", checks['carFingerprint'])
print("Starting processes")
for p in tested_procs:
manager.start_managed_process(p)
# Start unlogger
print("Start unlogger")
if route in non_public_routes:
unlogger_cmd = [os.path.join(BASEDIR, os.environ['UNLOGGER_PATH']), route]
else:
class CarController():
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.steer_rate_limited = False
self.resume_cnt = 0
self.last_resume_frame = 0
self.last_lead_distance = 0
self.lanechange_manual_timer = 0
self.emergency_manual_timer = 0
self.driver_steering_torque_above_timer = 0
self.mode_change_timer = 0
self.steer_mode = ""
self.mdps_status = ""
self.lkas_switch = ""
self.lkas11_cnt = 0
self.nBlinker = 0
self.dRel = 0
self.yRel = 0
self.vRel = 0
self.timer1 = tm.CTime1000("time")
self.model_speed = 0
self.model_sum = 0
# hud
self.hud_timer_left = 0
self.hud_timer_right = 0
self.command_cnt = 0
self.command_load = 0
self.params = Params()
# param
self.param_preOpkrAccelProfile = -1
self.param_OpkrAccelProfile = 0
self.param_OpkrAutoResume = 0
self.SC = None
self.traceCC = trace1.Loger("CarController")
self.res_cnt = 7
self.res_delay = 0
self.params = Params()
self.mode_change_switch = int(
self.params.get('CruiseStatemodeSelInit'))
def process_hud_alert(self, enabled, CC):
visual_alert = CC.hudControl.visualAlert
left_lane = CC.hudControl.leftLaneVisible
right_lane = CC.hudControl.rightLaneVisible
sys_warning = (visual_alert == VisualAlert.steerRequired)
if left_lane:
self.hud_timer_left = 100
if right_lane:
self.hud_timer_right = 100
if self.hud_timer_left:
self.hud_timer_left -= 1
if self.hud_timer_right:
self.hud_timer_right -= 1
# initialize to no line visible
sys_state = 1
if self.hud_timer_left and self.hud_timer_right or sys_warning: # HUD alert only display when LKAS status is active
if enabled or sys_warning:
sys_state = 3
else:
sys_state = 4
elif self.hud_timer_left:
sys_state = 5
elif self.hud_timer_right:
sys_state = 6
return sys_warning, sys_state
def param_load(self):
self.command_cnt += 1
if self.command_cnt > 100:
self.command_cnt = 0
if self.command_cnt % 10:
return
self.command_load += 1
if self.command_load == 1:
self.param_OpkrAccelProfile = int(
self.params.get('OpkrAccelProfile'))
elif self.command_load == 2:
self.param_OpkrAutoResume = int(self.params.get('OpkrAutoResume'))
else:
self.command_load = 0
# speed controller
if self.param_preOpkrAccelProfile != self.param_OpkrAccelProfile:
self.param_preOpkrAccelProfile = self.param_OpkrAccelProfile
if self.param_OpkrAccelProfile == 1:
self.SC = SpdctrlSlow()
elif self.param_OpkrAccelProfile == 2:
self.SC = SpdctrlNormal()
else:
self.SC = SpdctrlFast()
def update(self, CC, CS, frame, sm, CP):
if self.CP != CP:
self.CP = CP
self.param_load()
enabled = CC.enabled
actuators = CC.actuators
pcm_cancel_cmd = CC.cruiseControl.cancel
self.dRel, self.yRel, self.vRel = SpdController.get_lead(sm)
if self.SC is not None:
self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)
else:
self.model_speed = self.model_sum = 0
# Steering Torque
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer,
self.apply_steer_last,
CS.out.steeringTorque,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(CS.out.steeringAngle) < 180. #90
if ((CS.out.leftBlinker and not CS.out.rightBlinker) or
(CS.out.rightBlinker
and not CS.out.leftBlinker)) and CS.out.vEgo < 60 * CV.KPH_TO_MS:
self.lanechange_manual_timer = 10
if CS.out.leftBlinker and CS.out.rightBlinker:
self.emergency_manual_timer = 10
if abs(CS.out.steeringTorque) > 360: #180
self.driver_steering_torque_above_timer = 100
if self.lanechange_manual_timer or self.driver_steering_torque_above_timer:
lkas_active = 0
if self.lanechange_manual_timer > 0:
self.lanechange_manual_timer -= 1
if self.emergency_manual_timer > 0:
self.emergency_manual_timer -= 1
if self.driver_steering_torque_above_timer > 0:
self.driver_steering_torque_above_timer -= 1
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
self.apply_steer_last = apply_steer
sys_warning, sys_state = self.process_hud_alert(lkas_active, CC)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 55
if clu11_speed > enabled_speed:
enabled_speed = clu11_speed
can_sends = []
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"] + 1
self.lkas11_cnt %= 0x10
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt, self.car_fingerprint,
apply_steer, steer_req, CS.lkas11, sys_warning,
sys_state, CC, enabled, 0))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps is on bus 1
can_sends.append(
create_lkas11(self.packer, self.lkas11_cnt,
self.car_fingerprint, apply_steer, steer_req,
CS.lkas11, sys_warning, sys_state, CC, enabled,
1))
if CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
#if frame % 2 and CS.mdps_bus == 1: # send clu11 to mdps if it is not on bus 0
can_sends.append(
create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11,
Buttons.NONE, enabled_speed))
#if CS.mdps_bus:
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
str_log1 = '곡률={:04.1f}/{:=+06.3f} 토크={:=+04.0f}/{:=+04.0f}'.format(
self.model_speed, self.model_sum, new_steer, CS.out.steeringTorque)
str_log2 = '프레임율={:03.0f}'.format(self.timer1.sampleTime())
trace1.printf('{} {}'.format(str_log1, str_log2))
if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 4:
self.mode_change_timer = 50
self.mode_change_switch = 0
elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0:
self.mode_change_timer = 50
self.mode_change_switch = 1
elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1:
self.mode_change_timer = 50
self.mode_change_switch = 2
elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2:
self.mode_change_timer = 50
self.mode_change_switch = 3
elif CS.out.cruiseState.modeSel == 4 and self.mode_change_switch == 3:
self.mode_change_timer = 50
self.mode_change_switch = 4
if self.mode_change_timer > 0:
self.mode_change_timer -= 1
run_speed_ctrl = self.param_OpkrAccelProfile and CS.acc_active and self.SC != None
if not run_speed_ctrl:
if CS.out.cruiseState.modeSel == 0:
self.steer_mode = "오파모드"
elif CS.out.cruiseState.modeSel == 1:
self.steer_mode = "차간+커브"
elif CS.out.cruiseState.modeSel == 2:
self.steer_mode = "차간ONLY"
elif CS.out.cruiseState.modeSel == 3:
self.steer_mode = "자동RES"
elif CS.out.cruiseState.modeSel == 4:
self.steer_mode = "순정모드"
if CS.out.steerWarning == 0:
self.mdps_status = "정상"
elif CS.out.steerWarning == 1:
self.mdps_status = "오류"
if CS.lkas_button_on == 0:
self.lkas_switch = "OFF"
elif CS.lkas_button_on == 1:
self.lkas_switch = "ON"
else:
self.lkas_switch = "-"
str_log2 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s}'.format(
self.steer_mode, self.mdps_status, self.lkas_switch)
trace1.printf2('{}'.format(str_log2))
#print( 'st={} cmd={} long={} steer={} req={}'.format(CS.out.cruiseState.standstill, pcm_cancel_cmd, self.CP.openpilotLongitudinalControl, apply_steer, steer_req ) )
if pcm_cancel_cmd and self.CP.openpilotLongitudinalControl:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.CANCEL, clu11_speed))
elif CS.out.cruiseState.standstill and not self.car_fingerprint == CAR.NIRO_EV:
# run only first time when the car stopped
if self.last_lead_distance == 0 or not self.param_OpkrAutoResume:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance != self.last_lead_distance and (
frame - self.last_resume_frame) > 5:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
self.resume_cnt = 0
elif CS.out.cruiseState.standstill and self.car_fingerprint == CAR.NIRO_EV:
if CS.lead_distance > 3.7 and (
frame - self.last_resume_frame
) * DT_CTRL > 0.2 and self.param_OpkrAutoResume:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.last_resume_frame = frame
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
elif run_speed_ctrl and self.SC != None:
is_sc_run = self.SC.update(CS, sm, self)
if is_sc_run:
can_sends.append(
create_clu11(self.packer, self.resume_cnt, CS.scc_bus,
CS.clu11, self.SC.btn_type,
self.SC.sc_clu_speed))
self.resume_cnt += 1
else:
self.resume_cnt = 0
if CS.out.cruiseState.modeSel == 3:
if CS.out.brakeLights and CS.VSetDis > 30:
self.res_cnt = 0
self.res_delay = 50
elif self.res_delay:
self.res_delay -= 1
elif not self.res_delay and self.res_cnt < 0 and CS.VSetDis > 30 and CS.out.vEgo > 30 * CV.KPH_TO_MS:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.CANCEL, clu11_speed))
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.RES_ACCEL, clu11_speed))
self.res_cnt += 1
else:
self.res_cnt = 7
self.res_delay = 0
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in FEATURES["send_lfa_mfa"]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
self.lkas11_cnt += 1
return can_sends
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
ret.carName = "hyundai"
ret.safetyConfigs = [
get_safety_config(car.CarParams.SafetyModel.hyundai, 0)
]
ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1]
# WARNING: disabling radar also disables AEB (and we show the same warning on the instrument cluster as if you manually disabled AEB)
ret.openpilotLongitudinalControl = Params().get_bool(
"DisableRadar") and candidate in [
CAR.SONATA, CAR.SONATA_HYBRID, CAR.PALISADE, CAR.SANTA_FE
]
ret.pcmCruise = not ret.openpilotLongitudinalControl
ret.steerActuatorDelay = 0.1 # Default delay
ret.steerRateCost = 0.5
ret.steerLimitTimer = 0.4
tire_stiffness_factor = 1.
ret.stoppingControl = True
ret.vEgoStopping = 1.0
ret.longitudinalTuning.kpV = [0.1]
ret.longitudinalTuning.kiV = [0.0]
ret.stopAccel = 0.0
ret.startAccel = 0.0
ret.longitudinalActuatorDelayUpperBound = 1.0 # s
if candidate in [CAR.SANTA_FE, CAR.SANTA_FE_2022]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.766
# Values from optimizer
ret.steerRatio = 16.55 # 13.8 is spec end-to-end
tire_stiffness_factor = 0.82
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[
9., 22.
], [9., 22.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[
0.2, 0.35
], [0.05, 0.09]]
elif candidate in [CAR.SONATA, CAR.SONATA_HYBRID]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1513. + STD_CARGO_KG
ret.wheelbase = 2.84
ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable
tire_stiffness_factor = 0.65
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.SONATA_LF:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 4497. * CV.LB_TO_KG
ret.wheelbase = 2.804
ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.PALISADE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1999. + STD_CARGO_KG
ret.wheelbase = 2.90
ret.steerRatio = 15.6 * 1.15
tire_stiffness_factor = 0.63
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3],
[0.05]]
elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1275. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 15.4 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535
tire_stiffness_factor = 0.385 # stiffnessFactor settled on 1.0081302973865127
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.ELANTRA_2021:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = (2800. * CV.LB_TO_KG) + STD_CARGO_KG
ret.wheelbase = 2.72
ret.steerRatio = 12.9
tire_stiffness_factor = 0.65
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.ELANTRA_HEV_2021:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = (3017. * CV.LB_TO_KG) + STD_CARGO_KG
ret.wheelbase = 2.72
ret.steerRatio = 12.9
tire_stiffness_factor = 0.65
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.HYUNDAI_GENESIS:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 2060. + STD_CARGO_KG
ret.wheelbase = 3.01
ret.steerRatio = 16.5
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGainBP = [0.]
ret.lateralTuning.indi.innerLoopGainV = [3.5]
ret.lateralTuning.indi.outerLoopGainBP = [0.]
ret.lateralTuning.indi.outerLoopGainV = [2.0]
ret.lateralTuning.indi.timeConstantBP = [0.]
ret.lateralTuning.indi.timeConstantV = [1.4]
ret.lateralTuning.indi.actuatorEffectivenessBP = [0.]
ret.lateralTuning.indi.actuatorEffectivenessV = [2.3]
ret.minSteerSpeed = 60 * CV.KPH_TO_MS
elif candidate in [CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = {
CAR.KONA_EV: 1685.,
CAR.KONA_HEV: 1425.
}.get(candidate, 1275.) + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 * 1.15 # Spec
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate in [
CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV
]:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1490. + STD_CARGO_KG # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
ret.wheelbase = 2.7
ret.steerRatio = 13.73 # Spec
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
if candidate not in [CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV]:
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.VELOSTER:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3558. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75 * 1.15
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
# Kia
elif candidate == CAR.KIA_SORENTO:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1985. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate in [
CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021
]:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1737. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.9 if CAR.KIA_NIRO_HEV_2021 else 13.73 # Spec
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
if candidate == CAR.KIA_NIRO_HEV:
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.KIA_SELTOS:
ret.mass = 1337. + STD_CARGO_KG
ret.wheelbase = 2.63
ret.steerRatio = 14.56
tire_stiffness_factor = 1
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGainBP = [0.]
ret.lateralTuning.indi.innerLoopGainV = [4.]
ret.lateralTuning.indi.outerLoopGainBP = [0.]
ret.lateralTuning.indi.outerLoopGainV = [3.]
ret.lateralTuning.indi.timeConstantBP = [0.]
ret.lateralTuning.indi.timeConstantV = [1.4]
ret.lateralTuning.indi.actuatorEffectivenessBP = [0.]
ret.lateralTuning.indi.actuatorEffectivenessV = [1.8]
elif candidate in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3558. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.KIA_STINGER:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1825. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.KIA_FORTE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3558. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.KIA_CEED:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1450. + STD_CARGO_KG
ret.wheelbase = 2.65
ret.steerRatio = 13.75
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kf = 0.00005
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
elif candidate == CAR.KIA_K5_2021:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3228. * CV.LB_TO_KG
ret.wheelbase = 2.85
ret.steerRatio = 13.27 # 2021 Kia K5 Steering Ratio (all trims)
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25],
[0.05]]
# Genesis
elif candidate == CAR.GENESIS_G70:
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGainBP = [0.]
ret.lateralTuning.indi.innerLoopGainV = [2.5]
ret.lateralTuning.indi.outerLoopGainBP = [0.]
ret.lateralTuning.indi.outerLoopGainV = [3.5]
ret.lateralTuning.indi.timeConstantBP = [0.]
ret.lateralTuning.indi.timeConstantV = [1.4]
ret.lateralTuning.indi.actuatorEffectivenessBP = [0.]
ret.lateralTuning.indi.actuatorEffectivenessV = [1.8]
ret.steerActuatorDelay = 0.1
ret.mass = 1640.0 + STD_CARGO_KG
ret.wheelbase = 2.84
ret.steerRatio = 13.56
elif candidate == CAR.GENESIS_G80:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 2060. + STD_CARGO_KG
ret.wheelbase = 3.01
ret.steerRatio = 16.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16],
[0.01]]
elif candidate == CAR.GENESIS_G90:
ret.mass = 2200
ret.wheelbase = 3.15
ret.steerRatio = 12.069
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.],
[0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16],
[0.01]]
# these cars require a special panda safety mode due to missing counters and checksums in the messages
if candidate in [
CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD,
CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO,
CAR.SONATA_LF, CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER,
CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80,
CAR.KIA_CEED, CAR.ELANTRA
]:
ret.safetyConfigs = [
get_safety_config(car.CarParams.SafetyModel.hyundaiLegacy)
]
# set appropriate safety param for gas signal
if candidate in HYBRID_CAR:
ret.safetyConfigs[0].safetyParam = 2
elif candidate in EV_CAR:
ret.safetyConfigs[0].safetyParam = 1
ret.centerToFront = ret.wheelbase * 0.4
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(
ret.mass,
ret.wheelbase,
ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
ret.enableBsm = 0x58b in fingerprint[0]
if ret.openpilotLongitudinalControl:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LONG
return ret
def main():
os.environ['PARAMS_PATH'] = PARAMS
if ANDROID:
# the flippening!
os.system(
'LD_LIBRARY_PATH="" content insert --uri content://settings/system --bind name:s:user_rotation --bind value:i:1'
)
# disable bluetooth
os.system('service call bluetooth_manager 8')
params = Params()
params.manager_start()
default_params = [
("CommunityFeaturesToggle", "0"),
("CompletedTrainingVersion", "0"),
("IsRHD", "0"),
("IsMetric", "0"),
("RecordFront", "0"),
("HasAcceptedTerms", "0"),
("HasCompletedSetup", "0"),
("IsUploadRawEnabled", "1"),
("IsLdwEnabled", "1"),
("LastUpdateTime",
datetime.datetime.utcnow().isoformat().encode('utf8')),
("OpenpilotEnabledToggle", "1"),
("LaneChangeEnabled", "1"),
("IsDriverViewEnabled", "0"),
]
# set unset params
for k, v in default_params:
if params.get(k) is None:
params.put(k, v)
# is this chffrplus?
if os.getenv("PASSIVE") is not None:
params.put("Passive", str(int(os.getenv("PASSIVE"))))
if params.get("Passive") is None:
raise Exception("Passive must be set to continue")
if ANDROID:
update_apks()
manager_init()
manager_prepare(spinner)
spinner.close()
if os.getenv("PREPAREONLY") is not None:
return
# SystemExit on sigterm
signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1))
try:
manager_thread()
except Exception:
traceback.print_exc()
crash.capture_exception()
finally:
cleanup_all_processes(None, None)
if params.get("DoUninstall", encoding='utf8') == "1":
uninstall()
def getSshAuthorizedKeys():
return Params().get("GithubSshKeys", encoding='utf8') or ''
def main(sm=None, pm=None):
gc.disable()
set_realtime_priority(5)
if sm is None:
sm = messaging.SubMaster(['liveLocationKalman', 'carState'],
poll=['liveLocationKalman'])
if pm is None:
pm = messaging.PubMaster(['liveParameters'])
params_reader = Params()
# wait for stats about the car to come in from controls
cloudlog.info("paramsd is waiting for CarParams")
CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
cloudlog.info("paramsd got CarParams")
min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
params = params_reader.get("LiveParameters") if not params_reader.get_bool(
'dp_reset_live_param_on_start') else None
# Check if car model matches
if params is not None:
params = json.loads(params)
if params.get('carFingerprint', None) != CP.carFingerprint:
cloudlog.info("Parameter learner found parameters for wrong car.")
params = None
# Check if starting values are sane
if params is not None:
try:
angle_offset_sane = abs(params.get('angleOffsetAverageDeg')) < 10.0
steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr
params_sane = angle_offset_sane and steer_ratio_sane
if not params_sane:
cloudlog.info(f"Invalid starting values found {params}")
params = None
except Exception as e:
cloudlog.info(f"Error reading params {params}: {str(e)}")
params = None
# TODO: cache the params with the capnp struct
if params is None:
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': CP.steerRatio,
'stiffnessFactor': 1.0,
'angleOffsetAverageDeg': 0.0,
}
cloudlog.info("Parameter learner resetting to default values")
# When driving in wet conditions the stiffness can go down, and then be too low on the next drive
# Without a way to detect this we have to reset the stiffness every drive
params['stiffnessFactor'] = 1.0
learner = ParamsLearner(CP, params['steerRatio'],
params['stiffnessFactor'],
math.radians(params['angleOffsetAverageDeg']))
angle_offset_average = params['angleOffsetAverageDeg']
angle_offset = angle_offset_average
while True:
sm.update()
for which in sorted(sm.updated.keys(),
key=lambda x: sm.logMonoTime[x]):
if sm.updated[which]:
t = sm.logMonoTime[which] * 1e-9
learner.handle_log(t, which, sm[which])
if sm.updated['liveLocationKalman']:
x = learner.kf.x
P = np.sqrt(learner.kf.P.diagonal())
if not all(map(math.isfinite, x)):
cloudlog.error(
"NaN in liveParameters estimate. Resetting to default values"
)
learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
x = learner.kf.x
angle_offset_average = clip(
math.degrees(x[States.ANGLE_OFFSET]),
angle_offset_average - MAX_ANGLE_OFFSET_DELTA,
angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
angle_offset = clip(
math.degrees(x[States.ANGLE_OFFSET] +
x[States.ANGLE_OFFSET_FAST]),
angle_offset - MAX_ANGLE_OFFSET_DELTA,
angle_offset + MAX_ANGLE_OFFSET_DELTA)
msg = messaging.new_message('liveParameters')
msg.logMonoTime = sm.logMonoTime['carState']
msg.liveParameters.posenetValid = True
msg.liveParameters.sensorValid = True
msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
msg.liveParameters.angleOffsetAverageDeg = angle_offset_average
msg.liveParameters.angleOffsetDeg = angle_offset
msg.liveParameters.valid = all((
abs(msg.liveParameters.angleOffsetAverageDeg) < 10.0,
abs(msg.liveParameters.angleOffsetDeg) < 10.0,
0.2 <= msg.liveParameters.stiffnessFactor <= 5.0,
min_sr <= msg.liveParameters.steerRatio <= max_sr,
))
msg.liveParameters.steerRatioStd = float(P[States.STEER_RATIO])
msg.liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS])
msg.liveParameters.angleOffsetAverageStd = float(
P[States.ANGLE_OFFSET])
msg.liveParameters.angleOffsetFastStd = float(
P[States.ANGLE_OFFSET_FAST])
if sm.frame % 1200 == 0: # once a minute
params = {
'carFingerprint':
CP.carFingerprint,
'steerRatio':
msg.liveParameters.steerRatio,
'stiffnessFactor':
msg.liveParameters.stiffnessFactor,
'angleOffsetAverageDeg':
msg.liveParameters.angleOffsetAverageDeg,
}
put_nonblocking("LiveParameters", json.dumps(params))
pm.send('liveParameters', msg)
def manager_thread():
# now loop
thermal_sock = messaging.sub_sock('thermal')
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
# save boot log
subprocess.call(["./loggerd", "--bootlog"],
cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
params = Params()
# start daemon processes
for p in daemon_processes:
start_daemon_process(p)
# start persistent processes
for p in persistent_processes:
start_managed_process(p)
# start offroad
if ANDROID:
pm_apply_packages('enable')
start_offroad()
if os.getenv("NOBOARD") is None:
start_managed_process("pandad")
if os.getenv("BLOCK") is not None:
for k in os.getenv("BLOCK").split(","):
del managed_processes[k]
started_prev = False
logger_dead = False
while 1:
msg = messaging.recv_sock(thermal_sock, wait=True)
# heavyweight batch processes are gated on favorable thermal conditions
if msg.thermal.thermalStatus >= ThermalStatus.yellow:
for p in green_temp_processes:
if p in persistent_processes:
kill_managed_process(p)
else:
for p in green_temp_processes:
if p in persistent_processes:
start_managed_process(p)
if msg.thermal.freeSpace < 0.05:
logger_dead = True
if msg.thermal.started:
for p in car_started_processes:
if p == "loggerd" and logger_dead:
kill_managed_process(p)
else:
start_managed_process(p)
else:
logger_dead = False
driver_view = params.get("IsDriverViewEnabled") == b"1"
# TODO: refactor how manager manages processes
for p in reversed(car_started_processes):
if p not in driver_view_processes or not driver_view:
kill_managed_process(p)
for p in driver_view_processes:
if driver_view:
start_managed_process(p)
else:
kill_managed_process(p)
# trigger an update after going offroad
if started_prev:
send_managed_process_signal("updated", signal.SIGHUP)
started_prev = msg.thermal.started
# check the status of all processes, did any of them die?
running_list = [
"%s%s\u001b[0m" %
("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p)
for p in running
]
cloudlog.debug(' '.join(running_list))
# Exit main loop when uninstall is needed
if params.get("DoUninstall", encoding='utf8') == "1":
break
def dmonitoringd_thread(sm=None, pm=None):
if pm is None:
pm = messaging.PubMaster(['dMonitoringState'])
if sm is None:
sm = messaging.SubMaster([
'driverState', 'liveCalibration', 'carState', 'controlsState',
'model'
],
poll=['driverState'])
driver_status = DriverStatus()
driver_status.is_rhd_region = Params().get("IsRHD") == b"1"
offroad = Params().get("IsOffroad") == b"1"
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['liveCalibration'].rpyCalib = [0, 0, 0]
sm['carState'].vEgo = 0.
sm['carState'].cruiseState.speed = 0.
sm['carState'].buttonEvents = []
sm['carState'].steeringPressed = False
sm['carState'].gasPressed = False
sm['carState'].standstill = True
v_cruise_last = 0
driver_engaged = False
# 10Hz <- dmonitoringmodeld
while True:
sm.update()
if not sm.updated['driverState']:
continue
butpressed = False
# Get interaction
if sm.updated['carState']:
v_cruise = sm['carState'].cruiseState.speed
for b in sm['carState'].buttonEvents:
butpressed = b.pressed
driver_engaged = butpressed or \
v_cruise != v_cruise_last or \
sm['carState'].steeringPressed or \
sm['carState'].gasPressed
if driver_engaged:
driver_status.update(Events(), True,
sm['controlsState'].enabled,
sm['carState'].standstill)
v_cruise_last = v_cruise
if sm.updated['model']:
driver_status.set_policy(sm['model'])
# Get data from dmonitoringmodeld
events = Events()
driver_status.get_pose(sm['driverState'],
sm['liveCalibration'].rpyCalib,
sm['carState'].vEgo,
sm['controlsState'].enabled)
# Block engaging after max number of distrations
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
events.add(car.CarEvent.EventName.tooDistracted)
# Update events from driver state
driver_status.update(events, driver_engaged,
sm['controlsState'].enabled,
sm['carState'].standstill)
# build dMonitoringState packet
dat = messaging.new_message('dMonitoringState')
dat.dMonitoringState = {
"events":
events.to_msg(),
"faceDetected":
driver_status.face_detected,
"isDistracted":
driver_status.driver_distracted,
"awarenessStatus":
driver_status.awareness,
"isRHD":
driver_status.is_rhd_region,
"posePitchOffset":
driver_status.pose.pitch_offseter.filtered_stat.mean(),
"posePitchValidCount":
driver_status.pose.pitch_offseter.filtered_stat.n,
"poseYawOffset":
driver_status.pose.yaw_offseter.filtered_stat.mean(),
"poseYawValidCount":
driver_status.pose.yaw_offseter.filtered_stat.n,
"stepChange":
driver_status.step_change,
"awarenessActive":
driver_status.awareness_active,
"awarenessPassive":
driver_status.awareness_passive,
"isLowStd":
driver_status.pose.low_std,
"hiStdCount":
driver_status.hi_stds,
"isPreview":
offroad,
}
pm.send('dMonitoringState', dat)
def manager_init():
# update system time from panda
set_time(cloudlog)
params = Params()
params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START)
default_params = [
("CompletedTrainingVersion", "0"),
("HasAcceptedTerms", "0"),
("OpenpilotEnabledToggle", "1"),
("IsMetric", "1"),
("CommunityFeaturesToggle", "1"),
("EndToEndToggle", "1"),
("IsOpenpilotViewEnabled", "0"),
("OpkrAutoShutdown", "2"),
("OpkrForceShutdown", "5"),
("OpkrAutoScreenOff", "0"),
("OpkrUIBrightness", "0"),
("OpkrUIBrightness", "0"),
("OpkrUIVolumeBoost", "0"),
("OpkrEnableDriverMonitoring", "1"),
("OpkrEnableLogger", "0"),
("OpkrEnableUploader", "0"),
("OpkrEnableGetoffAlert", "1"),
("OpkrAutoResume", "1"),
("OpkrVariableCruise", "1"),
("OpkrLaneChangeSpeed", "45"),
("OpkrAutoLaneChangeDelay", "0"),
("OpkrSteerAngleCorrection", "0"),
("PutPrebuiltOn", "1"),
("LdwsCarFix", "0"),
("LateralControlMethod", "0"),
("CruiseStatemodeSelInit", "1"),
("InnerLoopGain", "35"),
("OuterLoopGain", "20"),
("TimeConstant", "14"),
("ActuatorEffectiveness", "20"),
("Scale", "1500"),
("LqrKi", "15"),
("DcGain", "27"),
("IgnoreZone", "0"),
("PidKp", "20"),
("PidKi", "40"),
("PidKd", "150"),
("PidKf", "7"),
("CameraOffsetAdj", "60"),
("SteerRatioAdj", "155"),
("SteerRatioMaxAdj", "175"),
("SteerActuatorDelayAdj", "20"),
("SteerRateCostAdj", "35"),
("SteerLimitTimerAdj", "80"),
("TireStiffnessFactorAdj", "100"),
("SteerMaxBaseAdj", "384"),
("SteerMaxAdj", "384"),
("SteerDeltaUpBaseAdj", "3"),
("SteerDeltaUpAdj", "3"),
("SteerDeltaDownBaseAdj", "7"),
("SteerDeltaDownAdj", "7"),
("SteerMaxvAdj", "10"),
("OpkrBatteryChargingControl", "1"),
("OpkrBatteryChargingMin", "70"),
("OpkrBatteryChargingMax", "80"),
("LeftCurvOffsetAdj", "0"),
("RightCurvOffsetAdj", "0"),
("DebugUi1", "0"),
("DebugUi2", "0"),
("LongLogDisplay", "0"),
("OpkrBlindSpotDetect", "1"),
("OpkrMaxAngleLimit", "90"),
("OpkrSpeedLimitOffset", "0"),
("OpkrLiveSteerRatio", "1"),
("OpkrVariableSteerMax", "1"),
("OpkrVariableSteerDelta", "0"),
("FingerprintTwoSet", "0"),
("OpkrVariableCruiseProfile", "1"),
("OpkrLiveTune", "0"),
("OpkrDrivingRecord", "0"),
("OpkrTurnSteeringDisable", "0"),
("CarModel", ""),
("CarModelAbb", ""),
("OpkrHotspotOnBoot", "0"),
("OpkrSSHLegacy", "1"),
("ShaneFeedForward", "0"),
("CruiseOverMaxSpeed", "0"),
("JustDoGearD", "0"),
("LanelessMode", "0"),
("ComIssueGone", "0"),
("MaxSteer", "384"),
("MaxRTDelta", "112"),
("MaxRateUp", "3"),
("MaxRateDown", "7"),
("SteerThreshold", "150"),
("RecordingCount", "100"),
("RecordingQuality", "1"),
("CruiseGapAdjust", "0"),
("AutoEnable", "1"),
("CruiseAutoRes", "0"),
("AutoResOption", "0"),
("SteerWindDown", "0"),
("OpkrMonitoringMode", "0"),
("OpkrMonitorEyesThreshold", "75"),
("OpkrMonitorNormalEyesThreshold", "50"),
("OpkrMonitorBlinkThreshold", "50"),
("MadModeEnabled", "1"),
("OpkrFanSpeedGain", "0"),
("WhitePandaSupport", "0"),
("SteerWarningFix", "0"),
("OpkrRunNaviOnBoot", "0"),
("OpkrApksEnable", "0"),
("CruiseGap1", "10"),
("CruiseGap2", "12"),
("CruiseGap3", "15"),
("CruiseGap4", "20"),
("DynamicTR", "2"),
("OpkrBattLess", "0"),
("LCTimingFactorUD", "0"),
("LCTimingFactor30", "10"),
("LCTimingFactor60", "20"),
("LCTimingFactor80", "70"),
("LCTimingFactor110", "110"),
]
if not PC:
default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')))
if params.get_bool("RecordFrontLock"):
params.put_bool("RecordFront", True)
# set unset params
for k, v in default_params:
if params.get(k) is None:
params.put(k, v)
# is this dashcam?
if os.getenv("PASSIVE") is not None:
params.put_bool("Passive", bool(int(os.getenv("PASSIVE"))))
if params.get("Passive") is None:
raise Exception("Passive must be set to continue")
if EON and params.get_bool("OpkrApksEnable"):
update_apks(show_spinner=True)
os.umask(0) # Make sure we can create files with 777 permissions
# Create folders needed for msgq
try:
os.mkdir("/dev/shm")
except FileExistsError:
pass
except PermissionError:
print("WARNING: failed to make /dev/shm")
# set version params
params.put("Version", version)
params.put("TermsVersion", terms_version)
params.put("TrainingVersion", training_version)
params.put("GitCommit", get_git_commit(default=""))
params.put("GitBranch", get_git_branch(default=""))
params.put("GitRemote", get_git_remote(default=""))
# set dongle id
reg_res = register(show_spinner=True)
if reg_res:
dongle_id = reg_res
elif not reg_res:
dongle_id = "maintenance"
else:
serial = params.get("HardwareSerial")
raise Exception(f"Registration failed for device {serial}")
os.environ['DONGLE_ID'] = dongle_id # Needed for swaglog
if not dirty:
os.environ['CLEAN'] = '1'
cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty,
device=HARDWARE.get_device_type())
if comma_remote and not (os.getenv("NOLOG") or os.getenv("NOCRASH") or PC):
crash.init()
# ensure shared libraries are readable by apks
if EON and params.get_bool("OpkrApksEnable"):
os.chmod(BASEDIR, 0o755)
os.chmod("/dev/shm", 0o777)
os.chmod(os.path.join(BASEDIR, "cereal"), 0o755)
crash.bind_user(id=dongle_id)
crash.bind_extra(dirty=dirty, origin=origin, branch=branch, commit=commit,
device=HARDWARE.get_device_type())
os.system("/data/openpilot/gitcommit.sh")
def replay_process(cfg, lr):
sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub]
pub_sockets = [s for s in cfg.pub_sub.keys() if s != 'can']
fsm = FakeSubMaster(pub_sockets)
fpm = FakePubMaster(sub_sockets)
args = (fsm, fpm)
if 'can' in list(cfg.pub_sub.keys()):
can_sock = FakeSocket()
args = (fsm, fpm, can_sock)
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
pub_msgs = [
msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())
]
params = Params()
params.clear_all()
params.manager_start()
params.put("OpenpilotEnabledToggle", "1")
params.put("Passive", "0")
params.put("CommunityFeaturesToggle", "1")
os.environ['NO_RADAR_SLEEP'] = "1"
manager.prepare_managed_process(cfg.proc_name)
mod = importlib.import_module(manager.managed_processes[cfg.proc_name])
thread = threading.Thread(target=mod.main, args=args)
thread.daemon = True
thread.start()
if cfg.init_callback is not None:
if 'can' not in list(cfg.pub_sub.keys()):
can_sock = None
cfg.init_callback(all_msgs, fsm, can_sock)
CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
# wait for started process to be ready
if 'can' in list(cfg.pub_sub.keys()):
can_sock.wait_for_recv()
else:
fsm.wait_for_update()
log_msgs, msg_queue = [], []
for msg in tqdm(pub_msgs):
if cfg.should_recv_callback is not None:
recv_socks, should_recv = cfg.should_recv_callback(
msg, CP, cfg, fsm)
else:
recv_socks = [
s for s in cfg.pub_sub[msg.which()]
if (fsm.frame + 1) % int(service_list[msg.which()].frequency /
service_list[s].frequency) == 0
]
should_recv = bool(len(recv_socks))
if msg.which() == 'can':
can_sock.send(msg.as_builder().to_bytes())
else:
msg_queue.append(msg.as_builder())
if should_recv:
fsm.update_msgs(0, msg_queue)
msg_queue = []
recv_cnt = len(recv_socks)
while recv_cnt > 0:
m = fpm.wait_for_msg()
log_msgs.append(m)
recv_cnt -= m.which() in recv_socks
return log_msgs
def main():
os.environ['PARAMS_PATH'] = PARAMS
if ANDROID:
# the flippening!
os.system(
'LD_LIBRARY_PATH="" content insert --uri content://settings/system --bind name:s:user_rotation --bind value:i:1'
)
# disable bluetooth
os.system('service call bluetooth_manager 8')
params = Params()
params.manager_start()
default_params = [
("CommunityFeaturesToggle", "0"),
("CompletedTrainingVersion", "0"),
("IsRHD", "0"),
("IsMetric", "0"),
("RecordFront", "0"),
("HasAcceptedTerms", "0"),
("HasCompletedSetup", "0"),
("IsUploadRawEnabled", "1"),
("IsLdwEnabled", "1"),
("IsGeofenceEnabled", "-1"),
("SpeedLimitOffset", "0"),
("LongitudinalControl", "0"),
("LimitSetSpeed", "0"),
("LimitSetSpeedNeural", "0"),
("LastUpdateTime",
datetime.datetime.utcnow().isoformat().encode('utf8')),
("OpenpilotEnabledToggle", "1"),
("LaneChangeEnabled", "1"),
("IsDriverViewEnabled", "0"),
("IsOpenpilotViewEnabled", "0"),
("OpkrAutoShutdown", "0"),
("OpkrAutoScreenOff", "0"),
("OpkrUIBrightness", "0"),
("OpkrEnableDriverMonitoring", "1"),
("OpkrEnableLogger", "0"),
("OpkrEnableGetoffAlert", "1"),
("OpkrAutoResume", "1"),
("OpkrAccelProfile", "0"),
("OpkrAutoLanechangedelay", "0"),
("PutPrebuiltOn", "0"),
("FingerprintIssuedFix", "0"),
("LdwsCarFix", "0"),
("LateralControlMethod", "0"),
("CruiseStatemodeSelInit", "0"),
("LateralControlPriority", "0"),
("OuterLoopGain", "20"),
("InnerLoopGain", "30"),
("TimeConstant", "10"),
("ActuatorEffectiveness", "15"),
("Scale", "1630"),
("LqrKi", "10"),
("DcGain", "28"),
("IgnoreZone", "0"),
("PidKp", "20"),
("PidKi", "40"),
("PidKf", "4"),
("CameraOffsetAdj", "60"),
("SteerRatioAdj", "130"),
("SteerActuatorDelayAdj", "20"),
("SteerRateCostAdj", "55"),
("SteerLimitTimerAdj", "240"),
("TireStiffnessFactorAdj", "100"),
("SteerMaxAdj", "430"),
("SteerDeltaUpAdj", "3"),
("SteerDeltaDownAdj", "6"),
]
# set unset params
for k, v in default_params:
if params.get(k) is None:
params.put(k, v)
# is this chffrplus?
if os.getenv("PASSIVE") is not None:
params.put("Passive", str(int(os.getenv("PASSIVE"))))
if params.get("Passive") is None:
raise Exception("Passive must be set to continue")
if ANDROID:
update_apks()
manager_init()
manager_prepare(spinner)
spinner.close()
if os.getenv("PREPAREONLY") is not None:
return
# SystemExit on sigterm
signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1))
try:
manager_thread()
except Exception:
traceback.print_exc()
crash.capture_exception()
finally:
cleanup_all_processes(None, None)
if params.get("DoUninstall", encoding='utf8') == "1":
uninstall()
def thermald_thread():
pm = messaging.PubMaster(['thermal'])
health_timeout = int(1000 * 2.5 *
DT_TRML) # 2.5x the expected health frequency
health_sock = messaging.sub_sock('health', timeout=health_timeout)
location_sock = messaging.sub_sock('gpsLocation')
fan_speed = 0
count = 0
startup_conditions = {
"ignition": False,
}
startup_conditions_prev = startup_conditions.copy()
off_ts = None
started_ts = None
started_seen = False
thermal_status = ThermalStatus.green
usb_power = True
current_branch = get_git_branch()
network_type = NetworkType.none
network_strength = NetworkStrength.unknown
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML)
health_prev = None
should_start_prev = False
handle_fan = None
is_uno = False
params = Params()
power_monitor = PowerMonitoring()
no_panda_cnt = 0
thermal_config = get_thermal_config()
while 1:
health = messaging.recv_sock(health_sock, wait=True)
msg = read_thermal(thermal_config)
if health is not None:
usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client
# If we lose connection to the panda, wait 5 seconds before going offroad
if health.health.hwType == log.HealthData.HwType.unknown:
no_panda_cnt += 1
if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML:
if startup_conditions["ignition"]:
cloudlog.error("Lost panda connection while onroad")
startup_conditions["ignition"] = False
else:
no_panda_cnt = 0
startup_conditions[
"ignition"] = health.health.ignitionLine or health.health.ignitionCan
# Setup fan handler on first connect to panda
if handle_fan is None and health.health.hwType != log.HealthData.HwType.unknown:
is_uno = health.health.hwType == log.HealthData.HwType.uno
if (not EON) or is_uno:
cloudlog.info("Setting up UNO fan handler")
handle_fan = handle_fan_uno
else:
cloudlog.info("Setting up EON fan handler")
setup_eon_fan()
handle_fan = handle_fan_eon
# Handle disconnect
if health_prev is not None:
if health.health.hwType == log.HealthData.HwType.unknown and \
health_prev.health.hwType != log.HealthData.HwType.unknown:
params.panda_disconnect()
health_prev = health
# get_network_type is an expensive call. update every 10s
if (count % int(10. / DT_TRML)) == 0:
try:
network_type = HARDWARE.get_network_type()
network_strength = HARDWARE.get_network_strength(network_type)
except Exception:
cloudlog.exception("Error getting network status")
msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0
msg.thermal.memUsedPercent = int(round(
psutil.virtual_memory().percent))
msg.thermal.cpuPerc = int(round(psutil.cpu_percent()))
msg.thermal.networkType = network_type
msg.thermal.networkStrength = network_strength
msg.thermal.batteryPercent = HARDWARE.get_battery_capacity()
msg.thermal.batteryStatus = HARDWARE.get_battery_status()
msg.thermal.batteryCurrent = HARDWARE.get_battery_current()
msg.thermal.batteryVoltage = HARDWARE.get_battery_voltage()
msg.thermal.usbOnline = HARDWARE.get_usb_present()
# Fake battery levels on uno for frame
if (not EON) or is_uno:
msg.thermal.batteryPercent = 100
msg.thermal.batteryStatus = "Charging"
msg.thermal.bat = 0
current_filter.update(msg.thermal.batteryCurrent / 1e6)
# TODO: add car battery voltage check
max_cpu_temp = cpu_temp_filter.update(max(msg.thermal.cpu))
max_comp_temp = max(max_cpu_temp, msg.thermal.mem,
max(msg.thermal.gpu))
bat_temp = msg.thermal.bat
if handle_fan is not None:
fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed,
startup_conditions["ignition"])
msg.thermal.fanSpeed = fan_speed
# If device is offroad we want to cool down before going onroad
# since going onroad increases load and can make temps go over 107
# We only do this if there is a relay that prevents the car from faulting
is_offroad_for_5_min = (started_ts is None) and (
(not started_seen) or (off_ts is None) or
(sec_since_boot() - off_ts > 60 * 5))
if max_cpu_temp > 107. or bat_temp >= 63. or (is_offroad_for_5_min
and max_cpu_temp > 70.0):
# onroad not allowed
thermal_status = ThermalStatus.danger
elif max_comp_temp > 96.0 or bat_temp > 60.:
# hysteresis between onroad not allowed and engage not allowed
thermal_status = clip(thermal_status, ThermalStatus.red,
ThermalStatus.danger)
elif max_cpu_temp > 94.0:
# hysteresis between engage not allowed and uploader not allowed
thermal_status = clip(thermal_status, ThermalStatus.yellow,
ThermalStatus.red)
elif max_cpu_temp > 80.0:
# uploader not allowed
thermal_status = ThermalStatus.yellow
elif max_cpu_temp > 75.0:
# hysteresis between uploader not allowed and all good
thermal_status = clip(thermal_status, ThermalStatus.green,
ThermalStatus.yellow)
else:
thermal_status = ThermalStatus.green # default to good condition
# **** starting logic ****
# Check for last update time and display alerts if needed
now = datetime.datetime.utcnow()
# show invalid date/time alert
startup_conditions["time_valid"] = (now.year > 2020) or (
now.year == 2020 and now.month >= 10)
set_offroad_alert_if_changed("Offroad_InvalidTime",
(not startup_conditions["time_valid"]))
# Show update prompt
try:
last_update = datetime.datetime.fromisoformat(
params.get("LastUpdateTime", encoding='utf8'))
except (TypeError, ValueError):
last_update = now
dt = now - last_update
update_failed_count = params.get("UpdateFailedCount")
update_failed_count = 0 if update_failed_count is None else int(
update_failed_count)
last_update_exception = params.get("LastUpdateException",
encoding='utf8')
if update_failed_count > 15 and last_update_exception is not None:
if current_branch in ["release2", "dashcam"]:
extra_text = "Ensure the software is correctly installed"
else:
extra_text = last_update_exception
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
False)
set_offroad_alert_if_changed("Offroad_UpdateFailed",
True,
extra_text=extra_text)
elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True)
elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
True,
extra_text=f"{remaining_time} days.")
else:
set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
False)
startup_conditions["up_to_date"] = params.get(
"Offroad_ConnectivityNeeded") is None or params.get(
"DisableUpdates") == b"1"
startup_conditions["not_uninstalling"] = not params.get(
"DoUninstall") == b"1"
startup_conditions["accepted_terms"] = params.get(
"HasAcceptedTerms") == terms_version
panda_signature = params.get("PandaFirmware")
startup_conditions["fw_version_match"] = (panda_signature is None) or (
panda_signature == FW_SIGNATURE
) # don't show alert is no panda is connected (None)
set_offroad_alert_if_changed(
"Offroad_PandaFirmwareMismatch",
(not startup_conditions["fw_version_match"]))
# with 2% left, we killall, otherwise the phone will take a long time to boot
startup_conditions["free_space"] = msg.thermal.freeSpace > 0.02
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \
(current_branch in ['dashcam', 'dashcam-staging'])
startup_conditions["not_driver_view"] = not params.get(
"IsDriverViewEnabled") == b"1"
startup_conditions["not_taking_snapshot"] = not params.get(
"IsTakingSnapshot") == b"1"
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
startup_conditions[
"device_temp_good"] = thermal_status < ThermalStatus.danger
set_offroad_alert_if_changed(
"Offroad_TemperatureTooHigh",
(not startup_conditions["device_temp_good"]))
startup_conditions[
"hardware_supported"] = health is not None and health.health.hwType not in [
log.HealthData.HwType.whitePanda,
log.HealthData.HwType.greyPanda
]
set_offroad_alert_if_changed(
"Offroad_HardwareUnsupported", health is not None
and not startup_conditions["hardware_supported"])
# Handle offroad/onroad transition
should_start = all(startup_conditions.values())
if should_start:
if not should_start_prev:
params.delete("IsOffroad")
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
else:
if startup_conditions["ignition"] and (startup_conditions !=
startup_conditions_prev):
cloudlog.event("Startup blocked",
startup_conditions=startup_conditions)
if should_start_prev or (count == 0):
params.put("IsOffroad", "1")
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
# Offroad power monitoring
power_monitor.calculate(health)
msg.thermal.offroadPowerUsage = power_monitor.get_power_used()
msg.thermal.carBatteryCapacity = max(
0, power_monitor.get_car_battery_capacity())
# Check if we need to disable charging (handled by boardd)
msg.thermal.chargingDisabled = power_monitor.should_disable_charging(
health, off_ts)
# Check if we need to shut down
if power_monitor.should_shutdown(health, off_ts, started_seen, LEON):
cloudlog.info(f"shutting device down, offroad since {off_ts}")
# TODO: add function for blocking cloudlog instead of sleep
time.sleep(10)
os.system('LD_LIBRARY_PATH="" svc power shutdown')
msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.thermal.started = started_ts is not None
msg.thermal.startedTs = int(1e9 * (started_ts or 0))
msg.thermal.thermalStatus = thermal_status
pm.send("thermal", msg)
set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power))
should_start_prev = should_start
startup_conditions_prev = startup_conditions.copy()
# report to server once per minute
if (count % int(60. / DT_TRML)) == 0:
location = messaging.recv_sock(location_sock)
cloudlog.event("STATUS_PACKET",
count=count,
health=(health.to_dict() if health else None),
location=(location.gpsLocation.to_dict()
if location else None),
thermal=msg.to_dict())
count += 1
#
# courtesy of pjlao307 (https://github.com/pjlao307/)
# this is just his original implementation but
# in openpilot service form so it's always on
#
# with the highest bit rates, the video is approx. 0.5MB per second
# the default value is set to 2.56Mbps = 0.32MB per second
#
import os
import time
import datetime
import cereal.messaging as messaging
import subprocess
from selfdrive.swaglog import cloudlog
from common.params import Params
params = Params()
dashcam_videos = '/sdcard/dashcam/'
duration = 60 # max is 180
bit_rates = 2560000 # max is 4000000
max_size_per_file = bit_rates / 8 * duration # 2.56Mbps / 8 * 60 = 19.2MB per 60 seconds
max_storage = max_size_per_file / duration * 60 * 60 * 6 # 6 hours worth of footage (around 7gb)
freespace_limit = 0.15 # we start cleaning up footage when freespace is below 15%
def main(gctx=None):
if not os.path.exists(dashcam_videos):
os.makedirs(dashcam_videos)
thermal_sock = messaging.sub_sock('thermal')
while 1:
def manager_thread():
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
# save boot log
subprocess.call("./bootlog",
cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
# start daemon processes
for p in daemon_processes:
start_daemon_process(p)
# start persistent processes
for p in persistent_processes:
start_managed_process(p)
# start offroad
if EON:
pm_apply_packages('enable')
start_offroad()
if os.getenv("NOBOARD") is not None:
del managed_processes["pandad"]
if os.getenv("BLOCK") is not None:
for k in os.getenv("BLOCK").split(","):
del managed_processes[k]
started_prev = False
logger_dead = False
params = Params()
thermal_sock = messaging.sub_sock('thermal')
pm = messaging.PubMaster(['managerState'])
while 1:
msg = messaging.recv_sock(thermal_sock, wait=True)
if msg.thermal.freeSpacePercent < 0.05:
logger_dead = True
if msg.thermal.started:
for p in car_started_processes:
if p == "loggerd" and logger_dead:
kill_managed_process(p)
else:
start_managed_process(p)
else:
logger_dead = False
driver_view = params.get("IsDriverViewEnabled") == b"1"
# TODO: refactor how manager manages processes
for p in reversed(car_started_processes):
if p not in driver_view_processes or not driver_view:
kill_managed_process(p)
for p in driver_view_processes:
if driver_view:
start_managed_process(p)
else:
kill_managed_process(p)
# trigger an update after going offroad
if started_prev:
os.sync()
send_managed_process_signal("updated", signal.SIGHUP)
started_prev = msg.thermal.started
# check the status of all processes, did any of them die?
running_list = [
"%s%s\u001b[0m" %
("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p)
for p in running
]
cloudlog.debug(' '.join(running_list))
# send managerState
states = []
for p in managed_processes:
state = log.ManagerState.ProcessState.new_message()
state.name = p
if p in running:
state.running = running[p].is_alive()
state.pid = running[p].pid
state.exitCode = running[p].exitcode or 0
states.append(state)
msg = messaging.new_message('managerState')
msg.managerState.processes = states
pm.send('managerState', msg)
# Exit main loop when uninstall is needed
if params.get("DoUninstall", encoding='utf8') == "1":
break
def register():
params = Params()
params.put("Version", version)
params.put("TermsVersion", terms_version)
params.put("TrainingVersion", training_version)
params.put("GitCommit", get_git_commit())
params.put("GitBranch", get_git_branch())
params.put("GitRemote", get_git_remote())
params.put("SubscriberInfo", get_subscriber_info())
# create a key for auth
# your private key is kept on your device persist partition and never sent to our servers
# do not erase your persist partition
if not os.path.isfile("/persist/comma/id_rsa.pub"):
cloudlog.warning("generating your personal RSA key")
mkdirs_exists_ok("/persist/comma")
assert os.system(
"openssl genrsa -out /persist/comma/id_rsa.tmp 2048") == 0
assert os.system(
"openssl rsa -in /persist/comma/id_rsa.tmp -pubout -out /persist/comma/id_rsa.tmp.pub"
) == 0
os.rename("/persist/comma/id_rsa.tmp", "/persist/comma/id_rsa")
os.rename("/persist/comma/id_rsa.tmp.pub", "/persist/comma/id_rsa.pub")
# make key readable by app users (ai.comma.plus.offroad)
os.chmod('/persist/comma/', 0o755)
os.chmod('/persist/comma/id_rsa', 0o744)
dongle_id, access_token = params.get("DongleId"), params.get("AccessToken")
public_key = open("/persist/comma/id_rsa.pub").read()
# create registration token
# in the future, this key will make JWTs directly
private_key = open("/persist/comma/id_rsa").read()
# late import
import jwt
register_token = jwt.encode(
{
'register': True,
'exp': datetime.utcnow() + timedelta(hours=1)
},
private_key,
algorithm='RS256')
try:
cloudlog.info("getting pilotauth")
resp = api_get("v2/pilotauth/",
method='POST',
timeout=15,
imei=get_imei(),
serial=get_serial(),
public_key=public_key,
register_token=register_token)
dongleauth = json.loads(resp.text)
dongle_id, access_token = dongleauth["dongle_id"].encode(
'ascii'), dongleauth["access_token"].encode('ascii')
params.put("DongleId", dongle_id)
params.put("AccessToken", access_token)
return dongle_id, access_token
except Exception:
cloudlog.exception("failed to authenticate")
if dongle_id is not None and access_token is not None:
return dongle_id, access_token
else:
return None
def main():
params = Params()
params.manager_start()
default_params = [
("CommunityFeaturesToggle", "0"),
("CompletedTrainingVersion", "0"),
("IsRHD", "0"),
("IsMetric", "0"),
("RecordFront", "0"),
("HasAcceptedTerms", "0"),
("HasCompletedSetup", "0"),
("IsUploadRawEnabled", "1"),
("IsLdwEnabled", "1"),
("LastUpdateTime",
datetime.datetime.utcnow().isoformat().encode('utf8')),
("OpenpilotEnabledToggle", "1"),
("VisionRadarToggle", "0"),
("LaneChangeEnabled", "1"),
("IsDriverViewEnabled", "0"),
]
# set unset params
for k, v in default_params:
if params.get(k) is None:
params.put(k, v)
# is this dashcam?
if os.getenv("PASSIVE") is not None:
params.put("Passive", str(int(os.getenv("PASSIVE"))))
if params.get("Passive") is None:
raise Exception("Passive must be set to continue")
if EON:
update_apks()
manager_init()
manager_prepare()
spinner.close()
if os.getenv("PREPAREONLY") is not None:
return
# SystemExit on sigterm
signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1))
try:
manager_thread()
except Exception:
traceback.print_exc()
crash.capture_exception()
finally:
cleanup_all_processes(None, None)
if params.get("DoUninstall", encoding='utf8') == "1":
cloudlog.warning("uninstalling")
HARDWARE.uninstall()
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.sm = sm
if self.sm is None:
self.sm = messaging.SubMaster([
'thermal', 'health', 'model', 'liveCalibration', 'frontFrame',
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman'
])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one health and one CAN packet
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
# read params
params = Params()
self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
self.is_ldw_enabled = params.get("IsLdwEnabled",
encoding='utf8') == "1"
internet_needed = (params.get("Offroad_ConnectivityNeeded",
encoding='utf8') is not None) and (
params.get("DisableUpdates") != b"1")
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1" or \
internet_needed or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
if self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
self.sm['thermal'].freeSpace = 1.
self.sm['dMonitoringState'].events = []
self.sm['dMonitoringState'].awarenessStatus = 1.
self.sm['dMonitoringState'].faceDetected = False
self.startup_event = get_startup_event(car_recognized,
controller_available)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if internet_needed:
self.events.add(EventName.internetConnectivityNeeded, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
def lead_control(self, CS, sm, CC):
dRel = CC.dRel
yRel = CC.yRel
vRel = CC.vRel
active_time = 10
btn_type = Buttons.NONE
#lead_1 = sm['radarState'].leadOne
long_wait_cmd = 500
set_speed = int(round(self.cruise_set_speed_kph))
if self.long_curv_timer < 600:
self.long_curv_timer += 1
# 선행 차량 거리유지
lead_wait_cmd, lead_set_speed = self.update_lead(
sm, CS, dRel, yRel, vRel)
# 커브 감속.
model_speed = CC.model_speed #calc_va( CS.out.vEgo )
curv_wait_cmd, curv_set_speed = self.update_curv(CS, sm, model_speed)
if curv_wait_cmd != 0:
if lead_set_speed > curv_set_speed:
set_speed = curv_set_speed
long_wait_cmd = curv_wait_cmd
else:
set_speed = lead_set_speed
long_wait_cmd = lead_wait_cmd
else:
set_speed = lead_set_speed
long_wait_cmd = lead_wait_cmd
if set_speed >= int(round(self.cruise_set_speed_kph)):
set_speed = int(round(self.cruise_set_speed_kph))
elif set_speed <= 30:
set_speed = 30
# control process
target_set_speed = set_speed
delta = int(round(set_speed)) - int(CS.VSetDis)
dec_step_cmd = 1
camspeed = Params().get("LimitSetSpeedCamera", encoding="utf8")
if camspeed is not None:
self.map_spd_camera = int(float(camspeed.rstrip('\n')))
self.map_spd_enable = self.map_spd_camera > 29
else:
self.map_spd_enable = False
self.map_spd_camera = 0
self.map_spd_limit_offset = int(Params().get("OpkrSpeedLimitOffset",
encoding='utf8'))
if self.long_curv_timer < long_wait_cmd:
pass
elif delta > 0:
if ((self.map_spd_camera +
round(self.map_spd_camera * 0.01 * self.map_spd_limit_offset))
== int(CS.VSetDis)) and self.map_spd_enable:
set_speed = int(CS.VSetDis) + 0
btn_type = Buttons.NONE
self.long_curv_timer = 0
else:
set_speed = int(CS.VSetDis) + dec_step_cmd
btn_type = Buttons.RES_ACCEL
self.long_curv_timer = 0
elif delta < 0:
set_speed = int(CS.VSetDis) - dec_step_cmd
btn_type = Buttons.SET_DECEL
self.long_curv_timer = 0
if self.cruise_set_mode == 0:
btn_type = Buttons.NONE
self.update_log(CS, set_speed, target_set_speed, long_wait_cmd)
return btn_type, set_speed, active_time
def python_replay_process(cfg, lr):
sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub]
pub_sockets = [s for s in cfg.pub_sub.keys() if s != 'can']
fsm = FakeSubMaster(pub_sockets)
fpm = FakePubMaster(sub_sockets)
args = (fsm, fpm)
if 'can' in list(cfg.pub_sub.keys()):
can_sock = FakeSocket()
args = (fsm, fpm, can_sock)
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
pub_msgs = [
msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())
]
params = Params()
params.clear_all()
params.put_bool("OpenpilotEnabledToggle", True)
params.put_bool("Passive", False)
params.put_bool("CommunityFeaturesToggle", True)
os.environ['NO_RADAR_SLEEP'] = "1"
os.environ['SKIP_FW_QUERY'] = ""
os.environ['FINGERPRINT'] = ""
# TODO: remove after getting new route for civic & accord
migration = {
"HONDA CIVIC 2016 TOURING": "HONDA CIVIC 2016",
"HONDA ACCORD 2018 SPORT 2T": "HONDA ACCORD 2018 2T",
}
for msg in lr:
if msg.which() == 'carParams':
car_fingerprint = migration.get(msg.carParams.carFingerprint,
msg.carParams.carFingerprint)
if len(msg.carParams.carFw) and (car_fingerprint in FW_VERSIONS):
params.put("CarParamsCache",
msg.carParams.as_builder().to_bytes())
else:
os.environ['SKIP_FW_QUERY'] = "1"
os.environ['FINGERPRINT'] = car_fingerprint
assert (type(managed_processes[cfg.proc_name]) is PythonProcess)
managed_processes[cfg.proc_name].prepare()
mod = importlib.import_module(managed_processes[cfg.proc_name].module)
thread = threading.Thread(target=mod.main, args=args)
thread.daemon = True
thread.start()
if cfg.init_callback is not None:
if 'can' not in list(cfg.pub_sub.keys()):
can_sock = None
cfg.init_callback(all_msgs, fsm, can_sock)
CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
# wait for started process to be ready
if 'can' in list(cfg.pub_sub.keys()):
can_sock.wait_for_recv()
else:
fsm.wait_for_update()
log_msgs, msg_queue = [], []
for msg in tqdm(pub_msgs, disable=CI):
if cfg.should_recv_callback is not None:
recv_socks, should_recv = cfg.should_recv_callback(
msg, CP, cfg, fsm)
else:
recv_socks = [
s for s in cfg.pub_sub[msg.which()]
if (fsm.frame + 1) % int(service_list[msg.which()].frequency /
service_list[s].frequency) == 0
]
should_recv = bool(len(recv_socks))
if msg.which() == 'can':
can_sock.send(msg.as_builder().to_bytes())
else:
msg_queue.append(msg.as_builder())
if should_recv:
fsm.update_msgs(0, msg_queue)
msg_queue = []
recv_cnt = len(recv_socks)
while recv_cnt > 0:
m = fpm.wait_for_msg()
log_msgs.append(m)
recv_cnt -= m.which() in recv_socks
return log_msgs
class SpdController():
def __init__(self, CP=None):
self.long_control_state = 0 # initialized to off
self.seq_step_debug = ""
self.long_curv_timer = 0
self.path_x = np.arange(192)
self.traceSC = trace1.Loger("SPD_CTRL")
self.wheelbase = 2.8
self.steerRatio = 13.5 # 13.5
self.v_model = 0
self.a_model = 0
self.v_cruise = 0
self.a_cruise = 0
self.l_poly = []
self.r_poly = []
self.movAvg = moveavg1.MoveAvg()
self.Timer1 = tm.CTime1000("SPD")
self.time_no_lean = 0
self.wait_timer2 = 0
self.cruise_set_speed_kph = 0
self.curise_set_first = 0
self.curise_sw_check = 0
self.prev_clu_CruiseSwState = 0
self.prev_VSetDis = 0
self.sc_clu_speed = 0
self.btn_type = Buttons.NONE
self.active_time = 0
self.old_model_speed = 0
self.old_model_init = 0
self.params = Params()
self.cruise_set_mode = int(self.params.get('CruiseStatemodeSelInit'))
self.map_spd_enable = False
self.map_spd_camera = 0
def reset(self):
self.v_model = 0
self.a_model = 0
self.v_cruise = 0
self.a_cruise = 0
def calc_laneProb(self, prob, v_ego):
if len(prob) >= 32:
path = list(prob)
# TODO: compute max speed without using a list of points and without numpy
y_p = 3 * path[0] * self.path_x**2 + \
2 * path[15] * self.path_x + path[30]
y_pp = 6 * path[0] * self.path_x + 2 * path[15]
curv = y_pp / (1. + y_p**2)**1.5
#print( 'curv = {}'.format( curv) )
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
model_speed = np.min(v_curvature)
# Don't slow down below 20mph
# model_speed = max(30.0 * CV.KPH_TO_MS, model_speed)
# print( 'v_curvature = {}'.format( v_curvature) )
#print( 'model_speed = {} '.format( model_speed) )
# model_speed = model_speed * CV.MS_TO_KPH
#if model_speed > MAX_SPEED:
# model_speed = MAX_SPEED
else:
model_speed = MAX_SPEED
return model_speed
def calc_va(self, sm, v_ego):
md = sm['modelV2']
#print('{}'.format( md ) )
if len(md.laneLines) > 1:
self.prob = md.laneLines[1].y
#self.prob = md.curv
model_speed = self.calc_laneProb(self.prob, v_ego)
delta_model = model_speed - self.old_model_speed
if self.old_model_init < 10:
self.old_model_init += 1
self.old_model_speed = model_speed
elif self.old_model_speed == model_speed:
pass
elif delta_model < -1:
self.old_model_speed -= 1 #model_speed
elif delta_model > 0:
self.old_model_speed += 0.1
else:
self.old_model_speed = model_speed
return self.old_model_speed
def update_cruiseSW(self, CS):
set_speed_kph = int(round(self.cruise_set_speed_kph))
delta_vsetdis = 0
if CS.acc_active:
delta_vsetdis = abs(int(CS.VSetDis) - self.prev_VSetDis)
if self.prev_clu_CruiseSwState != CS.cruise_buttons:
if CS.cruise_buttons == Buttons.RES_ACCEL or CS.cruise_buttons == Buttons.SET_DECEL:
self.prev_VSetDis = int(CS.VSetDis)
elif CS.driverOverride:
set_speed_kph = int(CS.VSetDis)
elif self.prev_clu_CruiseSwState == Buttons.RES_ACCEL: # up
if self.curise_set_first:
self.curise_set_first = 0
set_speed_kph = int(CS.VSetDis)
elif delta_vsetdis > 0:
set_speed_kph = int(CS.VSetDis)
elif not self.curise_sw_check:
set_speed_kph += 1
elif self.prev_clu_CruiseSwState == Buttons.SET_DECEL: # dn
if self.curise_set_first:
self.curise_set_first = 0
set_speed_kph = int(CS.VSetDis)
elif delta_vsetdis > 0:
set_speed_kph = int(CS.VSetDis)
elif not self.curise_sw_check:
set_speed_kph -= 1
self.prev_clu_CruiseSwState = CS.cruise_buttons
elif (CS.cruise_buttons == Buttons.RES_ACCEL or CS.cruise_buttons
== Buttons.SET_DECEL) and delta_vsetdis > 0:
self.curise_sw_check = True
set_speed_kph = int(CS.VSetDis)
else:
self.curise_sw_check = False
self.curise_set_first = 1
self.prev_VSetDis = int(CS.VSetDis)
set_speed_kph = int(CS.VSetDis)
if self.prev_clu_CruiseSwState != CS.cruise_buttons: # MODE 전환.
if CS.cruise_buttons == Buttons.GAP_DIST and not CS.acc_active and CS.out.cruiseState.available:
self.cruise_set_mode += 1
if self.cruise_set_mode > 3:
self.cruise_set_mode = 0
self.prev_clu_CruiseSwState = CS.cruise_buttons
if set_speed_kph <= 30:
set_speed_kph = 30
self.cruise_set_speed_kph = set_speed_kph
return self.cruise_set_mode, set_speed_kph
@staticmethod
def get_lead(sm):
plan = sm['longitudinalPlan']
if 0 < plan.dRel1 < 149:
dRel = int(plan.dRel1) #EON Lead
yRel = int(plan.yRel1) #EON Lead
vRel = int(plan.vRel1 * 3.6 + 0.5) #EON Lead
else:
dRel = 150
yRel = 0
vRel = 0
return dRel, yRel, vRel
def get_tm_speed(self, CS, set_time, add_val, safety_dis=5):
time = int(set_time)
delta_speed = int(CS.VSetDis) - int(round(CS.clu_Vanz))
set_speed = int(CS.VSetDis) + add_val
if add_val > 0: # 증가
if delta_speed > safety_dis:
time = int(set_time)
else:
if delta_speed < -safety_dis:
time = int(set_time)
return time, set_speed
# returns a
def update_lead(self, c, can_strings):
raise NotImplementedError
def update_curv(self, CS, sm, model_speed):
raise NotImplementedError
def update_log(self, CS, set_speed, target_set_speed, long_wait_cmd):
str3 = 'M={:3.0f} DST={:3.0f} VSD={:.0f} DA={:.0f}/{:.0f}/{:.0f} DG={:s} DO={:.0f}'.format(
CS.out.cruiseState.modeSel, target_set_speed, CS.VSetDis,
CS.driverAcc_time, long_wait_cmd, self.long_curv_timer,
self.seq_step_debug, CS.driverOverride)
str4 = ' CS={:.1f}/{:.1f} '.format(CS.lead_distance, CS.lead_objspd)
str5 = str3 + str4
trace1.printf2(str5)
def lead_control(self, CS, sm, CC):
dRel = CC.dRel
yRel = CC.yRel
vRel = CC.vRel
active_time = 10
btn_type = Buttons.NONE
#lead_1 = sm['radarState'].leadOne
long_wait_cmd = 500
set_speed = int(round(self.cruise_set_speed_kph))
if self.long_curv_timer < 600:
self.long_curv_timer += 1
# 선행 차량 거리유지
lead_wait_cmd, lead_set_speed = self.update_lead(
sm, CS, dRel, yRel, vRel)
# 커브 감속.
model_speed = CC.model_speed #calc_va( CS.out.vEgo )
curv_wait_cmd, curv_set_speed = self.update_curv(CS, sm, model_speed)
if curv_wait_cmd != 0:
if lead_set_speed > curv_set_speed:
set_speed = curv_set_speed
long_wait_cmd = curv_wait_cmd
else:
set_speed = lead_set_speed
long_wait_cmd = lead_wait_cmd
else:
set_speed = lead_set_speed
long_wait_cmd = lead_wait_cmd
if set_speed >= int(round(self.cruise_set_speed_kph)):
set_speed = int(round(self.cruise_set_speed_kph))
elif set_speed <= 30:
set_speed = 30
# control process
target_set_speed = set_speed
delta = int(round(set_speed)) - int(CS.VSetDis)
dec_step_cmd = 1
camspeed = Params().get("LimitSetSpeedCamera", encoding="utf8")
if camspeed is not None:
self.map_spd_camera = int(float(camspeed.rstrip('\n')))
self.map_spd_enable = self.map_spd_camera > 29
else:
self.map_spd_enable = False
self.map_spd_camera = 0
self.map_spd_limit_offset = int(Params().get("OpkrSpeedLimitOffset",
encoding='utf8'))
if self.long_curv_timer < long_wait_cmd:
pass
elif delta > 0:
if ((self.map_spd_camera +
round(self.map_spd_camera * 0.01 * self.map_spd_limit_offset))
== int(CS.VSetDis)) and self.map_spd_enable:
set_speed = int(CS.VSetDis) + 0
btn_type = Buttons.NONE
self.long_curv_timer = 0
else:
set_speed = int(CS.VSetDis) + dec_step_cmd
btn_type = Buttons.RES_ACCEL
self.long_curv_timer = 0
elif delta < 0:
set_speed = int(CS.VSetDis) - dec_step_cmd
btn_type = Buttons.SET_DECEL
self.long_curv_timer = 0
if self.cruise_set_mode == 0:
btn_type = Buttons.NONE
self.update_log(CS, set_speed, target_set_speed, long_wait_cmd)
return btn_type, set_speed, active_time
def update(self, CS, sm, CC):
self.cruise_set_mode = CS.out.cruiseState.modeSel
#self.cruise_set_speed_kph = int(round(CS.out.cruiseState.speed * CV.MS_TO_KPH))
self.cruise_set_speed_kph = int(round(CC.vCruiseSet))
if CS.driverOverride == 2 or not CS.acc_active or CS.cruise_buttons == Buttons.RES_ACCEL or CS.cruise_buttons == Buttons.SET_DECEL:
self.resume_cnt = 0
self.btn_type = Buttons.NONE
self.wait_timer2 = 10
self.active_timer2 = 0
elif self.wait_timer2:
self.wait_timer2 -= 1
else:
btn_type, clu_speed, active_time = self.lead_control(
CS, sm, CC) # speed controller spdcontroller.py
if (0 <= int(CS.clu_Vanz) <= 1
or 7 < int(CS.clu_Vanz) < 15) and CC.vRel <= 0:
self.btn_type = Buttons.NONE
elif self.btn_type != Buttons.NONE:
pass
elif btn_type != Buttons.NONE:
self.resume_cnt = 0
self.active_timer2 = 0
self.btn_type = btn_type
self.sc_clu_speed = clu_speed
self.active_time = max(5, active_time)
if self.btn_type != Buttons.NONE:
self.active_timer2 += 1
if self.active_timer2 > self.active_time:
self.wait_timer2 = 5
self.resume_cnt = 0
self.active_timer2 = 0
self.btn_type = Buttons.NONE
else:
return 1
return 0
def thermald_thread():
pm = messaging.PubMaster(['deviceState'])
pandaState_timeout = int(1000 * 2.5 *
DT_TRML) # 2.5x the expected pandaState frequency
pandaState_sock = messaging.sub_sock('pandaState',
timeout=pandaState_timeout)
location_sock = messaging.sub_sock('gpsLocationExternal')
managerState_sock = messaging.sub_sock('managerState', conflate=True)
fan_speed = 0
count = 0
startup_conditions = {
"ignition": False,
}
startup_conditions_prev = startup_conditions.copy()
off_ts = None
started_ts = None
started_seen = False
thermal_status = ThermalStatus.green
usb_power = True
current_branch = get_git_branch()
network_type = NetworkType.none
network_strength = NetworkStrength.unknown
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML)
charging_disabled = False #CHO: Add battery management
pandaState_prev = None
should_start_prev = False
handle_fan = None
is_uno = False
#ui_running_prev = False
ui_running_prev = True #CHO: Add battery management
params = Params()
power_monitor = PowerMonitoring()
no_panda_cnt = 0
thermal_config = HARDWARE.get_thermal_config()
# CPR3 logging
if EON:
base_path = "/sys/kernel/debug/cpr3-regulator/"
cpr_files = [p for p in Path(base_path).glob("**/*") if p.is_file()]
cpr_data = {}
for cf in cpr_files:
with open(cf, "r") as f:
try:
cpr_data[str(cf)] = f.read().strip()
except Exception:
pass
cloudlog.event("CPR", data=cpr_data)
while 1:
pandaState = messaging.recv_sock(pandaState_sock, wait=True)
msg = read_thermal(thermal_config)
if pandaState is not None:
usb_power = pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client
# If we lose connection to the panda, wait 5 seconds before going offroad
if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown:
no_panda_cnt += 1
if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML:
if startup_conditions["ignition"]:
cloudlog.error("Lost panda connection while onroad")
startup_conditions["ignition"] = False
else:
no_panda_cnt = 0
startup_conditions[
"ignition"] = pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan
startup_conditions["hardware_supported"] = True
set_offroad_alert_if_changed(
"Offroad_HardwareUnsupported",
not startup_conditions["hardware_supported"])
# Setup fan handler on first connect to panda
if handle_fan is None and pandaState.pandaState.pandaType != log.PandaState.PandaType.unknown:
is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno
if (not EON) or is_uno:
cloudlog.info("Setting up UNO fan handler")
handle_fan = handle_fan_uno
else:
cloudlog.info("Setting up EON fan handler")
setup_eon_fan()
handle_fan = handle_fan_eon
# Handle disconnect
if pandaState_prev is not None:
if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown and \
pandaState_prev.pandaState.pandaType != log.PandaState.PandaType.unknown:
params.panda_disconnect()
pandaState_prev = pandaState
# get_network_type is an expensive call. update every 10s
if (count % int(10. / DT_TRML)) == 0:
try:
network_type = HARDWARE.get_network_type()
network_strength = HARDWARE.get_network_strength(network_type)
except Exception:
cloudlog.exception("Error getting network status")
msg.deviceState.freeSpacePercent = get_available_percent(default=100.0)
msg.deviceState.memoryUsagePercent = int(
round(psutil.virtual_memory().percent))
msg.deviceState.cpuUsagePercent = int(round(psutil.cpu_percent()))
msg.deviceState.networkType = network_type
msg.deviceState.networkStrength = network_strength
msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity()
msg.deviceState.batteryStatus = HARDWARE.get_battery_status()
msg.deviceState.batteryCurrent = HARDWARE.get_battery_current()
msg.deviceState.batteryVoltage = HARDWARE.get_battery_voltage()
msg.deviceState.usbOnline = HARDWARE.get_usb_present()
# Fake battery levels on uno for frame
if (not EON) or is_uno:
msg.deviceState.batteryPercent = 100
msg.deviceState.batteryStatus = "Charging"
msg.deviceState.batteryTempC = 0
current_filter.update(msg.deviceState.batteryCurrent / 1e6)
# TODO: add car battery voltage check
max_cpu_temp = cpu_temp_filter.update(max(msg.deviceState.cpuTempC))
max_comp_temp = max(max_cpu_temp, msg.deviceState.memoryTempC,
max(msg.deviceState.gpuTempC))
bat_temp = msg.deviceState.batteryTempC
if handle_fan is not None:
fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed,
startup_conditions["ignition"])
msg.deviceState.fanSpeedPercentDesired = fan_speed
# If device is offroad we want to cool down before going onroad
# since going onroad increases load and can make temps go over 107
# We only do this if there is a relay that prevents the car from faulting
is_offroad_for_5_min = (started_ts is None) and (
(not started_seen) or (off_ts is None) or
(sec_since_boot() - off_ts > 60 * 5))
if max_cpu_temp > 107. or bat_temp >= 63. or (is_offroad_for_5_min
and max_cpu_temp > 70.0):
# onroad not allowed
thermal_status = ThermalStatus.danger
elif max_comp_temp > 96.0 or bat_temp > 60.:
# hysteresis between onroad not allowed and engage not allowed
thermal_status = clip(thermal_status, ThermalStatus.red,
ThermalStatus.danger)
elif max_cpu_temp > 94.0:
# hysteresis between engage not allowed and uploader not allowed
thermal_status = clip(thermal_status, ThermalStatus.yellow,
ThermalStatus.red)
elif max_cpu_temp > 80.0:
# uploader not allowed
thermal_status = ThermalStatus.yellow
elif max_cpu_temp > 75.0:
# hysteresis between uploader not allowed and all good
thermal_status = clip(thermal_status, ThermalStatus.green,
ThermalStatus.yellow)
else:
thermal_status = ThermalStatus.green # default to good condition
# **** starting logic ****
# Check for last update time and display alerts if needed
now = datetime.datetime.utcnow()
# show invalid date/time alert
startup_conditions["time_valid"] = (now.year > 2020) or (
now.year == 2020 and now.month >= 10)
set_offroad_alert_if_changed("Offroad_InvalidTime",
(not startup_conditions["time_valid"]))
# Show update prompt
try:
last_update = datetime.datetime.fromisoformat(
params.get("LastUpdateTime", encoding='utf8'))
except (TypeError, ValueError):
last_update = now
dt = now - last_update
update_failed_count = params.get("UpdateFailedCount")
update_failed_count = 0 if update_failed_count is None else int(
update_failed_count)
last_update_exception = params.get("LastUpdateException",
encoding='utf8')
if update_failed_count > 15 and last_update_exception is not None:
if current_branch in ["release2", "dashcam"]:
extra_text = "Ensure the software is correctly installed"
else:
extra_text = last_update_exception
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
False)
set_offroad_alert_if_changed("Offroad_UpdateFailed",
True,
extra_text=extra_text)
elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", True)
elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
True,
extra_text=f"{remaining_time} days.")
else:
set_offroad_alert_if_changed("Offroad_UpdateFailed", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False)
set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt",
False)
startup_conditions["up_to_date"] = params.get(
"Offroad_ConnectivityNeeded") is None or params.get(
"DisableUpdates") == b"1"
startup_conditions["not_uninstalling"] = not params.get(
"DoUninstall") == b"1"
startup_conditions["accepted_terms"] = params.get(
"HasAcceptedTerms") == terms_version
panda_signature = params.get("PandaFirmware")
startup_conditions["fw_version_match"] = (panda_signature is None) or (
panda_signature == FW_SIGNATURE
) # don't show alert is no panda is connected (None)
set_offroad_alert_if_changed(
"Offroad_PandaFirmwareMismatch",
(not startup_conditions["fw_version_match"]))
# with 2% left, we killall, otherwise the phone will take a long time to boot
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \
(current_branch in ['dashcam', 'dashcam-staging'])
startup_conditions["not_driver_view"] = not params.get(
"IsDriverViewEnabled") == b"1"
startup_conditions["not_taking_snapshot"] = not params.get(
"IsTakingSnapshot") == b"1"
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
startup_conditions[
"device_temp_good"] = thermal_status < ThermalStatus.danger
set_offroad_alert_if_changed(
"Offroad_TemperatureTooHigh",
(not startup_conditions["device_temp_good"]))
# Handle offroad/onroad transition
should_start = all(startup_conditions.values())
if should_start:
if not should_start_prev:
params.delete("IsOffroad")
if TICI and DISABLE_LTE_ONROAD:
os.system("sudo systemctl stop --no-block lte")
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
else:
if startup_conditions["ignition"] and (startup_conditions !=
startup_conditions_prev):
cloudlog.event("Startup blocked",
startup_conditions=startup_conditions)
if should_start_prev or (count == 0):
params.put("IsOffroad", "1")
if TICI and DISABLE_LTE_ONROAD:
os.system("sudo systemctl start --no-block lte")
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
#CHO: Added battery management
charging_disabled = check_car_battery_voltage(should_start, pandaState,
charging_disabled, msg)
if msg.deviceState.batteryCurrent > 0:
msg.deviceState.batteryStatus = "Discharging"
else:
msg.deviceState.batteryStatus = "Charging"
msg.deviceState.chargingDisabled = charging_disabled
#CHO: END
# Offroad power monitoring
power_monitor.calculate(pandaState)
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(
0, power_monitor.get_car_battery_capacity())
# Check if we need to disable charging (handled by boardd)
msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(
pandaState, off_ts)
# Check if we need to shut down
if power_monitor.should_shutdown(pandaState, off_ts, started_seen):
cloudlog.info(f"shutting device down, offroad since {off_ts}")
# TODO: add function for blocking cloudlog instead of sleep
time.sleep(10)
HARDWARE.shutdown()
# If UI has crashed, set the brightness to reasonable non-zero value
manager_state = messaging.recv_one_or_none(managerState_sock)
if manager_state is not None:
ui_running = "ui" in (p.name
for p in manager_state.managerState.processes
if p.running)
if ui_running_prev and not ui_running:
HARDWARE.set_screen_brightness(20)
ui_running_prev = ui_running
msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.deviceState.started = started_ts is not None
msg.deviceState.startedMonoTime = int(1e9 * (started_ts or 0))
msg.deviceState.thermalStatus = thermal_status
pm.send("deviceState", msg)
if EON and not is_uno:
set_offroad_alert_if_changed("Offroad_ChargeDisabled",
(not usb_power))
should_start_prev = should_start
startup_conditions_prev = startup_conditions.copy()
# report to server once per minute
if (count % int(60. / DT_TRML)) == 0:
location = messaging.recv_sock(location_sock)
cloudlog.event("STATUS_PACKET",
count=count,
pandaState=(strip_deprecated_keys(
pandaState.to_dict()) if pandaState else None),
location=(strip_deprecated_keys(
location.gpsLocationExternal.to_dict())
if location else None),
deviceState=strip_deprecated_keys(msg.to_dict()))
count += 1
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.camera_packets = ["roadCameraState", "driverCameraState"]
if TICI:
self.camera_packets.append("wideRoadCameraState")
params = Params()
self.joystick_mode = params.get_bool("JoystickDebugMode")
joystick_packet = ['testJoystick'] if self.joystick_mode else []
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'
] if SIMULATION else None
self.sm = messaging.SubMaster(
[
'deviceState', 'pandaStates', 'peripheralState', 'modelV2',
'liveCalibration', 'driverMonitoringState',
'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState'
] + self.camera_packets + joystick_packet,
ignore_alive=ignore,
ignore_avg_freq=['radarState', 'longitudinalPlan'])
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
if TICI:
self.log_sock = messaging.sub_sock('androidLog')
# wait for one pandaState and one CAN packet
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
# read params
self.is_metric = params.get_bool("IsMetric")
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
community_feature_toggle = params.get_bool("CommunityFeaturesToggle")
openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
passive = params.get_bool("Passive") or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature = self.CP.communityFeature or \
self.CP.fingerprintSource == car.CarParams.FingerprintSource.can
community_feature_disallowed = community_feature and (
not community_feature_toggle)
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
safety_config = car.CarParams.SafetyConfig.new_message()
safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
self.CP.safetyConfigs = [safety_config]
# Write CarParams for radard
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP)
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.initialized = False
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.cruise_mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
self.button_timers = {
ButtonEvent.Type.decelCruise: 0,
ButtonEvent.Type.accelCruise: 0
}
# scc smoother
self.is_cruise_enabled = False
self.applyMaxSpeed = 0
self.apply_accel = 0.
self.fused_accel = 0.
self.lead_drel = 0.
self.aReqValue = 0.
self.aReqValueMin = 0.
self.aReqValueMax = 0.
self.sccStockCamStatus = 0
self.sccStockCamAct = 0
self.left_lane_visible = False
self.right_lane_visible = False
self.wide_camera = TICI and params.get_bool('EnableWideCamera')
self.disable_op_fcw = params.get_bool('DisableOpFcw')
# TODO: no longer necessary, aside from process replay
self.sm['liveParameters'].valid = True
self.startup_event = get_startup_event(car_recognized,
controller_available,
len(self.CP.carFw) > 0)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed and car_recognized and not self.CP.dashcamOnly:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
if len(self.CP.carFw) > 0:
set_offroad_alert("Offroad_CarUnrecognized", True)
else:
set_offroad_alert("Offroad_NoFirmware", True)
elif self.read_only:
self.events.add(EventName.dashcamMode, static=True)
elif self.joystick_mode:
self.events.add(EventName.joystickDebug, static=True)
self.startup_event = None
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
class CarController():
def __init__(self, dbc_name, CP, VM):
self.car_fingerprint = CP.carFingerprint
self.packer = CANPacker(dbc_name)
self.accel_steady = 0
self.apply_steer_last = 0
self.steer_rate_limited = False
self.lkas11_cnt = 0
self.scc12_cnt = 0
self.resume_cnt = 0
self.last_lead_distance = 0
self.resume_wait_timer = 0
self.last_resume_frame = 0
self.lanechange_manual_timer = 0
self.emergency_manual_timer = 0
self.driver_steering_torque_above_timer = 0
self.mode_change_timer = 0
self.params = Params()
self.mode_change_switch = int(self.params.get('CruiseStatemodeSelInit'))
self.opkr_variablecruise = int(self.params.get('OpkrVariableCruise'))
self.opkr_autoresume = int(self.params.get('OpkrAutoResume'))
self.opkr_autoresumeoption = int(self.params.get('OpkrAutoResumeOption'))
self.opkr_maxanglelimit = int(self.params.get('OpkrMaxAngleLimit'))
self.steer_mode = ""
self.mdps_status = ""
self.lkas_switch = ""
self.timer1 = tm.CTime1000("time")
self.SC = Spdctrl()
self.model_speed = 0
self.model_sum = 0
self.dRele = 0
self.yRele = 0
self.vRele = 0
self.cruise_gap = 0.0
self.cruise_gap_prev = 0
self.cruise_gap_set_init = 0
self.cruise_gap_switch_timer = 0
self.lkas_button_on = True
self.longcontrol = CP.openpilotLongitudinalControl
self.scc_live = not CP.radarOffCan
if CP.lateralTuning.which() == 'pid':
self.str_log2 = 'TUNE={:0.2f}/{:0.3f}/{:0.5f}'.format(CP.lateralTuning.pid.kpV[1], CP.lateralTuning.pid.kiV[1], CP.lateralTuning.pid.kf)
elif CP.lateralTuning.which() == 'indi':
self.str_log2 = 'TUNE={:03.1f}/{:03.1f}/{:03.1f}/{:03.1f}'.format(CP.lateralTuning.indi.innerLoopGain, CP.lateralTuning.indi.outerLoopGain, CP.lateralTuning.indi.timeConstant, CP.lateralTuning.indi.actuatorEffectiveness)
elif CP.lateralTuning.which() == 'lqr':
self.str_log2 = 'TUNE={:04.0f}/{:05.3f}/{:06.4f}'.format(CP.lateralTuning.lqr.scale, CP.lateralTuning.lqr.ki, CP.lateralTuning.lqr.dcGain)
if CP.spasEnabled:
self.en_cnt = 0
self.apply_steer_ang = 0.0
self.en_spas = 3
self.mdps11_stat_last = 0
self.spas_always = False
def update(self, enabled, CS, frame, CC, actuators, pcm_cancel_cmd, visual_alert,
left_lane, right_lane, left_lane_depart, right_lane_depart, set_speed, lead_visible, sm):
# *** compute control surfaces ***
# gas and brake
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
self.model_speed, self.model_sum = self.SC.calc_va(sm, CS.out.vEgo)
plan = sm['plan']
self.dRele = plan.ddRel #EON Lead
self.yRele = plan.yyRel #EON Lead
self.vRele = plan.vvRel #EON Lead
# Steering Torque
steerAngleAbs = abs(actuators.steerAngle)
limitParams = copy.copy(SteerLimitParams)
limitParams.STEER_MAX = int(interp(steerAngleAbs, [5., 10., 20., 30., 70.], [255, SteerLimitParams.STEER_MAX, 500, 600, 800]))
limitParams.STEER_DELTA_UP = int(interp(steerAngleAbs, [5., 20., 50.], [2, 3, 3]))
limitParams.STEER_DELTA_DOWN = int(interp(steerAngleAbs, [5., 20., 50.], [4, 5, 5]))
if self.driver_steering_torque_above_timer:
new_steer = actuators.steer * limitParams.STEER_MAX * (self.driver_steering_torque_above_timer / 100)
else:
new_steer = actuators.steer * limitParams.STEER_MAX
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, limitParams)
self.steer_rate_limited = new_steer != apply_steer
CC.applyAccel = apply_accel
CC.applySteer = apply_steer
# SPAS limit angle extremes for safety
if CS.spas_enabled:
apply_steer_ang_req = clip(actuators.steerAngle, -1*(STEER_ANG_MAX), STEER_ANG_MAX)
# SPAS limit angle rate for safety
if abs(self.apply_steer_ang - apply_steer_ang_req) > STEER_ANG_MAX_RATE:
if apply_steer_ang_req > self.apply_steer_ang:
self.apply_steer_ang += STEER_ANG_MAX_RATE
else:
self.apply_steer_ang -= STEER_ANG_MAX_RATE
else:
self.apply_steer_ang = apply_steer_ang_req
spas_active = CS.spas_enabled and enabled and (self.spas_always or CS.out.vEgo < 7.0) # 25km/h
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
if self.opkr_maxanglelimit >= 90:
lkas_active = enabled and abs(CS.out.steeringAngle) < self.opkr_maxanglelimit and not spas_active
else:
lkas_active = enabled and not spas_active
if (( CS.out.leftBlinker and not CS.out.rightBlinker) or ( CS.out.rightBlinker and not CS.out.leftBlinker)) and CS.out.vEgo < LANE_CHANGE_SPEED_MIN:
self.lanechange_manual_timer = 10
if CS.out.leftBlinker and CS.out.rightBlinker:
self.emergency_manual_timer = 10
if abs(CS.out.steeringTorque) > 200:
self.driver_steering_torque_above_timer = 100
if self.lanechange_manual_timer:
lkas_active = 0
if self.lanechange_manual_timer > 0:
self.lanechange_manual_timer -= 1
if self.emergency_manual_timer > 0:
self.emergency_manual_timer -= 1
if self.driver_steering_torque_above_timer > 0:
self.driver_steering_torque_above_timer -= 1
if not lkas_active:
apply_steer = 0
self.apply_accel_last = apply_accel
self.apply_steer_last = apply_steer
sys_warning, sys_state, left_lane_warning, right_lane_warning =\
process_hud_alert(lkas_active, self.car_fingerprint, visual_alert,
left_lane, right_lane, left_lane_depart, right_lane_depart,
self.lkas_button_on)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 55
if clu11_speed > enabled_speed or not lkas_active:
enabled_speed = clu11_speed
if not(min_set_speed < set_speed < 255 * CV.KPH_TO_MS):
set_speed = min_set_speed
set_speed *= CV.MS_TO_MPH if CS.is_set_speed_in_mph else CV.MS_TO_KPH
if frame == 0: # initialize counts from last received count signals
self.lkas11_cnt = CS.lkas11["CF_Lkas_MsgCount"]
self.scc12_cnt = CS.scc12["CR_VSM_Alive"] + 1 if not CS.no_radar else 0
#TODO: fix this
# self.prev_scc_cnt = CS.scc11["AliveCounterACC"]
# self.scc_update_frame = frame
# check if SCC is alive
# if frame % 7 == 0:
# if CS.scc11["AliveCounterACC"] == self.prev_scc_cnt:
# if frame - self.scc_update_frame > 20 and self.scc_live:
# self.scc_live = False
# else:
# self.scc_live = True
# self.prev_scc_cnt = CS.scc11["AliveCounterACC"]
# self.scc_update_frame = frame
self.prev_scc_cnt = CS.scc11["AliveCounterACC"]
self.lkas11_cnt = (self.lkas11_cnt + 1) % 0x10
self.scc12_cnt %= 0xF
can_sends = []
can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active,
CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane,
left_lane_warning, right_lane_warning, 0))
if CS.mdps_bus or CS.scc_bus == 1: # send lkas11 bus 1 if mdps or scc is on bus 1
can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active,
CS.lkas11, sys_warning, sys_state, enabled, left_lane, right_lane,
left_lane_warning, right_lane_warning, 1))
if frame % 2 and CS.mdps_bus: # send clu11 to mdps if it is not on bus 0
can_sends.append(create_clu11(self.packer, frame, CS.mdps_bus, CS.clu11, Buttons.NONE, enabled_speed))
str_log1 = '곡률={:03.0f} 토크={:03.0f} 프레임률={:03.0f} ST={:03.0f}/{:01.0f}/{:01.0f}'.format(abs(self.model_speed), abs(new_steer), self.timer1.sampleTime(), SteerLimitParams.STEER_MAX, SteerLimitParams.STEER_DELTA_UP, SteerLimitParams.STEER_DELTA_DOWN)
trace1.printf1('{} {}'.format(str_log1, self.str_log2))
if CS.out.cruiseState.modeSel == 0 and self.mode_change_switch == 3:
self.mode_change_timer = 50
self.mode_change_switch = 0
elif CS.out.cruiseState.modeSel == 1 and self.mode_change_switch == 0:
self.mode_change_timer = 50
self.mode_change_switch = 1
elif CS.out.cruiseState.modeSel == 2 and self.mode_change_switch == 1:
self.mode_change_timer = 50
self.mode_change_switch = 2
elif CS.out.cruiseState.modeSel == 3 and self.mode_change_switch == 2:
self.mode_change_timer = 50
self.mode_change_switch = 3
if self.mode_change_timer > 0:
self.mode_change_timer -= 1
run_speed_ctrl = self.opkr_variablecruise and CS.acc_active and (CS.out.cruiseState.modeSel == 1 or CS.out.cruiseState.modeSel == 2 or CS.out.cruiseState.modeSel == 3)
if not run_speed_ctrl:
if CS.out.cruiseState.modeSel == 0:
self.steer_mode = "오파모드"
elif CS.out.cruiseState.modeSel == 1:
self.steer_mode = "차간+커브"
elif CS.out.cruiseState.modeSel == 2:
self.steer_mode = "차간ONLY"
elif CS.out.cruiseState.modeSel == 3:
self.steer_mode = "편도1차선"
if CS.out.steerWarning == 0:
self.mdps_status = "정상"
elif CS.out.steerWarning == 1:
self.mdps_status = "오류"
if CS.lkas_button_on == 0:
self.lkas_switch = "OFF"
elif CS.lkas_button_on == 1:
self.lkas_switch = "ON"
else:
self.lkas_switch = "-"
if self.cruise_gap != CS.cruiseGapSet:
self.cruise_gap = CS.cruiseGapSet
str_log3 = '주행모드={:s} MDPS상태={:s} LKAS버튼={:s} 크루즈갭={:1.0f}'.format( self.steer_mode, self.mdps_status, self.lkas_switch, self.cruise_gap)
trace1.printf2( '{}'.format( str_log3 ) )
if pcm_cancel_cmd and self.longcontrol:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.CANCEL, clu11_speed))
if CS.out.cruiseState.standstill:
if self.opkr_autoresumeoption == 1:
if self.last_lead_distance == 0 or not self.opkr_autoresume:
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
self.resume_wait_timer = 0
elif self.resume_wait_timer > 0:
self.resume_wait_timer -= 1
elif CS.lead_distance != self.last_lead_distance:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
if self.resume_cnt > 5:
self.resume_cnt = 0
self.resume_wait_timer = int(0.25 / DT_CTRL)
elif self.cruise_gap_prev == 0 and run_speed_ctrl:
self.cruise_gap_prev = CS.cruiseGapSet
self.cruise_gap_set_init = 1
elif CS.cruiseGapSet != 1.0 and run_speed_ctrl:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 100:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
else:
# run only first time when the car stopped
if self.last_lead_distance == 0 or not self.opkr_autoresume:
# get the lead distance from the Radar
self.last_lead_distance = CS.lead_distance
self.resume_cnt = 0
# when lead car starts moving, create 6 RES msgs
elif CS.lead_distance != self.last_lead_distance and (frame - self.last_resume_frame) > 5:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.RES_ACCEL, clu11_speed))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
self.resume_cnt = 0
elif self.cruise_gap_prev == 0:
self.cruise_gap_prev = CS.cruiseGapSet
self.cruise_gap_set_init = 1
elif CS.cruiseGapSet != 1.0:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 100:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
elif run_speed_ctrl:
is_sc_run = self.SC.update(CS, sm, self)
if is_sc_run:
can_sends.append(create_clu11(self.packer, self.resume_cnt, CS.scc_bus, CS.clu11, self.SC.btn_type, self.SC.sc_clu_speed))
self.resume_cnt += 1
else:
self.resume_cnt = 0
if self.dRele > 18 and self.cruise_gap_prev != CS.cruiseGapSet and self.cruise_gap_set_init == 1:
self.cruise_gap_switch_timer += 1
if self.cruise_gap_switch_timer > 50:
can_sends.append(create_clu11(self.packer, frame, CS.scc_bus, CS.clu11, Buttons.GAP_DIST, clu11_speed))
self.cruise_gap_switch_timer = 0
elif self.cruise_gap_prev == CS.cruiseGapSet:
self.cruise_gap_set_init = 0
self.cruise_gap_prev = 0
if CS.mdps_bus: # send mdps12 to LKAS to prevent LKAS error
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
# send scc to car if longcontrol enabled and SCC not on bus 0 or ont live
if self.longcontrol and (CS.scc_bus or not self.scc_live) and frame % 2 == 0:
can_sends.append(create_scc12(self.packer, apply_accel, enabled, self.scc12_cnt, self.scc_live, CS.scc12))
can_sends.append(create_scc11(self.packer, frame, enabled, set_speed, lead_visible, self.scc_live, CS.scc11))
if CS.has_scc13 and frame % 20 == 0:
can_sends.append(create_scc13(self.packer, CS.scc13))
if CS.has_scc14:
can_sends.append(create_scc14(self.packer, enabled, CS.scc14))
self.scc12_cnt += 1
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in FEATURES["send_lfa_mfa"]:
can_sends.append(create_lfa_mfa(self.packer, frame, lkas_active))
if CS.spas_enabled:
if CS.mdps_bus:
can_sends.append(create_ems11(self.packer, CS.ems11, spas_active))
# SPAS11 50hz
if (frame % 2) == 0:
if CS.mdps11_stat == 7 and not self.mdps11_stat_last == 7:
self.en_spas = 7
self.en_cnt = 0
if self.en_spas == 7 and self.en_cnt >= 8:
self.en_spas = 3
self.en_cnt = 0
if self.en_cnt < 8 and spas_active:
self.en_spas = 4
elif self.en_cnt >= 8 and spas_active:
self.en_spas = 5
if not spas_active:
self.apply_steer_ang = CS.mdps11_strang
self.en_spas = 3
self.en_cnt = 0
self.mdps11_stat_last = CS.mdps11_stat
self.en_cnt += 1
can_sends.append(create_spas11(self.packer, self.car_fingerprint, (frame // 2), self.en_spas, self.apply_steer_ang, CS.mdps_bus))
# SPAS12 20Hz
if (frame % 5) == 0:
can_sends.append(create_spas12(CS.mdps_bus))
return can_sends
def manager_thread():
# now loop
thermal_sock = messaging.sub_sock('thermal')
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
params = Params()
EnableLogger = int(params.get('OpkrEnableLogger'))
#EnableLogger = (params.get("RecordFront") != b"0")
if not EnableLogger:
car_started_processes.remove('loggerd')
persistent_processes.remove('logmessaged')
persistent_processes.remove('uploader')
persistent_processes.remove('logcatd')
persistent_processes.remove('updated')
persistent_processes.remove('deleter')
persistent_processes.remove('tombstoned')
else:
# save boot log
subprocess.call(["./loggerd", "--bootlog"],
cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
# start daemon processes
for p in daemon_processes:
start_daemon_process(p)
# start persistent processes
for p in persistent_processes:
start_managed_process(p)
# start offroad
if ANDROID:
pm_apply_packages('enable')
start_offroad()
if os.getenv("NOBOARD") is None:
start_managed_process("pandad")
if os.getenv("BLOCK") is not None:
for k in os.getenv("BLOCK").split(","):
del managed_processes[k]
logger_dead = False
while 1:
msg = messaging.recv_sock(thermal_sock, wait=True)
# heavyweight batch processes are gated on favorable thermal conditions
if msg.thermal.thermalStatus >= ThermalStatus.yellow:
for p in green_temp_processes:
if p in persistent_processes:
kill_managed_process(p)
else:
for p in green_temp_processes:
if p in persistent_processes:
start_managed_process(p)
if msg.thermal.freeSpace < 0.05:
logger_dead = True
if msg.thermal.started and "driverview" not in running:
for p in car_started_processes:
if p == "loggerd" and logger_dead:
kill_managed_process(p)
else:
start_managed_process(p)
else:
logger_dead = False
for p in reversed(car_started_processes):
kill_managed_process(p)
# this is ugly
if "driverview" not in running and params.get(
"IsDriverViewEnabled") == b"1":
start_managed_process("driverview")
elif "driverview" in running and params.get(
"IsDriverViewEnabled") == b"0":
kill_managed_process("driverview")
# check the status of all processes, did any of them die?
running_list = [
"%s%s\u001b[0m" %
("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p)
for p in running
]
cloudlog.debug(' '.join(running_list))
# Exit main loop when uninstall is needed
if params.get("DoUninstall", encoding='utf8') == "1":
break
def radard_thread(sm=None, pm=None, can_sock=None):
config_realtime_process(5 if TICI else 1 if JETSON else 2,
Priority.CTRL_LOW)
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module(
'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
# *** setup messaging
if can_sock is None:
can_sock = messaging.sub_sock('can')
if sm is None:
sm = messaging.SubMaster(
['modelV2', 'carState'], ignore_avg_freq=[
'modelV2', 'carState'
]) # Can't check average frequency, since radar determines timing
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
RI = RadarInterface(CP)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, RI.delay)
# TODO: always log leads once we can hide them conditionally
enable_lead = True #CP.openpilotLongitudinalControl or not CP.radarOffCan
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr = RI.update(can_strings)
if rr is None:
continue
sm.update(0)
dat = RD.update(sm, rr, enable_lead)
dat.radarState.cumLagMs = -rk.remaining * 1000.
pm.send('radarState', dat)
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
dat = messaging.new_message('liveTracks', len(tracks))
for cnt, ids in enumerate(sorted(tracks.keys())):
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
}
pm.send('liveTracks', dat)
rk.monitor_time()
def __init__(self, sm=None, pm=None, can_sock=None):
config_realtime_process(3, Priority.CTRL_HIGH)
self.op_params = opParams()
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl',
'carEvents', 'carParams'
])
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'
] if SIMULATION else None
self.sm = messaging.SubMaster([
'deviceState', 'pandaState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan',
'liveLocationKalman', 'roadCameraState', 'driverCameraState',
'managerState', 'liveParameters', 'radarState'
],
ignore_alive=ignore)
self.sm_smiskol = messaging.SubMaster([
'radarState', 'dynamicFollowData', 'liveTracks',
'dynamicFollowButton', 'laneSpeed', 'dynamicCameraOffset',
'modelLongButton'
])
self.op_params = opParams()
self.df_manager = dfManager()
self.support_white_panda = self.op_params.get('support_white_panda')
self.last_model_long = False
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT',
False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one pandaState and one CAN packet
panda_type = messaging.recv_one(
self.sm.sock['pandaState']).pandaState.pandaType
has_relay = panda_type in [
PandaType.blackPanda, PandaType.uno, PandaType.dos
]
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
self.CI, self.CP, candidate = get_car(self.can_sock,
self.pm.sock['sendcan'],
has_relay)
threading.Thread(target=log_fingerprint, args=[candidate]).start()
# read params
params = Params()
self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
self.is_ldw_enabled = params.get("IsLdwEnabled",
encoding='utf8') == "1"
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
passive = params.get(
"Passive", encoding='utf8') == "1" or not openpilot_enabled_toggle
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive and not self.CP.dashcamOnly
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.events = Events()
self.LoC = LongControl(self.CP, self.CI.compute_gb, candidate)
self.VM = VehicleModel(self.CP)
if self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.distance_traveled = 0
self.last_functional_fan_frame = 0
self.events_prev = []
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = False
self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
self.sm['deviceState'].freeSpacePercent = 100
self.sm['driverMonitoringState'].events = []
self.sm['driverMonitoringState'].awarenessStatus = 1.
self.sm['driverMonitoringState'].faceDetected = False
self.startup_event = get_startup_event(car_recognized,
controller_available)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
self.events.add(EventName.carUnrecognized, static=True)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
self.lead_rel_speed = 255
self.lead_long_dist = 255
