Merge pull request #15 from dragonpilot-community/devel

dp 0.7.3.1

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

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

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

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)
 

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 = {

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:

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