From 6871bfdb9945920ea47a124a9e5ea442aab82e23 Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Mon, 12 May 2025 14:05:46 +0800 Subject: [PATCH] Adaptive Coasting Mode (ACM) - 2025/05/20 --- common/params_keys.h | 2 + dragonpilot/selfdrive/controls/lib/acm.py | 84 +++++++++++++++++++ .../controls/lib/longitudinal_planner.py | 24 ++++++ selfdrive/controls/plannerd.py | 4 + selfdrive/ui/qt/offroad/dp_panel.cc | 16 ++++ system/manager/manager.py | 2 + 6 files changed, 132 insertions(+) create mode 100644 dragonpilot/selfdrive/controls/lib/acm.py diff --git a/common/params_keys.h b/common/params_keys.h index b576f1a97..775a8ef9c 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -119,4 +119,6 @@ inline static std::unordered_map keys = { {"Version", PERSISTENT}, {"dp_device_last_log", CLEAR_ON_MANAGER_START}, {"dp_device_reset_conf", CLEAR_ON_MANAGER_START}, + {"dp_lon_acm", PERSISTENT}, + {"dp_lon_acm_downhill", PERSISTENT}, }; diff --git a/dragonpilot/selfdrive/controls/lib/acm.py b/dragonpilot/selfdrive/controls/lib/acm.py new file mode 100644 index 000000000..fe0fa90c5 --- /dev/null +++ b/dragonpilot/selfdrive/controls/lib/acm.py @@ -0,0 +1,84 @@ +''' +MIT Non-Commercial License + +Copyright (c) 2019, dragonpilot + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, for non-commercial purposes only, subject to the following conditions: + +- The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. +- Commercial use (e.g., use in a product, service, or activity intended to generate revenue) is prohibited without explicit written permission from dragonpilot. Contact ricklan@gmail.com for inquiries. +- Any project that uses the Software must visibly mention the following acknowledgment: "This project uses software from dragonpilot and is licensed under a custom license requiring permission for use." + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +Downhill Coasting allows the vehicle to maintain or slightly increase speed on downhill slopes without braking. +''' + +import numpy as np + +SLOPE = -0.04 +RATIO = 0.9 + +TTC = 5. +TTC_BP = [3.5, 2.5] +MIN_BRAKE_ALLOW_VALS = [0., -0.5] + +class ACM: + def __init__(self): + self.enabled = False + self.downhill_only = False + self._is_downhill = False + self._is_speed_over_cruise = False + self._has_lead = False + self._active_prev = False + + self.active = False + self.just_disabled = False + self.allowed_brake_val = 0. + + def update_states(self, cs, rs, user_ctrl_lon, v_ego, v_cruise): + self.lead_ttc = float('inf') # Default if no lead + + if not self.enabled: + self.active = False + return + + if len(cs.orientationNED) != 3: + self.active = False + return + + pitch_rad = cs.orientationNED[1] + self._is_downhill = np.sin(pitch_rad) < SLOPE + self._is_speed_over_cruise = v_ego > (v_cruise * RATIO) + + lead = rs.leadOne + if lead and lead.status: + self.lead_ttc = lead.dRel / v_ego if v_ego > 0 else float('inf') + self.allowed_brake_val = np.interp(self.lead_ttc, TTC_BP, MIN_BRAKE_ALLOW_VALS) + self._has_lead = self.lead_ttc < TTC + else: + self._has_lead = False + + self.active = not user_ctrl_lon and not self._has_lead and self._is_speed_over_cruise and (self._is_downhill if self.downhill_only else True) + + self.just_disabled = self._active_prev and not self.active + self._active_prev = self.active + + def update_a_desired_trajectory(self, a_desired_trajectory): + if not self.active: + return a_desired_trajectory + + # Suppress all braking to allow smooth coasting + for i in range(len(a_desired_trajectory)): + if a_desired_trajectory[i] < 0 and a_desired_trajectory[i] > self.allowed_brake_val: + a_desired_trajectory[i] = 0.0 + return a_desired_trajectory + + def update_output_a_target(self, output_a_target): + if not self.active: + return output_a_target + + # Suppress braking + if output_a_target < 0 and output_a_target > self.allowed_brake_val: + output_a_target = 0.0 + return output_a_target diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 2621dd11d..8160f13fd 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -14,6 +14,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDX from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_error from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog +from dragonpilot.selfdrive.controls.lib.acm import ACM LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] @@ -27,6 +28,8 @@ _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] class DPFlags: + ACM = 1 + ACM_DOWNHILL = 2 pass def get_max_accel(v_ego): @@ -86,6 +89,7 @@ class LongitudinalPlanner: self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 + self.acm = ACM() @staticmethod def parse_model(model_msg, model_error): @@ -128,6 +132,20 @@ class LongitudinalPlanner: # PCM cruise speed may be updated a few cycles later, check if initialized reset_state = reset_state or not v_cruise_initialized + # Update ACM status + if not sm['selfdriveState'].experimentalMode: + if not self.acm.enabled and dp_flags & DPFlags.ACM: + self.acm.enabled = True + self.acm.downhill_only = dp_flags & DPFlags.ACM_DOWNHILL + else: + self.acm.enabled = False + + user_control = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled + self.acm.update_states(sm['carControl'], sm['radarState'], user_control, v_ego, v_cruise) + + if self.acm.just_disabled: + reset_state = True + # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) @@ -167,6 +185,9 @@ class LongitudinalPlanner: self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution) + # Apply ACM post-processing to the acceleration trajectory if active + self.a_desired_trajectory = self.acm.update_a_desired_trajectory(self.a_desired_trajectory) + # TODO counter is only needed because radar is glitchy, remove once radar is gone self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill if self.fcw: @@ -181,6 +202,9 @@ class LongitudinalPlanner: output_a_target, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, action_t=action_t, vEgoStopping=self.CP.vEgoStopping) + # Apply ACM to the final output acceleration target as well + output_a_target = self.acm.update_output_a_target(output_a_target) + for idx in range(2): accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 646c00669..8980822fa 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -23,6 +23,10 @@ def main(): poll='modelV2') dp_flags = 0 + if params.get_bool("dp_lon_acm"): + dp_flags |= DPFlags.ACM + if params.get_bool("dp_lon_acm_downhill"): + dp_flags |= DPFlags.ACM_DOWNHILL while True: sm.update() if sm.updated['modelV2']: diff --git a/selfdrive/ui/qt/offroad/dp_panel.cc b/selfdrive/ui/qt/offroad/dp_panel.cc index 548b02be5..e28589b58 100644 --- a/selfdrive/ui/qt/offroad/dp_panel.cc +++ b/selfdrive/ui/qt/offroad/dp_panel.cc @@ -139,6 +139,16 @@ void DPPanel::add_longitudinal_toggles() { QString::fromUtf8("🐉 ") + tr("Longitudinal Ctrl"), "", }, + { + "dp_lon_acm", + QString::fromUtf8("🚧 ") + tr("Enable Adaptive Coasting Mode (ACM)"), + tr("Adaptive Coasting Mode (ACM) reduces braking to allow smoother coasting when appropriate.\nDOES NOT WORK with Experimental Mode enabled."), + }, + { + "dp_lon_acm_downhill", + QString::fromUtf8("　") + tr("Downhill Only"), + tr("Limited to downhill driving."), + }, }; QWidget *label = nullptr; @@ -150,6 +160,9 @@ void DPPanel::add_longitudinal_toggles() { addItem(label); continue; } + if ((param == "dp_lon_acm" || param == "dp_lon_acm_downhill") && !vehicle_has_long_ctrl) { + continue; + } has_toggle = true; auto toggle = new ParamControl(param, title, desc, "", this); @@ -287,6 +300,9 @@ void DPPanel::updateStates() { } // do state change logic here + if (vehicle_has_long_ctrl) { + toggles["dp_lon_acm_downhill"]->setVisible(params.getBool("dp_lon_acm")); + } } diff --git a/system/manager/manager.py b/system/manager/manager.py index 9fe81947f..38748a212 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -41,6 +41,8 @@ def manager_init() -> None: ("OpenpilotEnabledToggle", "1"), ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)), ("DisableLogging", "0"), + ("dp_lon_acm", "0"), + ("dp_lon_acm_downhill", "0"), ] if params.get_bool("RecordFrontLock"):