Merge branch 'devel-en' into devel-zht

common/params.py

@@ -130,6 +130,7 @@ keys = {
   "DragonUILead": [TxType.PERSISTENT],
   "DragonUIPath": [TxType.PERSISTENT],
   "DragonUIBlinker": [TxType.PERSISTENT],
+  "DragonEnableDriverMonitoring": [TxType.PERSISTENT],
 }
 
 

selfdrive/car/toyota/carcontroller.py

@@ -160,7 +160,7 @@ class CarController(object):
       self.last_fault_frame = frame
 
     # Cut steering for 2s after fault
-    if not enabled or (frame - self.last_fault_frame < 200):
+    if not enabled: # or (frame - self.last_fault_frame < 200):
       apply_steer = 0
       apply_steer_req = 0
     else:

selfdrive/controls/lib/curvature_learner.py

@@ -0,0 +1,66 @@
+from numpy import clip
+import pickle
+import csv
+import os
+
+# HOW TO
+# import this module to where you want to use it, such as from ```selfdrive.controls.lib.curvature_learner import CurvatureLearner```
+# create the object ```self.curvature_offset = CurvatureLearner(debug=False)```
+# call the update method ```self.curvature_offset.update(angle_steers - angle_offset, self.LP.d_poly)```
+# The learned curvature offsets will save and load automatically
+# If you still need help, check out how I have it implemented in the devel_curvaturefactorlearner branch
+# by Zorrobyte
+# version 2
+
+class CurvatureLearner:
+    def __init__(self, debug=False):
+        self.offset = 0.
+        self.learning_rate = 12000
+        self.frame = 0
+        self.debug = debug
+        try:
+            self.learned_offsets = pickle.load(open("/data/curvaturev2.p", "rb"))
+        except (OSError, IOError) as e:
+            self.learned_offsets = {
+                "center": 0.,
+                "leftinner": 0.,
+                "rightinner": 0.,
+                "leftouter": 0.,
+                "rightouter": 0.
+            }
+            pickle.dump(self.learned_offsets, open("/data/curvaturev2.p", "wb"))
+            os.chmod("/data/curvaturev2.p", 0o777)
+
+    def update(self, angle_steers=0., d_poly=None, v_ego=0.):
+        if angle_steers > 0.1:
+            if abs(angle_steers) < 2.:
+                self.learned_offsets["center"] -= d_poly[3] / self.learning_rate
+                self.offset = self.learned_offsets["center"]
+            elif 2. < abs(angle_steers) < 5.:
+                self.learned_offsets["leftinner"] -= d_poly[3] / self.learning_rate
+                self.offset = self.learned_offsets["leftinner"]
+            elif abs(angle_steers) > 5.:
+                self.learned_offsets["leftouter"] -= d_poly[3] / self.learning_rate
+                self.offset = self.learned_offsets["leftouter"]
+        elif angle_steers < -0.1:
+            if abs(angle_steers) < 2.:
+                self.learned_offsets["center"] += d_poly[3] / self.learning_rate
+                self.offset = self.learned_offsets["center"]
+            elif 2. < abs(angle_steers) < 5.:
+                self.learned_offsets["rightinner"] += d_poly[3] / self.learning_rate
+                self.offset = self.learned_offsets["rightinner"]
+            elif abs(angle_steers) > 5.:
+                self.learned_offsets["rightouter"] += d_poly[3] / self.learning_rate
+                self.offset = self.learned_offsets["rightouter"]
+
+        self.offset = clip(self.offset, -0.3, 0.3)
+        self.frame += 1
+
+        if self.frame == 12000:  # every 2 mins
+            pickle.dump(self.learned_offsets, open("/data/curvaturev2.p", "wb"))
+            self.frame = 0
+        if self.debug:
+            with open('/data/curvdebug.csv', 'a') as csv_file:
+                csv_file_writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
+                csv_file_writer.writerow([self.learned_offsets, v_ego])
+        return self.offset

selfdrive/controls/lib/driver_monitor.py

@@ -100,6 +100,7 @@ class DriverStatus():
     self.awareness_time = float(params.get("DragonSteeringMonitorTimer"))
     self.awareness_time = 86400 if self.awareness_time <= 0. else self.awareness_time * 60.
     self.dragon_enable_driver_safety_check = False if params.get("DragonEnableDriverSafetyCheck") == "0" else True
+    self.dragon_enable_driver_monitoring = False if params.get("DragonEnableDriverMonitoring") == "0" else True
 
     self._set_timers(active_monitoring=True)
 
@@ -162,6 +163,9 @@ class DriverStatus():
     self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.faceOrientation, driver_monitoring.facePosition, cal_rpy)
     self.blink.left_blink = driver_monitoring.leftBlinkProb * (driver_monitoring.leftEyeProb>_EYE_THRESHOLD)
     self.blink.right_blink = driver_monitoring.rightBlinkProb * (driver_monitoring.rightEyeProb>_EYE_THRESHOLD)
+    # we simply set to rhd to avoid driver monitoring
+    if not self.dragon_enable_driver_monitoring:
+      self.is_rhd_region = True
     self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD and not self.is_rhd_region
 
     self.driver_distracted = self._is_driver_distracted(self.pose, self.blink)>0

selfdrive/controls/lib/pathplanner.py

@@ -9,6 +9,8 @@ from selfdrive.controls.lib.lateral_mpc import libmpc_py
 from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
 from selfdrive.controls.lib.lane_planner import LanePlanner
 import selfdrive.messaging as messaging
+from selfdrive.controls.lib.curvature_learner import CurvatureLearner
+
 
 LOG_MPC = os.environ.get('LOG_MPC', False)
 
@@ -28,6 +30,7 @@ class PathPlanner(object):
     self.setup_mpc(CP.steerRateCost)
     self.solution_invalid_cnt = 0
     self.path_offset_i = 0.0
+    self.curvature_offset = CurvatureLearner(debug=False)
 
   def setup_mpc(self, steer_rate_cost):
     self.libmpc = libmpc_py.libmpc
@@ -57,7 +60,13 @@ class PathPlanner(object):
     # Run MPC
     self.angle_steers_des_prev = self.angle_steers_des_mpc
     VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
-    curvature_factor = VM.curvature_factor(v_ego)
+    # curvature_factor = VM.curvature_factor(v_ego)
+    if active:
+      curvfac = self.curvature_offset.update(angle_steers - angle_offset, self.LP.d_poly, v_ego)
+    else:
+      curvfac = 0.
+
+    curvature_factor = VM.curvature_factor(v_ego) + curvfac
 
     # TODO: Check for active, override, and saturation
     # if active:

selfdrive/dragonpilot/dragonconf/__init__.py

@@ -50,6 +50,7 @@ default_conf = {
   'DragonUILead': '1',
   'DragonUIPath': '1',
   'DragonUIBlinker': '0',
+  'DragonEnableDriverMonitoring': '1',
 }
 
 deprecated_conf = {

selfdrive/ui/ui.c

@@ -1377,7 +1377,7 @@ static void ui_draw_vision_event(UIState *s) {
     const int img_turn_size = 160*1.5;
     const int img_turn_x = viz_event_x-(img_turn_size/4);
     const int img_turn_y = viz_event_y+bdr_s-25;
-    float img_turn_alpha = 1.0f;
+    float img_turn_alpha = 0.7f;
     nvgBeginPath(s->vg);
     NVGpaint imgPaint = nvgImagePattern(s->vg, img_turn_x, img_turn_y,
       img_turn_size, img_turn_size, 0, s->img_turn, img_turn_alpha);
@@ -1407,7 +1407,7 @@ static void ui_draw_vision_event(UIState *s) {
         nvgFillColor(s->vg, nvgRGBA(23, 51, 73, 180));
       }
       nvgFill(s->vg);
-      img_wheel_alpha = 1.0f;
+      img_wheel_alpha = 0.7f;
     }
     nvgBeginPath(s->vg);
     NVGpaint imgPaint = nvgImagePattern(s->vg, img_wheel_x, img_wheel_y,
@@ -1578,12 +1578,12 @@ static void ui_draw_infobar(UIState *s) {
 
   nvgBeginPath(s->vg);
   nvgRoundedRect(s->vg, rect_x, rect_y, rect_w, rect_h, 15);
-  nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 150));
+  nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 180));
   nvgFill(s->vg);
 
   nvgFontSize(s->vg, hasSidebar? 40:50);
   nvgFontFace(s->vg, "courbd");
-  nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 175));
+  nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 180));
   nvgTextAlign(s->vg, NVG_ALIGN_CENTER);
   nvgText(s->vg, text_x, text_y, infobar, NULL);
 }