* Adds eco/nor/sport accel profile from @arne182\nAdds DM view

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],
 }
 

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)
 

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',
 }
 

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);
 

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);

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