From bc0c5934017f023848e6a4a4c7d38d6b09457bec Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Tue, 18 Feb 2020 17:34:02 +1000 Subject: [PATCH] dp 0.7.3 --- cereal/log.capnp | 2 +- common/params.py | 2 - selfdrive/controls/lib/pathplanner.py | 18 +- selfdrive/dragonpilot/appd/appd.py | 229 +++++++++---------- selfdrive/dragonpilot/dragonconf/__init__.py | 2 - 5 files changed, 118 insertions(+), 135 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index 1c545aa09..00e063b65 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -293,7 +293,7 @@ struct ThermalData { memUsedPercent @19 :Int8; cpuPerc @20 :Int8; - ipAddr @23 :Text; # dragonpilot + ipAddr @24 :Text; # dragonpilot enum ThermalStatus { green @0; # all processes run diff --git a/common/params.py b/common/params.py index e651536c9..d1e939a46 100755 --- a/common/params.py +++ b/common/params.py @@ -153,14 +153,12 @@ keys = { "DragonUIBlinker": [TxType.PERSISTENT], "DragonEnableDriverMonitoring": [TxType.PERSISTENT], "DragonCarModel": [TxType.PERSISTENT], - "DragonCarVIN": [TxType.PERSISTENT], #deprecated "DragonEnableSlowOnCurve": [TxType.PERSISTENT], "DragonEnableLeadCarMovingAlert": [TxType.PERSISTENT], "DragonToyotaSnGMod": [TxType.PERSISTENT], "DragonEnableSRLearner": [TxType.PERSISTENT], "DragonWazeMode": [TxType.PERSISTENT], "DragonRunWaze": [TxType.PERSISTENT], - "DragonEnableAssistedLC": [TxType.PERSISTENT], "DragonEnableAutoLC": [TxType.PERSISTENT], "DragonAssistedLCMinMPH": [TxType.PERSISTENT], "DragonAutoLCMinMPH": [TxType.PERSISTENT], diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index f64b30ac1..1f2eea032 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -68,8 +68,8 @@ class PathPlanner(): self.dragon_auto_lc_enabled = False self.dragon_auto_lc_allowed = False self.dragon_auto_lc_timer = None - self.dragon_assisted_lc_min_mph = 37 - self.dragon_auto_lc_min_mph = 60 + self.dragon_assisted_lc_min_mph = 37 * CV.MPH_TO_MS + self.dragon_auto_lc_min_mph = 60 * CV.MPH_TO_MS self.dragon_auto_lc_delay = 2. self.last_ts = 0. @@ -93,16 +93,16 @@ class PathPlanner(): # dragonpilot cur_time = sec_since_boot() if cur_time - self.last_ts > 5.: - self.dragon_assisted_lc_enabled = True if self.params.get("DragonEnableAssistedLC", encoding='utf8') == "1" else False + self.dragon_assisted_lc_enabled = self.lane_change_enabled if self.dragon_assisted_lc_enabled: self.dragon_auto_lc_enabled = True if self.params.get("DragonEnableAutoLC", encoding='utf8') == "1" else False # adjustable assisted lc min speed - self.dragon_assisted_lc_min_mph = int(self.params.get("DragonAssistedLCMinMPH", encoding='utf8')) + self.dragon_assisted_lc_min_mph = int(self.params.get("DragonAssistedLCMinMPH", encoding='utf8')) * 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 - self.dragon_auto_lc_min_mph = int(self.params.get("DragonAutoLCMinMPH", encoding='utf8')) + self.dragon_auto_lc_min_mph = int(self.params.get("DragonAutoLCMinMPH", encoding='utf8')) * 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 @@ -129,7 +129,7 @@ class PathPlanner(): # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker - below_lane_change_speed = not self.dragon_assisted_lc_enabled or v_ego < self.dragon_assisted_lc_min_mph * CV.MPH_TO_MS + below_lane_change_speed = v_ego < self.dragon_assisted_lc_min_mph if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left @@ -147,7 +147,7 @@ class PathPlanner(): 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 * CV.MPH_TO_MS: + 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 @@ -155,7 +155,7 @@ class PathPlanner(): # 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: + 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: @@ -169,7 +169,7 @@ class PathPlanner(): # State transitions # off - if self.dragon_assisted_lc_enabled and self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: + 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 # pre diff --git a/selfdrive/dragonpilot/appd/appd.py b/selfdrive/dragonpilot/appd/appd.py index 82bdb0b5f..a6a4e7112 100644 --- a/selfdrive/dragonpilot/appd/appd.py +++ b/selfdrive/dragonpilot/appd/appd.py @@ -75,35 +75,32 @@ class App(): self.last_ts = sec_since_boot() def read_params(self): - cur_time = sec_since_boot() - if cur_time - self.last_ts > 5: - self.last_is_enabled = self.is_enabled - if self.enable_param is None: - self.is_enabled = False - else: - self.is_enabled = True if params.get(self.enable_param, encoding='utf8') == "1" else False + self.last_is_enabled = self.is_enabled + if self.enable_param is None: + self.is_enabled = False + else: + self.is_enabled = True if params.get(self.enable_param, encoding='utf8') == "1" else False - if self.is_enabled: - # a service app should run automatically and not manual controllable. - if self.app_type in [App.TYPE_SERVICE, App.TYPE_GPS_SERVICE]: - self.is_auto_runnable = True + if self.is_enabled: + # a service app should run automatically and not manual controllable. + if self.app_type in [App.TYPE_SERVICE, App.TYPE_GPS_SERVICE]: + self.is_auto_runnable = True + self.manual_ctrl_status = self.MANUAL_IDLE + else: + if self.manual_ctrl_param is None: self.manual_ctrl_status = self.MANUAL_IDLE else: - if self.manual_ctrl_param is None: - self.manual_ctrl_status = self.MANUAL_IDLE - else: - self.manual_ctrl_status = params.get(self.manual_ctrl_param, encoding='utf8') + self.manual_ctrl_status = params.get(self.manual_ctrl_param, encoding='utf8') + if self.manual_ctrl_status == self.MANUAL_IDLE: if self.auto_run_param is None: self.is_auto_runnable = False else: self.is_auto_runnable = True if params.get(self.auto_run_param, encoding='utf8') == "1" else False - else: - self.is_auto_runnable = False - self.manual_ctrl_status = self.MANUAL_IDLE - self.manually_ctrled = False - - self.last_ts = cur_time + else: + self.is_auto_runnable = False + self.manual_ctrl_status = self.MANUAL_IDLE + self.manually_ctrled = False def run(self, force = False): if force or self.is_enabled: @@ -164,6 +161,16 @@ class App(): returncode=e.returncode) def init_apps(apps): + apps.append(App( + "cn.dragonpilot.gpsservice", + "cn.dragonpilot.gpsservice.MainService", + "DragonGreyPandaMode", + None, + None, + App.TYPE_GPS_SERVICE, + [], + [], + )) apps.append(App( # v1.16.2 "com.tomtom.speedcams.android.map", @@ -231,16 +238,6 @@ def init_apps(apps): "SYSTEM_ALERT_WINDOW", ] )) - apps.append(App( - "cn.dragonpilot.gpsservice", - "cn.dragonpilot.gpsservice.MainService", - "DragonGreyPandaMode", - None, - None, - App.TYPE_GPS_SERVICE, - [], - [], - )) apps.append(App( # v4.57.2.0 "com.waze", @@ -272,8 +269,6 @@ def main(): system(f"LD_LIBRARY_PATH= input tap 995 160") system(f"pkill com.android.settings") - init_apps(apps) - last_started = False thermal_sock = messaging.sub_sock('thermal') @@ -283,105 +278,97 @@ def main(): allow_auto_run = True last_thermal_status = None thermal_status = None - - set_location_provider_allowed = False + start_ts = sec_since_boot() + init_done = False while 1: #has_enabled_apps: - has_fullscreen_apps = False - has_gps_apps = False - has_gps_service_apps = False + if not init_done and sec_since_boot() - start_ts >= 10: + init_apps(apps) + init_done = True - for app in apps: - # read params loop - app.read_params() - if app.last_is_enabled and not app.is_enabled and app.is_running: - app.kill(True) + if init_done: + enabled_apps = [] + has_fullscreen_apps = False - if app.is_enabled: - if not has_fullscreen_apps and app.app_type == App.TYPE_FULLSCREEN: - has_fullscreen_apps = True - elif not has_gps_apps and app.app_type == App.TYPE_GPS: - has_gps_apps = True - elif not has_gps_service_apps and app.app_type == App.TYPE_GPS_SERVICE: - has_gps_service_apps = True - - # process manual ctrl apps - if app.manual_ctrl_status != App.MANUAL_IDLE: - if app.manual_ctrl_status == App.MANUAL_ON: - app.run(True) - else: - app.kill(True) - - # set location provider accuracy - if not set_location_provider_allowed and (has_gps_apps or has_gps_service_apps): - system(f"settings put secure location_providers_allowed -gps") - system(f"settings put secure location_providers_allowed -network") - system(f"settings put secure location_providers_allowed +gps,network") - set_location_provider_allowed = True - - 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 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 apps: - if not app.manually_ctrled: - if has_fullscreen_apps: - if app.app_type == App.TYPE_FULLSCREEN: + # read params loop + 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) + + 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 in [App.TYPE_GPS, App.TYPE_UTIL]: + elif app.app_type == 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: + + # 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() - # when car is stopped - else: - start_delay = None - # set delay to 30 seconds - if stop_delay is None: - stop_delay = frame + 30 + 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 apps: - if app.is_running and not app.manually_ctrled: - if has_fullscreen_apps or frame > stop_delay: - app.kill() + 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 apps: - app.manually_ctrled = False + if last_started != started: + for app in enabled_apps: + app.manually_ctrled = False - last_started = started - frame += 3 - time.sleep(3) + last_started = started + frame += 3 + time.sleep(3) def system(cmd): try: diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index c41e92aa0..87fb015c3 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -58,7 +58,6 @@ default_conf = { 'DragonEnableSRLearner': '1', 'DragonWazeMode': '0', 'DragonRunWaze': '0', - 'DragonEnableAssistedLC': '1', 'DragonEnableAutoLC': '0', 'DragonAssistedLCMinMPH': 45, 'DragonAutoLCMinMPH': 60, @@ -68,7 +67,6 @@ default_conf = { } deprecated_conf = { - 'DragonCarVIN': '', } deprecated_conf_invert = {