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