From 185887db37f5f6aea8d03221fcb60547a394e4c5 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Fri, 27 Sep 2019 14:32:35 +1000 Subject: [PATCH 1/4] Make steering wheel transparent --- selfdrive/ui/ui.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 33db9f1cf..6bc42608d 100644 --- a/selfdrive/ui/ui.c +++ b/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); } From fc7bb4ea1f21a5f4c72d67224eceb2bc5df47a21 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Fri, 27 Sep 2019 14:46:12 +1000 Subject: [PATCH 2/4] disable cut steering for 2s after fault --- selfdrive/car/toyota/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 9482e24f2..d61759ee1 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/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: From d927d61f85ee42548471c62bc06f81f09e8002c1 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Fri, 27 Sep 2019 14:50:59 +1000 Subject: [PATCH 3/4] Add ability to toggle driver monitoring --- common/params.py | 1 + selfdrive/controls/lib/driver_monitor.py | 4 ++++ selfdrive/dragonpilot/dragonconf/__init__.py | 1 + 3 files changed, 6 insertions(+) diff --git a/common/params.py b/common/params.py index f1a7528ce..0d2d69cb1 100755 --- a/common/params.py +++ b/common/params.py @@ -130,6 +130,7 @@ keys = { "DragonUILead": [TxType.PERSISTENT], "DragonUIPath": [TxType.PERSISTENT], "DragonUIBlinker": [TxType.PERSISTENT], + "DragonEnableDriverMonitoring": [TxType.PERSISTENT], } diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 40f835e9a..ad595e784 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/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 diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index de42b0342..70060ffab 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', + 'DragonEnableDriverMonitoring': '1', } deprecated_conf = { From 09162309d15dbcfb9fa957018f94bb97773e8e15 Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Fri, 27 Sep 2019 15:11:29 +1000 Subject: [PATCH 4/4] Add zorrobyte's curvature learner --- selfdrive/controls/lib/curvature_learner.py | 66 +++++++++++++++++++++ selfdrive/controls/lib/pathplanner.py | 11 +++- 2 files changed, 76 insertions(+), 1 deletion(-) create mode 100644 selfdrive/controls/lib/curvature_learner.py diff --git a/selfdrive/controls/lib/curvature_learner.py b/selfdrive/controls/lib/curvature_learner.py new file mode 100644 index 000000000..507aed48c --- /dev/null +++ b/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 diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index b62d9f110..e1dad095c 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/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: