def uploader_fn(exit_event):
cloudlog.info("uploader_fn")
params = Params()
dongle_id = params.get("DongleId").decode('utf8')
if dongle_id is None:
cloudlog.info("uploader missing dongle_id")
raise Exception("uploader can't start without dongle id")
uploader = Uploader(dongle_id, ROOT)
backoff = 0.1
# dp
last_ts = None
dp_last_modified = None
on_hotspot = False
on_wifi = False
should_upload = False
allow_raw_upload = True
while True:
ts = sec_since_boot()
if last_ts is None or ts - last_ts >= 5.:
modified = get_last_modified()
if dp_last_modified != modified:
on_hotspot = False if (
params.get("DragonEnableUploadOnHotspot")
== b"1") else is_on_hotspot()
on_wifi = False if (params.get("DragonEnableUploadOnMobile")
== b"1") else is_on_wifi()
allow_raw_upload = (params.get("IsUploadRawEnabled") != b"0")
should_upload = on_wifi and not on_hotspot
dp_last_modified = modified
last_ts = ts
if exit_event.is_set():
return
d = uploader.next_file_to_upload(
with_raw=allow_raw_upload and should_upload)
if d is None:
time.sleep(5)
continue
key, fn = d
cloudlog.event("uploader_netcheck",
is_on_hotspot=on_hotspot,
is_on_wifi=on_wifi)
cloudlog.info("to upload %r", d)
success = uploader.upload(key, fn)
if success:
backoff = 0.1
else:
cloudlog.info("backoff %r", backoff)
time.sleep(backoff + random.uniform(0, backoff))
backoff = min(backoff * 2, 120)
cloudlog.info("upload done, success=%r", success)
def dp_load_params(self, car_name):
# dp
ts = sec_since_boot()
if ts - self.ts_last_check >= 5.:
modified = get_last_modified()
if self.dp_last_modified != modified:
self.dragon_lat_ctrl = False if params.get(
"DragonLatCtrl", encoding='utf8') == "0" else True
if self.dragon_lat_ctrl:
self.dragon_enable_steering_on_signal = True if (
params.get("DragonEnableSteeringOnSignal",
encoding='utf8') == "1"
and params.get("LaneChangeEnabled",
encoding='utf8') == "0") else False
self.dragon_toyota_stock_dsu = True if (
car_name == 'toyota'
and params.get("DragonToyotaStockDSU",
encoding='utf8') == "1") else False
if not self.dragon_toyota_stock_dsu:
self.dragon_allow_gas = True if params.get(
"DragonAllowGas", encoding='utf8') == "1" else False
self.dp_gear_check = False if params.get(
"DragonEnableGearCheck", encoding='utf8') == "0" else True
self.dp_last_modified = modified
self.ts_last_check = ts
def get_TR(self, CS):
# load profile
ts = sec_since_boot()
if ts - self.last_ts >= 5.:
modified = get_last_modified()
if self.last_modified != modified:
try:
self.df_profile = int(self.params.get('DragonDynamicFollow', encoding='utf8').rstrip('\x00'))
self.df_profile = self.df_profile if self.df_profile in [DF_PROFILE_OFF, DF_PROFILE_LONG, DF_PROFILE_NORMAL, DF_PROFILE_SHORT] else DF_PROFILE_OFF
except TypeError:
self.df_profile = DF_PROFILE_OFF
put_nonblocking('DragonDynamicFollow', DF_PROFILE_OFF)
self.last_modified = modified
self.last_ts = ts
if not self.lead_data['status'] or self.df_profile == DF_PROFILE_OFF:
TR = 1.8
else:
self.store_df_data()
TR = self.dynamic_follow(CS)
# only change cost when profile is not off
if not self.df_profile == DF_PROFILE_OFF:
self.change_cost(TR)
return TR
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line):
""" Controls thread """
# dp
if frame % 500 == 0:
modified = get_last_modified()
if self.dp_last_modified != modified:
self.dragon_lat_ctrl, \
self.lane_change_enabled, \
self.dragon_enable_steering_on_signal = common_controller_update(self.lane_change_enabled)
self.dp_last_modified = modified
P = self.params
# Send CAN commands.
can_sends = []
### STEER ###
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = int(round(final_steer * P.STEER_MAX))
# limits due to driver torque
new_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
lkas_enabled = enabled
if not lkas_enabled:
apply_steer = 0
# dp
apply_steer = common_controller_ctrl(enabled,
self.dragon_lat_ctrl,
self.dragon_enable_steering_on_signal,
CS.out.leftBlinker,
CS.out.rightBlinker,
apply_steer)
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, P.STEER_STEP))
self.apply_steer_last = apply_steer
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
return can_sends
def _get_profiles(self):
# dp
# update() gets call every time so we can read profile from param here
# as usual, we update every 5 secs
ts = sec_since_boot()
if self.last_ts is None or ts - self.last_ts >= 5.:
modified = get_last_modified()
if self.dp_last_modified != modified:
try:
self.dp_df_profile = int(
self.params.get("DragonDynamicFollow",
encoding='utf8'))
if self.dp_df_profile > 4 or self.dp_df_profile < 0:
self.dp_df_profile = 0
except (TypeError, ValueError):
self.dp_df_profile = PROFILE_OFF
self.dp_last_modified = modified
self.last_ts = ts
if self.dp_df_profile == PROFILE_AUTO:
self._get_pred(
) # sets self.model_profile, all other checks are inside function
def main():
thermal_sock = messaging.sub_sock('thermal')
secs = 0
last_secs = None
shutdown_at = 0
started = False
usb_online = False
enabled = False
last_enabled = None
dp_last_modified = None
while 1:
cur_time = sec_since_boot()
modified = get_last_modified()
if dp_last_modified != modified:
enabled = True if params.get("DragonEnableAutoShutdown", encoding='utf8') == '1' else False
if enabled:
try:
secs = int(params.get("DragonAutoShutdownAt", encoding='utf8')) * 60
except (TypeError, ValueError):
secs = 0
dp_last_modified = modified
if last_enabled != enabled or last_secs != secs or started or usb_online:
shutdown_at = cur_time + secs
if enabled:
msg = messaging.recv_sock(thermal_sock, wait=True)
started = msg.thermal.started
usb_online = msg.thermal.usbOnline
if not started and not usb_online and secs > 0 and cur_time >= shutdown_at:
os.system('LD_LIBRARY_PATH="" svc power shutdown')
last_enabled = enabled
last_secs = secs
time.sleep(10)
def update_d_poly(self, v_ego):
ts = sec_since_boot()
if ts - self.last_ts >= 5.:
modified = get_last_modified()
if self.last_modified != modified:
try:
self.camera_offset = int(params.get("DragonCameraOffset", encoding='utf8')) * 0.01
except (TypeError, ValueError):
self.camera_offset = CAMERA_OFFSET
self.last_modified = modified
self.last_ts = ts
# only offset left and right lane lines; offsetting p_poly does not make sense
self.l_poly[3] += self.camera_offset
self.r_poly[3] += self.camera_offset
# Find current lanewidth
self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob - self.lane_width_certainty)
current_lane_width = abs(self.l_poly[3] - self.r_poly[3])
self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
(1 - self.lane_width_certainty) * speed_lane_width
self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width, v_ego)
def thermald_thread():
# prevent LEECO from undervoltage
BATT_PERC_OFF = 10 if LEON else 3
health_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected health frequency
# now loop
thermal_sock = messaging.pub_sock('thermal')
health_sock = messaging.sub_sock('health', timeout=health_timeout)
location_sock = messaging.sub_sock('gpsLocation')
ignition = False
fan_speed = 0
count = 0
off_ts = None
started_ts = None
started_seen = False
thermal_status = ThermalStatus.green
thermal_status_prev = ThermalStatus.green
usb_power = True
usb_power_prev = True
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
fw_version_match_prev = True
current_connectivity_alert = None
time_valid_prev = True
should_start_prev = False
handle_fan = None
is_uno = False
pm = PowerMonitoring()
no_panda_cnt = 0
# dp
ts_last_ip = 0
ts_last_update_vars = 0
ts_last_charging_ctrl = None
dp_last_modified = None
ip_addr = '255.255.255.255'
dragon_charging_ctrl = False
dragon_charging_ctrl_prev = False
dragon_to_discharge = 70
dragon_to_charge = 60
dp_temp_monitor = True
while 1:
# dp
ts = sec_since_boot()
# update variable status every 10 secs
if ts - ts_last_update_vars >= 10.:
modified = get_last_modified()
if dp_last_modified != modified:
dp_temp_monitor = False if params.get('DragonEnableTempMonitor', encoding='utf8') == "0" else True
if not is_uno:
dragon_charging_ctrl = True if params.get('DragonChargingCtrl', encoding='utf8') == "1" else False
if dragon_charging_ctrl:
try:
dragon_to_discharge = int(params.get('DragonCharging', encoding='utf8'))
except (TypeError, ValueError):
dragon_to_discharge = 70
try:
dragon_to_charge = int(params.get('DragonDisCharging', encoding='utf8'))
except (TypeError, ValueError):
dragon_to_charge = 60
else:
dragon_charging_ctrl = False
dp_last_modified = modified
ts_last_update_vars = ts
health = messaging.recv_sock(health_sock, wait=True)
location = messaging.recv_sock(location_sock)
location = location.gpsLocation if location else None
msg = read_thermal()
if handle_fan is None and params.get('DragonNoctuaMode', encoding='utf8') == "1":
setup_eon_fan()
handle_fan = handle_fan_eon
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 ignition:
cloudlog.error("Lost panda connection while onroad")
ignition = False
else:
no_panda_cnt = 0
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 is_uno or not ANDROID:
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 = get_network_type()
network_strength = 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 = get_battery_capacity()
msg.thermal.batteryStatus = get_battery_status()
msg.thermal.batteryCurrent = get_battery_current()
msg.thermal.batteryVoltage = get_battery_voltage()
msg.thermal.usbOnline = get_usb_present()
# Fake battery levels on uno for frame
if is_uno:
msg.thermal.batteryPercent = 100
msg.thermal.batteryStatus = "Charging"
# dp
# update ip every 10 seconds
ts = sec_since_boot()
if ts - ts_last_ip >= 10.:
try:
result = subprocess.check_output(["ifconfig", "wlan0"], encoding='utf8') # pylint: disable=unexpected-keyword-arg
ip_addr = re.findall(r"inet addr:((\d+\.){3}\d+)", result)[0][0]
except:
ip_addr = 'N/A'
ts_last_ip = ts
msg.thermal.ipAddr = ip_addr
current_filter.update(msg.thermal.batteryCurrent / 1e6)
# TODO: add car battery voltage check
max_cpu_temp = cpu_temp_filter.update(
max(msg.thermal.cpu0,
msg.thermal.cpu1,
msg.thermal.cpu2,
msg.thermal.cpu3) / 10.0)
max_comp_temp = max(max_cpu_temp, msg.thermal.mem / 10., msg.thermal.gpu / 10.)
bat_temp = msg.thermal.bat / 1000.
if handle_fan is not None:
fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, ignition)
msg.thermal.fanSpeed = fan_speed
# thermal logic with hysterisis
if max_cpu_temp > 107. or bat_temp >= 63.:
# onroad not allowed
thermal_status = ThermalStatus.danger
elif max_comp_temp > 92.5 or bat_temp > 60.: # CPU throttling starts around ~90C
# hysteresis between onroad not allowed and engage not allowed
thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger)
elif max_cpu_temp > 87.5:
# 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:
# all good
thermal_status = ThermalStatus.green
if not dp_temp_monitor and thermal_status in [ThermalStatus.yellow, ThermalStatus.red, ThermalStatus.danger]:
thermal_status = ThermalStatus.yellow
# **** starting logic ****
time_valid = True
# Check for last update time and display alerts if needed
# now = datetime.datetime.utcnow()
#
# # show invalid date/time alert
# time_valid = now.year >= 2019
# if time_valid and not time_valid_prev:
# params.delete("Offroad_InvalidTime")
# if not time_valid and time_valid_prev:
# put_nonblocking("Offroad_InvalidTime", json.dumps(OFFROAD_ALERTS["Offroad_InvalidTime"]))
# time_valid_prev = 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)
#
# if dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
# if current_connectivity_alert != "expired":
# current_connectivity_alert = "expired"
# params.delete("Offroad_ConnectivityNeededPrompt")
# put_nonblocking("Offroad_ConnectivityNeeded", json.dumps(OFFROAD_ALERTS["Offroad_ConnectivityNeeded"]))
# elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
# remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
# if current_connectivity_alert != "prompt" + remaining_time:
# current_connectivity_alert = "prompt" + remaining_time
# alert_connectivity_prompt = copy.copy(OFFROAD_ALERTS["Offroad_ConnectivityNeededPrompt"])
# alert_connectivity_prompt["text"] += remaining_time + " days."
# params.delete("Offroad_ConnectivityNeeded")
# put_nonblocking("Offroad_ConnectivityNeededPrompt", json.dumps(alert_connectivity_prompt))
# elif current_connectivity_alert is not None:
# current_connectivity_alert = None
# params.delete("Offroad_ConnectivityNeeded")
# params.delete("Offroad_ConnectivityNeededPrompt")
do_uninstall = params.get("DoUninstall") == b"1"
accepted_terms = params.get("HasAcceptedTerms") == terms_version
completed_training = params.get("CompletedTrainingVersion") == training_version
panda_signature = params.get("PandaFirmware")
fw_version_match = (panda_signature is None) or (panda_signature == FW_SIGNATURE) # don't show alert is no panda is connected (None)
should_start = ignition
# with 2% left, we killall, otherwise the phone will take a long time to boot
should_start = should_start and msg.thermal.freeSpace > 0.02
# confirm we have completed training and aren't uninstalling
should_start = should_start and accepted_terms and completed_training and (not do_uninstall)
# check for firmware mismatch
should_start = should_start and fw_version_match
# check if system time is valid
should_start = should_start and time_valid
# don't start while taking snapshot
if not should_start_prev:
is_viewing_driver = params.get("IsDriverViewEnabled") == b"1"
is_taking_snapshot = params.get("IsTakingSnapshot") == b"1"
should_start = should_start and (not is_taking_snapshot) and (not is_viewing_driver)
if fw_version_match and not fw_version_match_prev:
params.delete("Offroad_PandaFirmwareMismatch")
if not fw_version_match and fw_version_match_prev:
put_nonblocking("Offroad_PandaFirmwareMismatch", json.dumps(OFFROAD_ALERTS["Offroad_PandaFirmwareMismatch"]))
# 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
if thermal_status >= ThermalStatus.danger:
should_start = False
if thermal_status_prev < ThermalStatus.danger:
put_nonblocking("Offroad_TemperatureTooHigh", json.dumps(OFFROAD_ALERTS["Offroad_TemperatureTooHigh"]))
else:
if thermal_status_prev >= ThermalStatus.danger:
params.delete("Offroad_TemperatureTooHigh")
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
os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor')
else:
if should_start_prev or (count == 0):
put_nonblocking("IsOffroad", "1")
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
os.system('echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor')
# shutdown if the battery gets lower than 3%, it's discharging, we aren't running for
# more than a minute but we were running
if msg.thermal.batteryPercent < BATT_PERC_OFF and msg.thermal.batteryStatus == "Discharging" and \
started_seen and (sec_since_boot() - off_ts) > 60:
os.system('LD_LIBRARY_PATH="" svc power shutdown')
# Offroad power monitoring
pm.calculate(health)
msg.thermal.offroadPowerUsage = pm.get_power_used()
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
thermal_sock.send(msg.to_bytes())
if usb_power_prev and not usb_power:
put_nonblocking("Offroad_ChargeDisabled", json.dumps(OFFROAD_ALERTS["Offroad_ChargeDisabled"]))
elif usb_power and not usb_power_prev:
params.delete("Offroad_ChargeDisabled")
thermal_status_prev = thermal_status
usb_power_prev = usb_power
fw_version_match_prev = fw_version_match
should_start_prev = should_start
if dragon_charging_ctrl != dragon_charging_ctrl_prev:
set_battery_charging(True)
if dragon_charging_ctrl:
if ts_last_charging_ctrl is None or ts - ts_last_charging_ctrl >= 60.:
if msg.thermal.batteryPercent >= dragon_to_discharge and get_battery_charging():
set_battery_charging(False)
elif msg.thermal.batteryPercent <= dragon_to_charge and not get_battery_charging():
set_battery_charging(True)
ts_last_charging_ctrl = ts
dragon_charging_ctrl_prev = dragon_charging_ctrl
# report to server once per minute
if (count % int(60. / DT_TRML)) == 0:
cloudlog.event("STATUS_PACKET",
count=count,
health=(health.to_dict() if health else None),
location=(location.to_dict() if location else None),
thermal=msg.to_dict())
count += 1
def update(self, cp, cp_cam):
# dp
ts = sec_since_boot()
if ts - self.ts_last_check >= 5.:
modified = get_last_modified()
if self.last_modifed != modified:
self.dragon_toyota_stock_dsu = True if params.get(
"DragonToyotaStockDSU", encoding='utf8') == "1" else False
self.last_modifed = modified
self.ts_last_check = ts
ret = car.CarState.new_message()
ret.doorOpen = any([
cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'],
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']
])
ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"][
'SEATBELT_DRIVER_UNLATCHED'] != 0
ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0
ret.brakeLights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC']
or ret.brakePressed)
if self.CP.enableGasInterceptor:
ret.gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] +
cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
ret.gasPressed = ret.gas > 15
elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
ret.gas = cp.vl["GAS_PEDAL_ALT"]['GAS_PEDAL']
ret.gasPressed = ret.gas > 1e-5
else:
ret.gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
ret.gasPressed = cp.vl["PCM_CRUISE"]['GAS_RELEASED'] == 0
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_FL'] * CV.KPH_TO_MS
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_FR'] * CV.KPH_TO_MS
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_RL'] * CV.KPH_TO_MS
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"][
'WHEEL_SPEED_RR'] * CV.KPH_TO_MS
ret.vEgoRaw = mean([
ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl,
ret.wheelSpeeds.rr
])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.001
# Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:
self.accurate_steer_angle_seen = True
if self.accurate_steer_angle_seen:
ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"][
'STEER_ANGLE'] - self.angle_offset
if self.needs_angle_offset:
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"][
'STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"][
'STEER_FRACTION']
if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
self.needs_angle_offset = False
self.angle_offset = ret.steeringAngle - angle_wheel
else:
ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"][
'STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
ret.gearShifter = self.parse_gear_shifter(
self.shifter_values.get(can_gear, None))
ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"][
'STEER_TORQUE_DRIVER']
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"][
'STEER_TORQUE_EPS']
# we could use the override bit from dbc, but it's triggered at too high torque values
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
ret.steerWarning = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
if self.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_NXT]:
ret.cruiseState.available = cp.vl["DSU_CRUISE"]['MAIN_ON'] != 0
ret.cruiseState.speed = cp.vl["DSU_CRUISE"][
'SET_SPEED'] * CV.KPH_TO_MS
self.low_speed_lockout = False
elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
ret.cruiseState.available = cp.vl["PCM_CRUISE_ALT"]['MAIN_ON'] != 0
ret.cruiseState.speed = cp.vl["PCM_CRUISE_ALT"][
'SET_SPEED'] * CV.KPH_TO_MS
self.low_speed_lockout = False
else:
ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]['MAIN_ON'] != 0
ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"][
'SET_SPEED'] * CV.KPH_TO_MS
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"][
'LOW_SPEED_LOCKOUT'] == 2
if self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
# Lexus ISH does not have CRUISE_STATUS value (always 0), so we use CRUISE_ACTIVE value instead
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']
else:
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
# ignore standstill in hybrid vehicles, since pcm allows to restart without
# receiving any special command. Also if interceptor is detected
ret.cruiseState.standstill = False
else:
ret.cruiseState.standstill = self.pcm_acc_status == 7
ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
if self.CP.carFingerprint == CAR.PRIUS:
ret.genericToggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
ret.genericToggle = bool(
cp.vl["LIGHT_STALK_ISH"]['AUTO_HIGH_BEAM'])
else:
ret.genericToggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"]
and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
ret.espDisabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] != 0
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
if self.CP.carFingerprint in TSS2_CAR:
ret.leftBlindspot = cp.vl["BSM"]['L_ADJACENT'] == 1
ret.rightBlindspot = cp.vl["BSM"]['R_ADJACENT'] == 1
# dp
if self.dragon_toyota_stock_dsu and ret.cruiseState.available:
enable_acc = True
if ret.gearShifter in [
car.CarState.GearShifter.reverse,
car.CarState.GearShifter.park
]:
enable_acc = False
if ret.seatbeltUnlatched or ret.doorOpen:
enable_acc = False
ret.cruiseState.enabled = enable_acc
return ret
def dmonitoringd_thread(sm=None, pm=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
# Pub/Sub Sockets
if pm is None:
pm = messaging.PubMaster(['dMonitoringState'])
if sm is None:
sm = messaging.SubMaster(
['driverState', 'liveCalibration', 'carState', 'model'])
driver_status = DriverStatus()
is_rhd = params.get("IsRHD")
if is_rhd is not None:
driver_status.is_rhd_region = bool(int(is_rhd))
driver_status.is_rhd_region_checked = True
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['carState'].vEgo = 0.
sm['carState'].cruiseState.enabled = False
sm['carState'].cruiseState.speed = 0.
sm['carState'].buttonEvents = []
sm['carState'].steeringPressed = False
sm['carState'].standstill = True
cal_rpy = [0, 0, 0]
v_cruise_last = 0
driver_engaged = False
# dragonpilot
last_ts = 0
dp_last_modified = None
dp_enable_driver_monitoring = True
# 10Hz <- dmonitoringmodeld
while True:
cur_time = sec_since_boot()
if cur_time - last_ts >= 5.:
modified = get_last_modified()
if dp_last_modified != modified:
dp_enable_driver_monitoring = False if params.get(
"DragonEnableDriverMonitoring",
encoding='utf8') == "0" else True
try:
dp_awareness_time = int(
params.get("DragonSteeringMonitorTimer",
encoding='utf8'))
except (TypeError, ValueError):
dp_awareness_time = 70.
driver_status.awareness_time = 86400 if dp_awareness_time <= 0. else dp_awareness_time * 60.
dp_last_modified = modified
last_ts = cur_time
# reset all awareness val and set to rhd region, this will enforce steering monitor.
if not dp_enable_driver_monitoring:
driver_status.active_monitoring_mode = False
driver_status.awareness = 1.
driver_status.awareness_active = 1.
driver_status.awareness_passive = 1.
driver_status.terminal_alert_cnt = 0
driver_status.terminal_time = 0
driver_status.face_detected = False
driver_status.hi_stds = 0
sm.update()
# Handle calibration
if sm.updated['liveCalibration']:
if sm['liveCalibration'].calStatus == Calibration.CALIBRATED:
if len(sm['liveCalibration'].rpyCalib) == 3:
cal_rpy = sm['liveCalibration'].rpyCalib
# Get interaction
if sm.updated['carState']:
v_cruise = sm['carState'].cruiseState.speed
driver_engaged = len(sm['carState'].buttonEvents) > 0 or \
v_cruise != v_cruise_last or \
sm['carState'].steeringPressed
if driver_engaged:
_ = driver_status.update([], True,
sm['carState'].cruiseState.enabled,
sm['carState'].standstill)
v_cruise_last = v_cruise
# Get model meta
if sm.updated['model']:
driver_status.set_policy(sm['model'])
# Get data from dmonitoringmodeld
if sm.updated['driverState']:
events = []
driver_status.get_pose(sm['driverState'], cal_rpy,
sm['carState'].vEgo,
sm['carState'].cruiseState.enabled)
# Block any engage after certain distrations
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
# Update events from driver state
events = driver_status.update(events, driver_engaged,
sm['carState'].cruiseState.enabled,
sm['carState'].standstill)
# dMonitoringState packet
dat = messaging.new_message('dMonitoringState')
dat.dMonitoringState = {
"events":
events,
"faceDetected":
driver_status.face_detected,
"isDistracted":
driver_status.driver_distracted,
"awarenessStatus":
driver_status.awareness,
"isRHD":
driver_status.is_rhd_region,
"rhdChecked":
driver_status.is_rhd_region_checked,
"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":
False,
}
pm.send('dMonitoringState', dat)
def main(gctx=None):
retry = 0
folder_exists = False
dashcam_allowed = True
# make sure dashcam folder exists
while not folder_exists:
try:
if not os.path.exists(dashcam_videos_path):
os.makedirs(dashcam_videos_path)
else:
folder_exists = True
break
except OSError:
pass
if retry >= 5:
folder_exists = True
dashcam_allowed = False
retry += 1
time.sleep(5)
health_timeout = int(1000 * 2.5 *
DT_TRML) # 2.5x the expected health frequency
health_sock = messaging.sub_sock('health', timeout=health_timeout)
dragon_dashcam_hours = 24. * 60 * 60
max_storage = (max_size_per_file / dashcam_duration) * dragon_dashcam_hours
dashcam_enabled = False
last_ts = 0.
last_modified = None
thermal_sock = messaging.sub_sock('thermal')
while dashcam_allowed:
duration = dashcam_duration
ts = sec_since_boot()
if ts - last_ts >= 5.:
modified = get_last_modified()
if last_modified != modified:
dashcam_enabled = True if params.get(
"DragonEnableDashcam", encoding='utf8') == "1" else False
try:
dragon_dashcam_hours = float(
params.get("DragonDashcamHours",
encoding='utf8')) * 60 * 60
except (TypeError, ValueError):
dragon_dashcam_hours = 24. * 60 * 60
max_storage = (max_size_per_file /
dashcam_duration) * dragon_dashcam_hours
last_modified = modified
last_ts = ts
health = messaging.recv_sock(health_sock, wait=False)
started = True if health is not None and (
health.health.ignitionLine or health.health.ignitionCan) else False
if started and dashcam_enabled:
now = datetime.datetime.now()
file_name = now.strftime("%Y-%m-%d_%H-%M-%S")
os.system("screenrecord --bit-rate %s --time-limit %s %s%s.mp4 &" %
(bit_rates, duration, dashcam_videos_path, file_name))
start_time = time.time()
try:
used_spaces = get_used_spaces()
last_used_spaces = used_spaces
# we should clean up files here if use too much spaces
# when used spaces greater than max available storage
# or when free space is less than 10%
# get health of board, log this in "thermal"
msg = messaging.recv_sock(thermal_sock, wait=True)
if used_spaces >= max_storage or (
msg is not None
and msg.thermal.freeSpace < freespace_limit):
# get all the files in the dashcam_videos_path path
files = [
f for f in sorted(os.listdir(dashcam_videos_path))
if os.path.isfile(dashcam_videos_path + f)
]
for file in files:
msg = messaging.recv_sock(thermal_sock, wait=True)
# delete file one by one and once it has enough space for 1 video, we stop deleting
if used_spaces - last_used_spaces < max_size_per_file or msg.thermal.freeSpace < freespace_limit:
system("rm -fr %s" % (dashcam_videos_path + file))
last_used_spaces = get_used_spaces()
else:
break
except os.error as e:
pass
time_diff = time.time() - start_time
# we start the process 1 second before screenrecord ended
# to make sure there are no missing footage
sleep_time = duration - 1 - time_diff
if sleep_time >= 0.:
time.sleep(sleep_time)
else:
time.sleep(5)
def main():
apps = []
# enable hotspot on boot
if params.get("DragonBootHotspot", encoding='utf8') == "1":
system(f"service call wifi 37 i32 0 i32 1")
# system(f"settings put system accelerometer_rotation 0")
# system(f"settings put system user_rotation 1")
# system(f"pm enable com.android.settings")
# system(f"am start -n com.android.settings/.TetherSettings")
# time.sleep(1)
# system(f"LD_LIBRARY_PATH= input tap 995 160")
# system(f"pkill com.android.settings")
last_started = False
thermal_sock = messaging.sub_sock('thermal')
frame = 0
start_delay = None
stop_delay = None
allow_auto_run = True
last_thermal_status = None
thermal_status = None
start_ts = sec_since_boot()
init_done = False
last_modified = None
while 1: #has_enabled_apps:
if not init_done:
if sec_since_boot() - start_ts >= 10:
init_apps(apps)
init_done = True
else:
enabled_apps = []
has_fullscreen_apps = False
modified = get_last_modified()
for app in apps:
# read params loop
if last_modified != modified:
app.read_params()
if app.last_is_enabled and not app.is_enabled and app.is_running:
app.kill(True)
if app.is_enabled:
if not has_fullscreen_apps and app.app_type == App.TYPE_FULLSCREEN:
has_fullscreen_apps = True
# process manual ctrl apps
if app.manual_ctrl_status != App.MANUAL_IDLE:
app.run(
True
) if app.manual_ctrl_status == App.MANUAL_ON else app.kill(
True)
enabled_apps.append(app)
last_modified = modified
msg = messaging.recv_sock(thermal_sock, wait=True)
started = msg.thermal.started
# when car is running
if started:
stop_delay = None
# apps start 5 secs later
if start_delay is None:
start_delay = frame + 5
thermal_status = msg.thermal.thermalStatus
if thermal_status <= ThermalStatus.yellow:
allow_auto_run = True
# when temp reduce from red to yellow, we add start up delay as well
# so apps will not start up immediately
if last_thermal_status == ThermalStatus.red:
start_delay = frame + 60
elif thermal_status >= ThermalStatus.red:
allow_auto_run = False
last_thermal_status = thermal_status
# we run service apps and kill all util apps
# only run once
if last_started != started:
for app in enabled_apps:
if app.app_type in [
App.TYPE_SERVICE, App.TYPE_GPS_SERVICE
]:
app.run()
elif app.app_type == App.TYPE_UTIL:
app.kill()
# only run apps that's not manually ctrled
for app in enabled_apps:
if not app.manually_ctrled:
if has_fullscreen_apps:
if app.app_type == App.TYPE_FULLSCREEN:
app.run()
elif app.app_type in [App.TYPE_GPS, App.TYPE_UTIL]:
app.kill()
else:
if not allow_auto_run:
app.kill()
else:
if frame >= start_delay and app.is_auto_runnable and app.app_type == App.TYPE_GPS:
app.run()
# when car is stopped
else:
start_delay = None
# set delay to 30 seconds
if stop_delay is None:
stop_delay = frame + 30
for app in enabled_apps:
if app.is_running and not app.manually_ctrled:
if has_fullscreen_apps or frame >= stop_delay:
app.kill()
if last_started != started:
for app in enabled_apps:
app.manually_ctrled = False
last_started = started
frame += 3
time.sleep(3)
def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert):
# this seems needed to avoid steering faults and to force the sync with the EPS counter
frame = CS.lkas_counter
if self.prev_frame == frame:
return []
# dp
if frame % 500 == 0:
modified = get_last_modified()
if self.dp_last_modified != modified:
self.dragon_lat_ctrl, \
self.dragon_enable_steering_on_signal, \
self.dragon_blinker_off_timer = common_controller_update()
self.dp_last_modified = modified
# *** compute control surfaces ***
# steer torque
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
apply_steer = apply_toyota_steer_torque_limits(
new_steer, self.apply_steer_last, CS.out.steeringTorqueEps,
SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit
self.gone_fast_yet = True
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID,
CAR.JEEP_CHEROKEE_2019):
if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
lkas_active = moving_fast and enabled
if not lkas_active:
apply_steer = 0
# dp
blinker_on = CS.out.leftBlinker or CS.out.rightBlinker
if not enabled:
self.blinker_end_frame = 0
if self.last_blinker_on and not blinker_on:
self.blinker_end_frame = frame + self.dragon_blinker_off_timer
apply_steer = common_controller_ctrl(
enabled, self.dragon_lat_ctrl,
self.dragon_enable_steering_on_signal, blinker_on
or frame < self.blinker_end_frame, apply_steer)
self.last_blinker_on = blinker_on
self.apply_steer_last = apply_steer
can_sends = []
#*** control msgs ***
if pcm_cancel_cmd:
# TODO: would be better to start from frame_2b3
new_msg = create_wheel_buttons(self.packer,
self.ccframe,
cancel=True)
can_sends.append(new_msg)
# LKAS_HEARTBIT is forwarded by Panda so no need to send it here.
# frame is 100Hz (0.01s period)
if (self.ccframe % 25 == 0): # 0.25s period
if (CS.lkas_car_model != -1):
new_msg = create_lkas_hud(self.packer, CS.out.gearShifter,
lkas_active, hud_alert,
self.hud_count, CS.lkas_car_model)
can_sends.append(new_msg)
self.hud_count += 1
new_msg = create_lkas_command(self.packer, int(apply_steer),
self.gone_fast_yet, frame)
can_sends.append(new_msg)
self.ccframe += 1
self.prev_frame = frame
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_lane, right_lane, left_lane_depart,
right_lane_depart):
# dp
if frame % 500 == 0:
modified = get_last_modified()
if self.dp_last_modified != modified:
self.dragon_lat_ctrl, \
self.dragon_enable_steering_on_signal, \
self.dragon_blinker_off_timer = common_controller_update()
self.dp_last_modified = modified
# 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) < 90.
# fix for Genesis hard fault at low speed
if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS:
lkas_active = 0
if not lkas_active:
apply_steer = 0
self.apply_steer_last = apply_steer
# dp
blinker_on = CS.out.leftBlinker or CS.out.rightBlinker
if not enabled:
self.blinker_end_frame = 0
if self.last_blinker_on and not blinker_on:
self.blinker_end_frame = frame + self.dragon_blinker_off_timer
lkas_active = common_controller_ctrl(
enabled, self.dragon_lat_ctrl,
self.dragon_enable_steering_on_signal, blinker_on
or frame < self.blinker_end_frame, lkas_active)
self.last_blinker_on = blinker_on
sys_warning, sys_state, left_lane_warning, right_lane_warning =\
process_hud_alert(enabled, self.car_fingerprint, visual_alert,
left_lane, right_lane, left_lane_depart, right_lane_depart)
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))
if pcm_cancel_cmd:
can_sends.append(
create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
elif CS.out.cruiseState.standstill:
# run only first time when the car stopped
if self.last_lead_distance == 0:
# 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.clu11,
Buttons.RES_ACCEL))
self.resume_cnt += 1
# interval after 6 msgs
if self.resume_cnt > 5:
self.last_resume_frame = frame
self.clu11_cnt = 0
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [
CAR.SONATA, CAR.PALISADE
]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
return can_sends
def update(self, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
# dp
if frame % 500 == 0:
modified = get_last_modified()
if self.dp_last_modified != modified:
self.dragon_lat_ctrl, \
self.dragon_enable_steering_on_signal, \
self.dragon_blinker_off_timer = common_controller_update()
self.dp_last_modified = modified
P = self.params
# *** apply brake hysteresis ***
brake, self.braking, self.brake_steady = actuator_hystereses(
actuators.brake, self.braking, self.brake_steady, CS.out.vEgo,
CS.CP.carFingerprint)
# *** no output if not enabled ***
if not enabled and CS.out.cruiseState.enabled:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = True
# *** rate limit after the enable check ***
self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL)
# vehicle hud display, wait for one update from 10Hz 0x304 msg
if hud_show_lanes and CS.lkMode:
hud_lanes = 1
else:
hud_lanes = 0
if enabled:
if hud_show_car:
hud_car = 2
else:
hud_car = 1
else:
hud_car = 0
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required,
CS.lkMode)
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(
clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))
apply_steer = int(
interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP,
P.STEER_LOOKUP_V))
lkas_active = enabled and not CS.steer_not_allowed and CS.lkMode
# Send CAN commands.
can_sends = []
# dp
blinker_on = CS.out.leftBlinker or CS.out.rightBlinker
if not enabled:
self.blinker_end_frame = 0
if self.last_blinker_on and not blinker_on:
self.blinker_end_frame = frame + self.dragon_blinker_off_timer
lkas_active = common_controller_ctrl(
enabled, self.dragon_lat_ctrl,
self.dragon_enable_steering_on_signal, blinker_on
or frame < self.blinker_end_frame, lkas_active)
self.last_blinker_on = blinker_on
# Send steering command.
idx = frame % 4
can_sends.append(
hondacan.create_steering_control(self.packer, apply_steer,
lkas_active, CS.CP.carFingerprint,
idx, CS.CP.isPandaBlack))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame // 10) % 4
can_sends.extend(
hondacan.create_ui_commands(self.packer, pcm_speed, hud,
CS.CP.carFingerprint, CS.is_metric,
idx, CS.CP.isPandaBlack,
CS.stock_hud))
if CS.CP.radarOffCan:
if (frame % 2) == 0:
idx = frame // 2
can_sends.append(
hondacan.create_bosch_supplemental_1(
self.packer, CS.CP.carFingerprint, idx,
CS.CP.isPandaBlack))
# If using stock ACC, spam cancel command to kill gas when OP disengages.
if pcm_cancel_cmd:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.CANCEL, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
elif CS.out.cruiseState.standstill:
can_sends.append(
hondacan.spam_buttons_command(self.packer,
CruiseButtons.RES_ACCEL, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack))
else:
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
pump_on, self.last_pump_ts = brake_pump_hysteresis(
apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
can_sends.append(
hondacan.create_brake_command(self.packer, apply_brake,
pump_on, pcm_override,
pcm_cancel_cmd, hud.fcw, idx,
CS.CP.carFingerprint,
CS.CP.isPandaBlack,
CS.stock_brake))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(
create_gas_command(self.packer, apply_gas, idx))
return can_sends
def main(gctx=None):
retry = 0
folder_exists = False
dashcam_allowed = True
# make sure dashcam folder exists
while not folder_exists:
try:
if not os.path.exists(dashcam_videos_path):
os.makedirs(dashcam_videos_path)
else:
folder_exists = True
break
except OSError:
pass
if retry >= 5:
folder_exists = True
dashcam_allowed = False
retry += 1
time.sleep(5)
idlecam_enabled = False
health_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected health frequency
health_sock = messaging.sub_sock('health', timeout=health_timeout)
sensor_sock = messaging.sub_sock('sensorEvents')
samples = collections.deque([])
start_ts = None
last_started = False
dragon_dashcam_hours = 24. * 60 * 60
max_storage = (max_size_per_file/dashcam_duration) * dragon_dashcam_hours
dashcam_enabled = False
last_ts = 0.
last_modified = None
sleep = 5
thermal_sock = messaging.sub_sock('thermal')
params.put("DragonDashcamImpactDetectStarted", "0")
while dashcam_allowed:
type = TYPE_DRIVING
duration = dashcam_duration
ts = sec_since_boot()
if ts - last_ts >= 5.:
modified = get_last_modified()
if last_modified != modified:
dashcam_enabled = True if params.get("DragonEnableDashcam", encoding='utf8') == "1" else False
if dashcam_enabled:
idlecam_enabled = True if params.get("DragonDashcamImpactDetect", encoding='utf8') == "1" else False
else:
idlecam_enabled = False
try:
dragon_dashcam_hours = float(params.get("DragonDashcamHours", encoding='utf8')) * 60 * 60
except (TypeError, ValueError):
dragon_dashcam_hours = 24. * 60 * 60
max_storage = (max_size_per_file/dashcam_duration) * dragon_dashcam_hours
last_modified = modified
last_ts = ts
health = messaging.recv_sock(health_sock, wait=False)
started = True if health is not None and (health.health.ignitionLine or health.health.ignitionCan) else False
record_started = False
if last_started != started:
params.put("DragonDashcamImpactDetectStarted", "0")
samples.clear()
if started:
sleep = 5
if dashcam_enabled:
record_started = True
else:
if idlecam_enabled:
sleep = 0.02
sensor = messaging.recv_sock(sensor_sock, wait=False)
if sensor is not None:
for event in sensor.sensorEvents:
# accelerometer
if event.type == 1:
acc_x = event.acceleration.v[0]
acc_y = event.acceleration.v[1]
acc_z = event.acceleration.v[2]
g = math.sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z)
val = round(9.8-g,2)
if len(samples) >= 250:
avg_sample = round(sum(samples)/len(samples),2)
if start_ts is None and abs(val - avg_sample) >= 0.3:
duration = shock_duration
type = TYPE_SHOCK
start_ts = ts
record_started = True
params.put("DragonDashcamImpactDetectStarted", "1")
samples.pop()
samples.appendleft(val)
if start_ts is not None and ts - start_ts >= shock_duration-1:
start_ts = None
record_started = False
params.put("DragonDashcamImpactDetectStarted", "0")
last_started = started
if record_started:
now = datetime.datetime.now()
file_name = now.strftime("%Y-%m-%d_%H-%M-%S")
postfix = '_impact' if type == TYPE_SHOCK else ''
os.system("screenrecord --bit-rate %s --time-limit %s %s%s%s.mp4 &" % (bit_rates, duration, dashcam_videos_path, file_name, postfix))
start_time = time.time()
try:
used_spaces = get_used_spaces()
last_used_spaces = used_spaces
# we should clean up files here if use too much spaces
# when used spaces greater than max available storage
# or when free space is less than 10%
# get health of board, log this in "thermal"
msg = messaging.recv_sock(thermal_sock, wait=True)
if used_spaces >= max_storage or (msg is not None and msg.thermal.freeSpace < freespace_limit):
# get all the files in the dashcam_videos_path path
files = [f for f in sorted(os.listdir(dashcam_videos_path)) if os.path.isfile(dashcam_videos_path + f)]
for file in files:
msg = messaging.recv_sock(thermal_sock, wait=True)
# delete file one by one and once it has enough space for 1 video, we stop deleting
if used_spaces - last_used_spaces < max_size_per_file or msg.thermal.freeSpace < freespace_limit:
system("rm -fr %s" % (dashcam_videos_path + file))
last_used_spaces = get_used_spaces()
else:
break
except os.error as e:
pass
time_diff = time.time()-start_time
# we start the process 1 second before screenrecord ended
# to make sure there are no missing footage
sleep_time = duration-1-time_diff
if sleep_time >= 0.:
time.sleep(sleep_time)
else:
time.sleep(sleep)
def update(self, sm, pm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
# dp
# update variable status every 5 secs
if cur_time - self.last_ts >= 5.:
modified = get_last_modified()
if self.dp_last_modified != modified:
self.dragon_slow_on_curve = False if self.params.get(
"DragonEnableSlowOnCurve",
encoding='utf8') == "0" else True
try:
self.dragon_accel_profile = int(
self.params.get("DragonAccelProfile",
encoding='utf8').rstrip('\x00'))
except (TypeError, ValueError):
self.dragon_accel_profile = AP_OFF
put_nonblocking('DragonAccelProfile', AP_OFF)
self.dp_last_modified = modified
self.last_ts = cur_time
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
if self.dragon_slow_on_curve and len(sm['model'].path.poly):
path = list(sm['model'].path.poly)
# Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function
# y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b
# k = y'' / (1 + y'^2)^1.5
# TODO: compute max speed without using a list of points and without numpy
y_p = 3 * path[0] * self.path_x**2 + 2 * path[
1] * self.path_x + path[2]
y_pp = 6 * path[0] * self.path_x + 2 * path[1]
curv = y_pp / (1. + y_p**2)**1.5
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)
model_speed = max(20.0 * CV.MPH_TO_MS,
model_speed) # Don't slow down below 20mph
else:
model_speed = MAX_SPEED
# Calculate speed for normal cruise control
if enabled and not self.first_loop:
accel_limits = [
float(x) for x in calc_cruise_accel_limits(
v_ego, following, self.dragon_accel_profile)
]
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(
v_ego, sm['carState'].steeringAngle, accel_limits, self.CP,
self.dragon_accel_profile)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits_turns[1] = min(accel_limits_turns[1],
AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0],
accel_limits_turns[1])
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
self.v_model, self.a_model = speed_smoother(
self.v_acc_start, self.a_acc_start, model_speed,
2 * accel_limits[1], accel_limits[0], 2 * jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint)
self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, sm['controlsState'].active,
v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead,
lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw,
blinkers) and not sm['carState'].brakePressed
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
radar_dead = not sm.alive['radarState']
radar_errors = list(sm['radarState'].radarErrors)
radar_fault = car.RadarData.Error.fault in radar_errors
radar_can_error = car.RadarData.Error.canError in radar_errors
# **** send the plan ****
plan_send = messaging.new_message('plan')
plan_send.valid = sm.all_alive_and_valid(
service_list=['carState', 'controlsState', 'radarState'])
plan_send.plan.mdMonoTime = sm.logMonoTime['model']
plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
# longitudal plan
plan_send.plan.vCruise = float(self.v_cruise)
plan_send.plan.aCruise = float(self.a_cruise)
plan_send.plan.vStart = float(self.v_acc_start)
plan_send.plan.aStart = float(self.a_acc_start)
plan_send.plan.vTarget = float(self.v_acc)
plan_send.plan.aTarget = float(self.a_acc)
plan_send.plan.vTargetFuture = float(self.v_acc_future)
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(radar_can_error)
plan_send.plan.processingDelay = (plan_send.logMonoTime /
1e9) - sm.rcv_time['radarState']
# Send out fcw
plan_send.plan.fcw = fcw
pm.send('plan', plan_send)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (
self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (
a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
self.first_loop = False
def update(self, sm, pm, CP, VM):
# dragonpilot
cur_time = sec_since_boot()
if cur_time - self.last_ts >= 5.:
modified = get_last_modified()
if self.dp_last_modified != modified:
self.lane_change_enabled = True if self.params.get(
"LaneChangeEnabled", encoding='utf8') == "1" else False
if self.lane_change_enabled:
self.dragon_auto_lc_enabled = True if self.params.get(
"DragonEnableAutoLC",
encoding='utf8') == "1" else False
# adjustable assisted lc min speed
try:
self.dragon_assisted_lc_min_mph = float(
self.params.get("DragonAssistedLCMinMPH",
encoding='utf8'))
except (TypeError, ValueError):
self.dragon_assisted_lc_min_mph = 45
self.dragon_assisted_lc_min_mph *= CV.MPH_TO_MS
if self.dragon_assisted_lc_min_mph < 0:
self.dragon_assisted_lc_min_mph = 0
if self.dragon_auto_lc_enabled:
# adjustable auto lc min speed
try:
self.dragon_auto_lc_min_mph = float(
self.params.get("DragonAutoLCMinMPH",
encoding='utf8'))
except (TypeError, ValueError):
self.dragon_auto_lc_min_mph = 60
self.dragon_auto_lc_min_mph *= CV.MPH_TO_MS
if self.dragon_auto_lc_min_mph < 0:
self.dragon_auto_lc_min_mph = 0
# when auto lc is smaller than assisted lc, we set assisted lc to the same speed as auto lc
if self.dragon_auto_lc_min_mph < self.dragon_assisted_lc_min_mph:
self.dragon_assisted_lc_min_mph = self.dragon_auto_lc_min_mph
# adjustable auto lc delay
try:
self.dragon_auto_lc_delay = float(
self.params.get("DragonAutoLCDelay",
encoding='utf8'))
except (TypeError, ValueError):
self.dragon_auto_lc_delay = 2.
if self.dragon_auto_lc_delay < 0:
self.dragon_auto_lc_delay = 0
else:
self.dragon_auto_lc_enabled = False
self.dp_enable_sr_boost = True if self.params.get(
"DragonEnableSteerBoost",
encoding='utf8') == "1" else False
if self.dp_enable_sr_boost:
try:
sr_boost_min = float(
self.params.get("DragonSteerBoostMin",
encoding='utf8'))
sr_boost_Max = float(
self.params.get("DragonSteerBoostMax",
encoding='utf8'))
self.dp_sr_boost_range = [sr_boost_min, sr_boost_Max]
boost_min_at = float(
self.params.get("DragonSteerBoostMinAt",
encoding='utf8'))
boost_max_at = float(
self.params.get("DragonSteerBoostMaxAt",
encoding='utf8'))
self.dp_sr_boost_bp = [boost_min_at, boost_max_at]
except (TypeError, ValueError):
put_nonblocking("DragonEnableSteerBoost", '0')
self.dp_enable_sr_boost = False
if not self.dp_enable_sr_boost:
self.dp_sr_boost_range = [0., 0.]
self.dp_sr_boost_bp = [0., 0.]
self.dp_last_modified = modified
self.last_ts = cur_time
v_ego = sm['carState'].vEgo
angle_steers = sm['carState'].steeringAngle
active = sm['controlsState'].active
angle_offset = sm['liveParameters'].angleOffset
# atom
v_ego_kph = v_ego * CV.MS_TO_KPH
# Run MPC
self.angle_steers_des_prev = self.angle_steers_des_mpc
VM.update_params(sm['liveParameters'].stiffnessFactor,
sm['liveParameters'].steerRatio)
curvature_factor = VM.curvature_factor(v_ego)
boost_rate = (1 + (interp(abs(angle_steers), self.dp_sr_boost_bp,
self.dp_sr_boost_range) /
100)) if self.dp_enable_sr_boost else 1
self.dp_steer_ratio = VM.sR * boost_rate
self.LP.parse_model(sm['model'])
# Lane change logic
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
below_lane_change_speed = v_ego < self.dragon_assisted_lc_min_mph
if sm['carState'].leftBlinker:
self.lane_change_direction = LaneChangeDirection.left
elif sm['carState'].rightBlinker:
self.lane_change_direction = LaneChangeDirection.right
if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (
not one_blinker) or (not self.lane_change_enabled):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
torque_applied = sm['carState'].steeringPressed and \
((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or \
(sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
# dragonpilot auto lc
if not below_lane_change_speed and self.dragon_auto_lc_enabled and v_ego >= self.dragon_auto_lc_min_mph:
# we allow auto lc when speed reached dragon_auto_lc_min_mph
self.dragon_auto_lc_allowed = True
if self.dragon_auto_lc_timer is None:
# we only set timer when in preLaneChange state, dragon_auto_lc_delay delay
if self.lane_change_state == LaneChangeState.preLaneChange:
self.dragon_auto_lc_timer = cur_time + self.dragon_auto_lc_delay
elif cur_time >= self.dragon_auto_lc_timer:
# if timer is up, we set torque_applied to True to fake user input
torque_applied = True
else:
# if too slow, we reset all the variables
self.dragon_auto_lc_allowed = False
self.dragon_auto_lc_timer = None
# we reset the timers when torque is applied regardless
if torque_applied:
self.dragon_auto_lc_timer = None
# State transitions
# off
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
# pre
elif self.lane_change_state == LaneChangeState.preLaneChange:
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
elif torque_applied:
self.lane_change_state = LaneChangeState.laneChangeStarting
# starting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
# fade out lanelines over .5s
#self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0)
# fade out over .2s
self.lane_change_ll_prob = max(
self.lane_change_ll_prob - DT_MDL / 5, 0.0)
# 98% certainty
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
# finishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
# fade in laneline over 1s
self.lane_change_ll_prob = min(
self.lane_change_ll_prob + DT_MDL, 1.0)
if one_blinker and self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.preLaneChange
elif self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.off
# when finishing, we reset timer to none.
self.dragon_auto_lc_timer = None
if self.lane_change_state in [
LaneChangeState.off, LaneChangeState.preLaneChange
]:
self.lane_change_timer = 0.0
else:
self.lane_change_timer += DT_MDL
self.prev_one_blinker = one_blinker
desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Turn off lanes during lane change
if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft:
self.LP.l_prob *= self.lane_change_ll_prob
self.LP.r_prob *= self.lane_change_ll_prob
self.LP.update_d_poly(v_ego)
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego,
angle_steers - angle_offset,
curvature_factor,
self.dp_steer_ratio,
CP.steerActuatorDelay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
list(self.LP.l_poly), list(self.LP.r_poly),
list(self.LP.d_poly), self.LP.l_prob,
self.LP.r_prob, curvature_factor, v_ego_mpc,
self.LP.lane_width)
# reset to current steer angle if not active or overriding
if active:
delta_desired = self.mpc_solution[0].delta[1]
rate_desired = math.degrees(self.mpc_solution[0].rate[0] *
self.dp_steer_ratio)
else:
delta_desired = math.radians(angle_steers -
angle_offset) / self.dp_steer_ratio
rate_desired = 0.0
self.cur_state[0].delta = delta_desired
#self.angle_steers_des_mpc = float(math.degrees(delta_desired * self.dp_steer_ratio) + angle_offset)
#old_angle_steers_des = self.angle_steers_des_mpc
org_angle_steers_des = float(
math.degrees(delta_desired * self.steerRatio) + angle_offset)
self.angle_steers_des_mpc = org_angle_steers_des
# atom
if v_ego_kph < 40:
xp = [0, 5, 20, 40]
fp2 = [0.3, 0.5, 1, 1.5]
limit_steers = interp(v_ego_kph, xp, fp2)
angle_steers_des = self.limit_ctrl(org_angle_steers_des,
limit_steers, angle_steers)
self.angle_steers_des_mpc = angle_steers_des
elif self.lane_change_state != LaneChangeState.off:
self.angle_steers_des_mpc = self.limit_ctrl(
org_angle_steers_des, 10, angle_steers)
# Check for infeasable MPC solution
mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta)
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE,
MPC_COST_LAT.HEADING, CP.steerRateCost)
self.cur_state[0].delta = math.radians(
angle_steers - angle_offset) / self.dp_steer_ratio
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if self.mpc_solution[
0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge
self.solution_invalid_cnt += 1
else:
self.solution_invalid_cnt = 0
plan_solution_valid = self.solution_invalid_cnt < 2
plan_send = messaging.new_message('pathPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=[
'carState', 'controlsState', 'liveParameters', 'model'
])
plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly]
plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly]
plan_send.pathPlan.lProb = float(self.LP.l_prob)
plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly]
plan_send.pathPlan.rProb = float(self.LP.r_prob)
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
plan_send.pathPlan.rateSteers = float(rate_desired)
plan_send.pathPlan.angleOffset = float(
sm['liveParameters'].angleOffsetAverage)
plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid)
plan_send.pathPlan.posenetValid = bool(
sm['liveParameters'].posenetValid)
plan_send.pathPlan.desire = desire
plan_send.pathPlan.laneChangeState = self.lane_change_state
plan_send.pathPlan.laneChangeDirection = self.lane_change_direction
plan_send.pathPlan.alcAllowed = self.dragon_auto_lc_allowed
pm.send('pathPlan', plan_send)
if LOG_MPC:
dat = messaging.new_message('liveMpc')
dat.liveMpc.x = list(self.mpc_solution[0].x)
dat.liveMpc.y = list(self.mpc_solution[0].y)
dat.liveMpc.psi = list(self.mpc_solution[0].psi)
dat.liveMpc.delta = list(self.mpc_solution[0].delta)
dat.liveMpc.cost = self.mpc_solution[0].cost
pm.send('liveMpc', dat)
def controlsd_thread(sm=None, pm=None, can_sock=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
is_metric = params.get("IsMetric", encoding='utf8') == "1"
is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
passive = passive or not openpilot_enabled_toggle
# Passive if internet needed
internet_needed = params.get("Offroad_ConnectivityNeeded",
encoding='utf8') is not None
passive = passive or internet_needed
# Pub/Sub Sockets
if pm is None:
pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl', 'carEvents',
'carParams'
])
if sm is None:
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', \
'model'])
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for health and CAN packets
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
print("Waiting for CAN messages...")
messaging.get_one_can(can_sock)
CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay)
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
community_feature_disallowed = CP.communityFeature and not community_feature_toggle
read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed
if read_only:
CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
put_nonblocking("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)
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
v_cruise_kph_last = 0
mismatch_counter = 0
can_error_counter = 0
last_blinker_frame = 0
saturated_count = 0
events_prev = []
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['pathPlan'].sensorValid = True
sm['pathPlan'].posenetValid = True
sm['thermal'].freeSpace = 1.
sm['dMonitoringState'].events = []
sm['dMonitoringState'].awarenessStatus = 1.
sm['dMonitoringState'].faceDetected = False
# 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
# dp
ts_last_check = 0.
dragon_toyota_stock_dsu = False
dragon_lat_control = True
dragon_display_steering_limit_alert = True
dragon_stopped_has_lead_count = 0
dragon_lead_car_moving_alert = False
dp_last_modified = None
dp_camera_offset = CAMERA_OFFSET
hyundai_lkas = read_only
while True:
start_time = sec_since_boot()
# dp
ts = start_time
if ts - ts_last_check >= 5.:
modified = get_last_modified()
if dp_last_modified != modified:
try:
dp_camera_offset = int(
params.get("DragonCameraOffset",
encoding='utf8')) * 0.01
except (TypeError, ValueError):
dp_camera_offset = CAMERA_OFFSET
dragon_toyota_stock_dsu = True if params.get(
"DragonToyotaStockDSU", encoding='utf8') == "1" else False
dragon_lat_control = False if params.get(
"DragonLatCtrl", encoding='utf8') == "0" else True
if dragon_lat_control:
dragon_display_steering_limit_alert = False if params.get(
"DragonDisplaySteeringLimitAlert",
encoding='utf8') == "0" else True
else:
dragon_display_steering_limit_alert = False
dragon_lead_car_moving_alert = True if params.get(
"DragonEnableLeadCarMovingAlert",
encoding='utf8') == "1" else False
dp_last_modified = modified
ts_last_check = ts
prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events
CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample(
CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter,
params, dragon_toyota_stock_dsu)
if read_only:
hyundai_lkas = read_only
elif CS.cruiseState.enabled:
# elif state == State.enabled:
hyundai_lkas = False
prof.checkpoint("Sample")
# Create alerts
if not sm.alive['plan'] and sm.alive[
'pathPlan']: # only plan not being received: radar not communicating
events.append(
create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
elif 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 and os.getenv("NOSENSOR") is None:
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.WARNING]))
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:
if dragon_toyota_stock_dsu:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
else:
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]))
if internet_needed:
events.append(
create_event('internetConnectivityNeeded',
[ET.NO_ENTRY, ET.PERMANENT]))
if community_feature_disallowed:
events.append(
create_event('communityFeatureDisallowed', [ET.PERMANENT]))
if read_only and not passive:
events.append(create_event('carUnrecognized', [ET.PERMANENT]))
if log.HealthData.FaultType.relayMalfunction in sm['health'].faults:
events.append(
create_event(
'relayMalfunction',
[ET.NO_ENTRY, ET.PERMANENT, ET.IMMEDIATE_DISABLE]))
# Only allow engagement with brake pressed when stopped behind another stopped car
if not dragon_toyota_stock_dsu and 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]))
# dp
if dragon_lead_car_moving_alert:
# when car has a lead and is standstill and lead is barely moving, we start counting
if not CP.radarOffCan and sm[
'plan'].hasLead and CS.vEgo <= 0.01 and 0.3 >= abs(
sm['plan'].vTarget) >= 0:
dragon_stopped_has_lead_count += 1
else:
dragon_stopped_has_lead_count = 0
# when we detect lead car over 3 secs and the lead car is started moving, we are ready to send alerts
# once the condition is triggered, we want to keep the trigger
if dragon_stopped_has_lead_count >= 300:
if abs(sm['plan'].vTargetFuture) >= 0.1:
events.append(create_event('leadCarMoving', [ET.WARNING]))
else:
events.append(create_event('leadCarDetected',
[ET.WARNING]))
# we remove alert once our car is moving
if CS.vEgo > 0.:
dragon_stopped_has_lead_count = 0
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, v_acc, a_acc, lac_log, last_blinker_frame, saturated_count = \
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
LaC, LoC, hyundai_lkas, is_metric, cal_perc, last_blinker_frame, saturated_count, dragon_lat_control, dragon_display_steering_limit_alert, dragon_lead_car_moving_alert)
prof.checkpoint("State Control")
# Publish data
CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events,
actuators, v_cruise_kph, rk, AM, LaC, LoC,
hyundai_lkas, start_time, v_acc, a_acc,
lac_log, events_prev, last_blinker_frame,
is_ldw_enabled, can_error_counter,
dp_camera_offset)
prof.checkpoint("Sent")
rk.monitor_time()
prof.display()
if not CS.cruiseState.enabled:
# if state == State.disabled:
hyundai_lkas = True
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
left_line, right_line, lead, left_lane_depart, right_lane_depart):
# dp
if frame % 500 == 0:
modified = get_last_modified()
if self.dp_last_modified != modified:
self.dragon_lane_departure_warning = False if params.get("DragonToyotaLaneDepartureWarning", encoding='utf8') == "0" else True
self.dragon_toyota_sng_mod = True if params.get("DragonToyotaSnGMod", encoding='utf8') == "1" else False
self.dragon_lat_ctrl, \
self.lane_change_enabled, \
self.dragon_enable_steering_on_signal = common_controller_update(self.lane_change_enabled)
self.dp_last_modified = modified
# *** compute control surfaces ***
# gas and brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# steer torque
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams)
self.steer_rate_limited = new_steer != apply_steer
# only cut torque when steer state is a known fault
if CS.steer_state in [9, 25]:
self.last_fault_frame = frame
# # Cut steering for 2s after fault
if not enabled: # or (frame - self.last_fault_frame < 200):
apply_steer = 0
apply_steer_req = 0
else:
apply_steer_req = 1
if not enabled and CS.pcm_acc_status:
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
pcm_cancel_cmd = 1
# on entering standstill, send standstill request
if not self.dragon_toyota_sng_mod and CS.out.standstill and not self.last_standstill:
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_steer = apply_steer
self.last_accel = apply_accel
self.last_standstill = CS.out.standstill
# dp
apply_steer_req = common_controller_ctrl(enabled,
self.dragon_lat_ctrl,
self.dragon_enable_steering_on_signal,
CS.out.leftBlinker,
CS.out.rightBlinker,
apply_steer_req)
can_sends = []
#*** control msgs ***
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
if Ecu.fwdCamera in self.fake_ecus:
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead))
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))
# ui mesg is at 100Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
fcw_alert = hud_alert == VisualAlert.fcw
steer_alert = hud_alert == VisualAlert.steerRequired
send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
(not (fcw_alert or steer_alert) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
elif pcm_cancel_cmd:
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
send_ui = True
# dp
if self.dragon_lane_departure_warning:
dragon_left_lane_depart = left_lane_depart
dragon_right_lane_depart = right_lane_depart
else:
dragon_left_lane_depart = False
dragon_right_lane_depart = False
if (frame % 100 == 0 or send_ui) and Ecu.fwdCamera in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, dragon_left_lane_depart, dragon_right_lane_depart))
if frame % 100 == 0 and Ecu.dsu in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
#*** static msgs ***
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
if frame % fr_step == 0 and ecu in self.fake_ecus and CS.CP.carFingerprint in cars:
can_sends.append(make_can_msg(addr, vl, bus))
return can_sends
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd,
visual_alert, left_lane, right_lane, left_lane_depart,
right_lane_depart):
# *** compute control surfaces ***
# gas and brake
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = self.accel_hysteresis(
apply_accel, self.accel_steady)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
# dp
if frame % 500 == 0:
modified = get_last_modified()
if self.dp_last_modified != modified:
self.dragon_lat_ctrl, \
self.lane_change_enabled, \
self.dragon_enable_steering_on_signal = common_controller_update(self.lane_change_enabled)
self.dp_last_modified = modified
# 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
lkas_active = enabled
# Disable steering while turning blinker on and speed below 60 kph
if CS.out.leftBlinker or CS.out.rightBlinker:
if self.car_fingerprint in [CAR.KONA, CAR.IONIQ]:
self.turning_signal_timer = 100 # Disable for 1.0 Seconds after blinker turned off
elif CS.left_blinker_flash or CS.right_blinker_flash: # Optima has blinker flash signal only
self.turning_signal_timer = 100
if self.turning_signal_timer and CS.out.vEgo < 20.1168:
lkas_active = 0
if self.turning_signal_timer:
self.turning_signal_timer -= 1
if not lkas_active:
apply_steer = 0
steer_req = 1 if apply_steer else 0
self.apply_accel_last = apply_accel
self.apply_steer_last = apply_steer
# dp
lkas_active = common_controller_ctrl(
enabled, self.dragon_lat_ctrl,
self.dragon_enable_steering_on_signal, CS.out.leftBlinker,
CS.out.rightBlinker, lkas_active)
sys_warning, sys_state, left_lane_warning, right_lane_warning =\
self.process_hud_alert(enabled, self.car_fingerprint, visual_alert,
left_lane, right_lane, left_lane_depart, right_lane_depart)
clu11_speed = CS.clu11["CF_Clu_Vanz"]
enabled_speed = 38 if CS.is_set_speed_in_mph else 60
if clu11_speed > enabled_speed or not lkas_active:
enabled_speed = clu11_speed
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
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 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))
if pcm_cancel_cmd and self.longcontrol:
can_sends.append(
create_clu11(self.packer, frame, CS.scc_bus, CS.clu11,
Buttons.CANCEL, clu11_speed))
else: # send mdps12 to LKAS to prevent LKAS error if no cancel cmd
can_sends.append(create_mdps12(self.packer, frame, CS.mdps12))
if CS.scc_bus and self.longcontrol and frame % 2: # send scc12 to car if SCC not on bus 0 and longcontrol enabled
can_sends.append(
create_scc12(self.packer, apply_accel, enabled, self.scc12_cnt,
CS.scc12))
self.scc12_cnt += 1
if CS.out.cruiseState.standstill:
# run only first time when the car stopped
if self.last_lead_distance == 0:
# 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.clu11_cnt = 0
# reset lead distnce after the car starts moving
elif self.last_lead_distance != 0:
self.last_lead_distance = 0
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [
CAR.SONATA, CAR.PALISADE, CAR.SELTOS
]:
can_sends.append(create_lfa_mfa(self.packer, frame, enabled))
return can_sends
