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