From ed9c94d277f62d00a367e994cef6225a8a164d4c Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Fri, 28 Feb 2020 13:23:23 +1000 Subject: [PATCH 1/6] shutdownd to take car start stats into account. --- selfdrive/dragonpilot/shutdownd/shutdownd.py | 25 +++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/selfdrive/dragonpilot/shutdownd/shutdownd.py b/selfdrive/dragonpilot/shutdownd/shutdownd.py index fe1d9e815..cc0b1bf45 100644 --- a/selfdrive/dragonpilot/shutdownd/shutdownd.py +++ b/selfdrive/dragonpilot/shutdownd/shutdownd.py @@ -4,25 +4,32 @@ import os import time from common.params import Params params = Params() +import cereal +import cereal.messaging as messaging + def main(gctx=None): shutdown_count = 0 auto_shutdown_at = get_shutdown_val() frame = 0 - last_shutdown_val = get_shutdown_val() + last_shutdown_val = auto_shutdown_at + thermal_sock = messaging.sub_sock('thermal') + started = False while 1: - with open("/sys/class/power_supply/usb/present") as f: - usb_online = bool(int(f.read())) + if frame % 5 == 0: + msg = messaging.recv_sock(thermal_sock, wait=True) + started = msg.thermal.started + with open("/sys/class/power_supply/usb/present") as f: + usb_online = bool(int(f.read())) + auto_shutdown_at = get_shutdown_val() - if not usb_online: - # we update the value every 5 seconds in case of user updates it - if frame % 5 == 0: - auto_shutdown_at = get_shutdown_val() - shutdown_count += 1 - else: + if started or usb_online: shutdown_count = 0 + else: + if not usb_online: + shutdown_count += 1 if not last_shutdown_val == auto_shutdown_at: shutdown_count = 0 From 3dbe299c8642aeb738499823d82e877557c741be Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Fri, 28 Feb 2020 13:25:57 +1000 Subject: [PATCH 2/6] add fast acceleration toggle/profile. (thanks to @arne182) --- common/params.py | 1 + selfdrive/controls/lib/planner.py | 24 ++++++++++++++++---- selfdrive/dragonpilot/dragonconf/__init__.py | 1 + 3 files changed, 21 insertions(+), 5 deletions(-) diff --git a/common/params.py b/common/params.py index d1e939a46..cb8bc70a4 100755 --- a/common/params.py +++ b/common/params.py @@ -165,6 +165,7 @@ keys = { "DragonAutoLCDelay": [TxType.PERSISTENT], "DragonBTG": [TxType.PERSISTENT], "DragonBootHotspot": [TxType.PERSISTENT], + "DragonEnableFastAccel": [TxType.PERSISTENT], } diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 0b709dde8..6c491b201 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -25,12 +25,18 @@ AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distract _A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30] _A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.] +# dragonpilot +_A_CRUISE_MIN_V_SPORT = [-3.0, -3.5, -4.0, -4.0, -4.0] + # need fast accel at very low speed for stop and go # make sure these accelerations are smaller than mpc limits _A_CRUISE_MAX_V = [1.2, 1.2, 0.65, .4] _A_CRUISE_MAX_V_FOLLOWING = [1.6, 1.6, 0.65, .4] _A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.] +# dragonpilot +_A_CRUISE_MAX_V_SPORT = [3.0, 3.5, 4.0, 4.0, 4.0] + # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] @@ -39,13 +45,19 @@ _A_TOTAL_MAX_BP = [20., 40.] SPEED_PERCENTILE_IDX = 7 -def calc_cruise_accel_limits(v_ego, following): - a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) +def calc_cruise_accel_limits(v_ego, following, fast_accel): + if not following and fast_accel: + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_SPORT) + else: + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) if following: a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING) else: - a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) + if fast_accel: + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_SPORT) + else: + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) return np.vstack([a_cruise_min, a_cruise_max]) @@ -89,6 +101,7 @@ class Planner(): # dragonpilot self.dragon_slow_on_curve = True + self.dragon_fast_accel = False self.last_ts = 0. def choose_solution(self, v_cruise_setpoint, enabled): @@ -125,8 +138,9 @@ class Planner(): # dragonpilot # update variable status every 5 secs - if cur_time - self.last_ts > 5.: + if cur_time - self.last_ts >= 5.: self.dragon_slow_on_curve = False if self.params.get("DragonEnableSlowOnCurve", encoding='utf8') == "0" else True + self.dragon_fast_accel = True if self.params.get("DragonEnableFastAccel", encoding='utf8') == "1" else False self.last_ts = cur_time long_control_state = sm['controlsState'].longControlState @@ -160,7 +174,7 @@ class Planner(): # 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)] + accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following, self.dragon_fast_accel)] 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) diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 87fb015c3..368ea2a08 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -64,6 +64,7 @@ default_conf = { 'DragonAutoLCDelay': 2, 'DragonBTG': 0, 'DragonBootHotspot': 0, + 'DragonEnableFastAccel': '0', } deprecated_conf = { From 4e4248379a3c192ce654204f0f25daabeb60c8c4 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Fri, 28 Feb 2020 15:40:21 +1000 Subject: [PATCH 3/6] update shutdownd logic --- selfdrive/dragonpilot/shutdownd/shutdownd.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/selfdrive/dragonpilot/shutdownd/shutdownd.py b/selfdrive/dragonpilot/shutdownd/shutdownd.py index cc0b1bf45..a467415de 100644 --- a/selfdrive/dragonpilot/shutdownd/shutdownd.py +++ b/selfdrive/dragonpilot/shutdownd/shutdownd.py @@ -24,16 +24,14 @@ def main(gctx=None): with open("/sys/class/power_supply/usb/present") as f: usb_online = bool(int(f.read())) auto_shutdown_at = get_shutdown_val() + if not last_shutdown_val == auto_shutdown_at: + shutdown_count = 0 + last_shutdown_val = auto_shutdown_at - if started or usb_online: - shutdown_count = 0 + if not started and not usb_online: + shutdown_count += 1 else: - if not usb_online: - shutdown_count += 1 - - if not last_shutdown_val == auto_shutdown_at: shutdown_count = 0 - last_shutdown_val = auto_shutdown_at if auto_shutdown_at is None: auto_shutdown_at = get_shutdown_val() From 9f5c6ae26e7a1d3076f09f0c3d36c18df89d8845 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Mon, 2 Mar 2020 14:11:44 +1000 Subject: [PATCH 4/6] hopefully reduce the cause of cruiseDisabled alerts if user flip the steering control?) --- selfdrive/car/honda/interface.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index fae6486d5..e43050514 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -554,8 +554,8 @@ class CarInterface(CarInterfaceBase): if self.CP.enableCruise and not ret.cruiseState.enabled and (c.actuators.brake <= 0. or not self.CP.openpilotLongitudinalControl): # non loud alert if cruise disbales below 25mph as expected (+ a little margin) if ret.vEgo < self.CP.minEnableSpeed + 2.: - events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) - else: +# events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) +# else: events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE])) if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001: events.append(create_event('manualRestart', [ET.WARNING])) From bfc25a5e95f992c63e3da9c983349d5307e8c7fb Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Mon, 2 Mar 2020 17:55:06 +1000 Subject: [PATCH 5/6] * Adds eco/nor/sport accel profile from @arne182\nAdds DM view --- common/params.py | 2 + selfdrive/controls/lib/planner.py | 57 +++++++++++++------- selfdrive/dragonpilot/dragonconf/__init__.py | 2 + selfdrive/ui/paint.cc | 29 ++++++++++ selfdrive/ui/ui.cc | 5 ++ selfdrive/ui/ui.hpp | 2 + 6 files changed, 78 insertions(+), 19 deletions(-) diff --git a/common/params.py b/common/params.py index cb8bc70a4..845905024 100755 --- a/common/params.py +++ b/common/params.py @@ -151,6 +151,7 @@ keys = { "DragonUILead": [TxType.PERSISTENT], "DragonUIPath": [TxType.PERSISTENT], "DragonUIBlinker": [TxType.PERSISTENT], + "DragonUIDMView": [TxType.PERSISTENT], "DragonEnableDriverMonitoring": [TxType.PERSISTENT], "DragonCarModel": [TxType.PERSISTENT], "DragonEnableSlowOnCurve": [TxType.PERSISTENT], @@ -165,6 +166,7 @@ keys = { "DragonAutoLCDelay": [TxType.PERSISTENT], "DragonBTG": [TxType.PERSISTENT], "DragonBootHotspot": [TxType.PERSISTENT], + "DragonEnableAltAccelProfile": [TxType.PERSISTENT], "DragonEnableFastAccel": [TxType.PERSISTENT], } diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 6c491b201..b8c7fe719 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -22,45 +22,54 @@ AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distract # lookup tables VS speed to determine min and max accels in cruise # make sure these accelerations are smaller than mpc limits -_A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30] -_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.] - -# dragonpilot -_A_CRUISE_MIN_V_SPORT = [-3.0, -3.5, -4.0, -4.0, -4.0] +_A_CRUISE_MIN_V_ECO = [-1.0, -0.7, -0.6, -0.5, -0.3] +_A_CRUISE_MIN_V_SPORT = [-3.0, -2.6, -2.3, -2.0, -1.0] +_A_CRUISE_MIN_V_FOLLOWING = [-4.0, -4.0, -3.5, -2.5, -2.0] +_A_CRUISE_MIN_V = [-2.0, -1.5, -1.0, -0.7, -0.5] +_A_CRUISE_MIN_BP = [0.0, 5.0, 10.0, 20.0, 55.0] # need fast accel at very low speed for stop and go # make sure these accelerations are smaller than mpc limits -_A_CRUISE_MAX_V = [1.2, 1.2, 0.65, .4] -_A_CRUISE_MAX_V_FOLLOWING = [1.6, 1.6, 0.65, .4] -_A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.] - -# dragonpilot +_A_CRUISE_MAX_V = [2.0, 2.0, 1.5, .5, .3] +_A_CRUISE_MAX_V_ECO = [1.0, 1.5, 1.0, 0.3, 0.1] _A_CRUISE_MAX_V_SPORT = [3.0, 3.5, 4.0, 4.0, 4.0] +_A_CRUISE_MAX_V_FOLLOWING = [1.3, 1.6, 1.2, .7, .3] +_A_CRUISE_MAX_BP = [0., 5., 10., 20., 55.] # Lookup table for turns -_A_TOTAL_MAX_V = [1.7, 3.2] -_A_TOTAL_MAX_BP = [20., 40.] +_A_TOTAL_MAX_V = [3.3, 3.0, 3.9] +_A_TOTAL_MAX_BP = [0., 25., 55.] # 75th percentile SPEED_PERCENTILE_IDX = 7 +# dragonpilot, accel profiles +ACCEL_ECO_MODE = -1 +ACCEL_NORMAL_MODE = 0 +ACCEL_SPORT_MODE = 1 -def calc_cruise_accel_limits(v_ego, following, fast_accel): - if not following and fast_accel: - a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_SPORT) +def calc_cruise_accel_limits(v_ego, following, accel_profile): + if following: + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_FOLLOWING) else: - a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) + if accel_profile == ACCEL_ECO_MODE: + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_ECO) + elif accel_profile == ACCEL_SPORT_MODE: + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_SPORT) + else: + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) if following: a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING) else: - if fast_accel: + if accel_profile == ACCEL_ECO_MODE: + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_ECO) + elif accel_profile == ACCEL_SPORT_MODE: a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_SPORT) else: a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) return np.vstack([a_cruise_min, a_cruise_max]) - def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): """ This function returns a limited long acceleration allowed, depending on the existing lateral acceleration @@ -101,7 +110,9 @@ class Planner(): # dragonpilot self.dragon_slow_on_curve = True + self.dragon_alt_accel_profile = False self.dragon_fast_accel = False + self.dragon_accel_profile = ACCEL_NORMAL_MODE self.last_ts = 0. def choose_solution(self, v_cruise_setpoint, enabled): @@ -140,7 +151,15 @@ class Planner(): # update variable status every 5 secs if cur_time - self.last_ts >= 5.: self.dragon_slow_on_curve = False if self.params.get("DragonEnableSlowOnCurve", encoding='utf8') == "0" else True + self.dragon_alt_accel_profile = True if self.params.get("DragonEnableAltAccelProfile", encoding='utf8') == "1" else False self.dragon_fast_accel = True if self.params.get("DragonEnableFastAccel", encoding='utf8') == "1" else False + if self.dragon_accel_profile: + if self.dragon_fast_accel: + self.dragon_accel_profile = ACCEL_SPORT_MODE + else: + self.dragon_accel_profile = ACCEL_ECO_MODE + else: + self.dragon_accel_profile = ACCEL_NORMAL_MODE self.last_ts = cur_time long_control_state = sm['controlsState'].longControlState @@ -174,7 +193,7 @@ class Planner(): # 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_fast_accel)] + 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) diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 368ea2a08..0f9b9d307 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -50,6 +50,7 @@ default_conf = { 'DragonUILead': '1', 'DragonUIPath': '1', 'DragonUIBlinker': '0', + 'DragonUIDMView': '0', 'DragonEnableDriverMonitoring': '1', 'DragonCarModel': '', 'DragonEnableSlowOnCurve': '1', @@ -64,6 +65,7 @@ default_conf = { 'DragonAutoLCDelay': 2, 'DragonBTG': 0, 'DragonBootHotspot': 0, + 'DragonEnableAltAccelProfile': '0', 'DragonEnableFastAccel': '0', } diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 8eb539456..c3c0d35c0 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -264,6 +264,30 @@ static void draw_steering(UIState *s, float curvature) { // ui_draw_lane_edge(s, points, 0.0, nvgRGBA(0, 0, 255, 128), 5); } +static void draw_front_frame(UIState *s) { + const UIScene *scene = &s->scene; + + float x1, x2, y1, y2; + glBindVertexArray(s->frame_vao[1]); + + mat4 *out_mat; + out_mat = &s->front_frame_mat; + glActiveTexture(GL_TEXTURE0); + if (s->cur_vision_front_idx >= 0) { + glBindTexture(GL_TEXTURE_2D, s->frame_front_texs[s->cur_vision_front_idx]); + } + + glUseProgram(s->frame_program); + glUniform1i(s->frame_texture_loc, 0); + glUniformMatrix4fv(s->frame_transform_loc, 1, GL_TRUE, out_mat->v); + + assert(glGetError() == GL_NO_ERROR); + glEnableVertexAttribArray(0); + glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void*)0); + glDisableVertexAttribArray(0); + glBindVertexArray(0); +} + static void draw_frame(UIState *s) { const UIScene *scene = &s->scene; @@ -1158,6 +1182,11 @@ static void ui_draw_vision(UIState *s) { if (s->dragon_driving_ui) { draw_frame(s); } + if (s->dragon_ui_dm_view) { + glViewport(1240, 110, viz_w*0.4, box_h*0.4); + draw_front_frame(s); + glClear(GL_STENCIL_BUFFER_BIT); + } glViewport(0, 0, s->fb_w, s->fb_h); glDisable(GL_SCISSOR_TEST); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 75842c08d..2a7fccd1f 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -214,6 +214,7 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, s->dragon_ui_lead = false; s->dragon_ui_path = false; s->dragon_ui_blinker = false; + s->dragon_ui_dm_view = false; } else { read_param_bool(&s->dragon_ui_speed, "DragonUISpeed"); read_param_bool(&s->dragon_ui_event, "DragonUIEvent"); @@ -227,6 +228,7 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, read_param_bool(&s->dragon_ui_lead, "DragonUILead"); read_param_bool(&s->dragon_ui_path, "DragonUIPath"); read_param_bool(&s->dragon_ui_blinker, "DragonUIBlinker"); + read_param_bool(&s->dragon_ui_dm_view, "DragonUIDMView"); } @@ -250,6 +252,7 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, s->dragon_ui_path_timeout = UI_FREQ * 4.2; s->dragon_ui_blinker_timeout = UI_FREQ * 4.3; s->dragon_waze_mode_timeout = UI_FREQ * 4.4; + s->dragon_ui_dm_view_timeout = UI_FREQ * 4.5; } static PathData read_path(cereal_ModelData_PathData_ptr pathp) { @@ -1017,6 +1020,7 @@ int main(int argc, char* argv[]) { s->dragon_ui_lead = false; s->dragon_ui_path = false; s->dragon_ui_blinker = false; + s->dragon_ui_dm_view = false; } else { read_param_bool_timeout(&s->dragon_ui_speed, "DragonUISpeed", &s->dragon_ui_speed_timeout); read_param_bool_timeout(&s->dragon_ui_event, "DragonUIEvent", &s->dragon_ui_event_timeout); @@ -1030,6 +1034,7 @@ int main(int argc, char* argv[]) { read_param_bool_timeout(&s->dragon_ui_lead, "DragonUILead", &s->dragon_ui_lead_timeout); read_param_bool_timeout(&s->dragon_ui_path, "DragonUIPath", &s->dragon_ui_path_timeout); read_param_bool_timeout(&s->dragon_ui_blinker, "DragonUIBlinker", &s->dragon_ui_blinker_timeout); + read_param_bool_timeout(&s->dragon_ui_dm_view, "DragonUIDMView", &s->dragon_ui_dm_view_timeout); } pthread_mutex_unlock(&s->lock); diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index ad56ecdaa..7d885cf60 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -274,6 +274,7 @@ typedef struct UIState { int dragon_ui_path_timeout; int dragon_ui_blinker_timeout; int dragon_waze_mode_timeout; + int dragon_ui_dm_view_timeout; bool dragon_ui_speed; bool dragon_ui_event; @@ -289,6 +290,7 @@ typedef struct UIState { bool dragon_ui_path; bool dragon_ui_blinker; bool dragon_waze_mode; + bool dragon_ui_dm_view; } UIState; // API From fcd678ade94f2e2eb348a2c74110202108862c29 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Wed, 4 Mar 2020 13:04:15 +1000 Subject: [PATCH 6/6] include accel profile UI, adjustment to increase accel allowance in toyota --- common/params.py | 3 +-- panda/board/safety/safety_toyota.h | 2 +- selfdrive/controls/lib/planner.py | 27 +++++++------------- selfdrive/dragonpilot/dragonconf/__init__.py | 3 +-- 4 files changed, 12 insertions(+), 23 deletions(-) diff --git a/common/params.py b/common/params.py index 845905024..111810ef6 100755 --- a/common/params.py +++ b/common/params.py @@ -166,8 +166,7 @@ keys = { "DragonAutoLCDelay": [TxType.PERSISTENT], "DragonBTG": [TxType.PERSISTENT], "DragonBootHotspot": [TxType.PERSISTENT], - "DragonEnableAltAccelProfile": [TxType.PERSISTENT], - "DragonEnableFastAccel": [TxType.PERSISTENT], + "DragonAccelProfile": [TxType.PERSISTENT], } diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index e8ca94e65..9b06a7cd0 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -13,7 +13,7 @@ const int TOYOTA_MAX_RT_DELTA = 375; // max delta torque allowed for real t const uint32_t TOYOTA_RT_INTERVAL = 250000; // 250ms between real time checks // longitudinal limits -const int TOYOTA_MAX_ACCEL = 1500; // 1.5 m/s2 +const int TOYOTA_MAX_ACCEL = 4000; // 1.5 m/s2 const int TOYOTA_MIN_ACCEL = -3000; // 3.0 m/s2 const int TOYOTA_GAS_INTERCEPTOR_THRESHOLD = 475; // ratio between offset and gain from dbc file diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index b8c7fe719..1fe641d21 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -24,7 +24,7 @@ AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distract # make sure these accelerations are smaller than mpc limits _A_CRUISE_MIN_V_ECO = [-1.0, -0.7, -0.6, -0.5, -0.3] _A_CRUISE_MIN_V_SPORT = [-3.0, -2.6, -2.3, -2.0, -1.0] -_A_CRUISE_MIN_V_FOLLOWING = [-4.0, -4.0, -3.5, -2.5, -2.0] + _A_CRUISE_MIN_V = [-2.0, -1.5, -1.0, -0.7, -0.5] _A_CRUISE_MIN_BP = [0.0, 5.0, 10.0, 20.0, 55.0] @@ -49,15 +49,12 @@ ACCEL_NORMAL_MODE = 0 ACCEL_SPORT_MODE = 1 def calc_cruise_accel_limits(v_ego, following, accel_profile): - if following: - a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_FOLLOWING) + if accel_profile == ACCEL_ECO_MODE: + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_ECO) + elif accel_profile == ACCEL_SPORT_MODE: + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_SPORT) else: - if accel_profile == ACCEL_ECO_MODE: - a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_ECO) - elif accel_profile == ACCEL_SPORT_MODE: - a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_SPORT) - else: - a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) if following: a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING) @@ -151,15 +148,9 @@ class Planner(): # update variable status every 5 secs if cur_time - self.last_ts >= 5.: self.dragon_slow_on_curve = False if self.params.get("DragonEnableSlowOnCurve", encoding='utf8') == "0" else True - self.dragon_alt_accel_profile = True if self.params.get("DragonEnableAltAccelProfile", encoding='utf8') == "1" else False - self.dragon_fast_accel = True if self.params.get("DragonEnableFastAccel", encoding='utf8') == "1" else False - if self.dragon_accel_profile: - if self.dragon_fast_accel: - self.dragon_accel_profile = ACCEL_SPORT_MODE - else: - self.dragon_accel_profile = ACCEL_ECO_MODE - else: - self.dragon_accel_profile = ACCEL_NORMAL_MODE + self.dragon_accel_profile = int(self.params.get("DragonAccelProfile", encoding='utf8')) + if self.dragon_accel_profile >= 2 or self.dragon_accel_profile <= -2: + self.dragon_accel_profile = 0 self.last_ts = cur_time long_control_state = sm['controlsState'].longControlState diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 0f9b9d307..020864dd7 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -65,8 +65,7 @@ default_conf = { 'DragonAutoLCDelay': 2, 'DragonBTG': 0, 'DragonBootHotspot': 0, - 'DragonEnableAltAccelProfile': '0', - 'DragonEnableFastAccel': '0', + 'DragonAccelProfile': '0', } deprecated_conf = {