diff --git a/common/params.py b/common/params.py index d1e939a46..111810ef6 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], + "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/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])) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 0b709dde8..1fe641d21 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -22,33 +22,51 @@ 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.] +_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 = [-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.] +_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): - a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) +def calc_cruise_accel_limits(v_ego, following, accel_profile): + 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: - a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) + 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 @@ -89,6 +107,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): @@ -125,8 +146,11 @@ 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_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 @@ -160,7 +184,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_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 87fb015c3..020864dd7 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, + 'DragonAccelProfile': '0', } deprecated_conf = { diff --git a/selfdrive/dragonpilot/shutdownd/shutdownd.py b/selfdrive/dragonpilot/shutdownd/shutdownd.py index fe1d9e815..a467415de 100644 --- a/selfdrive/dragonpilot/shutdownd/shutdownd.py +++ b/selfdrive/dragonpilot/shutdownd/shutdownd.py @@ -4,30 +4,35 @@ 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 last_shutdown_val == auto_shutdown_at: + shutdown_count = 0 + last_shutdown_val = auto_shutdown_at - 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() + if not started and not usb_online: shutdown_count += 1 else: shutdown_count = 0 - 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() else: 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