diff --git a/common/params.py b/common/params.py index 607a09050..d0509b514 100755 --- a/common/params.py +++ b/common/params.py @@ -178,6 +178,8 @@ keys = { "DragonEnableSeatBeltCheck": [TxType.PERSISTENT], "DragonEnableGearCheck": [TxType.PERSISTENT], "DragonEnableTempMonitor": [TxType.PERSISTENT], + "DragonEnableCurvatureLearner": [TxType.PERSISTENT], + "DragonCurvatureLearnerOffset": [TxType.PERSISTENT], } diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 648ab9c06..e768246f9 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -6,11 +6,10 @@ from selfdrive.controls.lib.lateral_mpc import libmpc_py from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT from selfdrive.controls.lib.lane_planner import LanePlanner from selfdrive.config import Conversions as CV -from common.params import Params +from common.params import Params, put_nonblocking import cereal.messaging as messaging from cereal import log # dragonpilot -from common.params import Params from selfdrive.dragonpilot.dragonconf import dp_get_last_modified LaneChangeState = log.PathPlan.LaneChangeState @@ -75,6 +74,9 @@ class PathPlanner(): self.dragon_auto_lc_delay = 2. self.last_ts = 0. self.dp_last_modified = None + self.dp_curvature_learner = False + self.dp_curvature_learner_offset = 0. + self.last_curvature_update = 0. def setup_mpc(self): self.libmpc = libmpc_py.libmpc @@ -98,6 +100,14 @@ class PathPlanner(): if cur_time - self.last_ts >= 5.: modified = dp_get_last_modified() if self.dp_last_modified != modified: + self.dp_curvature_learner = True if self.params.get("DragonEnableCurvatureLearner", encoding='utf8') == "1" else False + if self.dp_curvature_learner: + try: + self.dp_curvature_learner_offset = float(self.params.get("DragonCurvatureLearnerOffset", encoding='utf8')) + except (TypeError, ValueError): + self.dp_curvature_learner_offset = 0. + else: + self.dp_curvature_learner_offset = 0. 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 @@ -132,6 +142,9 @@ class PathPlanner(): self.dragon_auto_lc_enabled = False self.dp_last_modified = modified self.last_ts = cur_time + if self.dp_curvature_learner and cur_time - self.last_curvature_update >= 120.: + put_nonblocking('DragonCurvatureLearnerOffset', str(self.dp_curvature_learner_offset)) + self.last_curvature_update = cur_time v_ego = sm['carState'].vEgo angle_steers = sm['carState'].steeringAngle @@ -142,7 +155,7 @@ class PathPlanner(): # 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) + curvature_factor = VM.curvature_factor(v_ego) + self.dp_curvature_learner_offset self.LP.parse_model(sm['model']) @@ -238,6 +251,14 @@ class PathPlanner(): self.LP.update_d_poly(v_ego) + if self.dp_curvature_learner: + if active: + if angle_steers - angle_offset > 0.5: + self.dp_curvature_learner_offset -= self.LP.d_poly[3] / 12000 + elif angle_steers - angle_offset < -0.5: + self.dp_curvature_learner_offset += self.LP.d_poly[3] / 12000 + self.dp_curvature_learner_offset = clip(self.dp_curvature_learner_offset, -0.3, 0.3) + # account for actuation delay self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay) diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 0f06e5bdd..8ce8a172e 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -76,6 +76,8 @@ default_conf = { 'DragonEnableSeatBeltCheck': '1', 'DragonEnableGearCheck': '1', 'DragonEnableTempMonitor': '1', + 'DragonEnableCurvatureLearner': '0', + 'DragonCurvatureLearnerOffset': '0.0', } deprecated_conf = {