Custom UI

Added toggles to customize the lane lines, path, road edges, path edges, show the acceleration/deceleration on the path, lead info, driving logics, adjacent lanes, blind spot path, fps tracker, and an "Unlimited Length" mode that extends the road UI out as far as the model can see.

selfdrive/controls/lib/desire_helper.py

@@ -2,6 +2,8 @@ from cereal import log
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.realtime import DT_MDL
 
+from openpilot.selfdrive.frogpilot.functions.frogpilot_planner import calculate_lane_width
+
 LaneChangeState = log.LateralPlan.LaneChangeState
 LaneChangeDirection = log.LateralPlan.LaneChangeDirection
 
@@ -47,6 +49,16 @@ class DesireHelper:
     one_blinker = carstate.leftBlinker != carstate.rightBlinker
     below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
 
+    # Calculate left and right lane widths for the blindspot path
+    turning = abs(carstate.steeringAngleDeg) >= 60
+    if frogpilot_planner.blindspot_path and not below_lane_change_speed and not turning:
+      # Calculate left and right lane widths
+      self.lane_width_left = calculate_lane_width(modeldata.laneLines[0], modeldata.laneLines[1], modeldata.roadEdges[0])
+      self.lane_width_right = calculate_lane_width(modeldata.laneLines[3], modeldata.laneLines[2], modeldata.roadEdges[1])
+    else:
+      self.lane_width_left = 0
+      self.lane_width_right = 0
+
     if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
       self.lane_change_state = LaneChangeState.off
       self.lane_change_direction = LaneChangeDirection.none

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -366,6 +366,16 @@ class LongitudinalMpc:
       t_follow_offset = np.clip((lead_xv_0[:,1] - v_ego) + (STOP_DISTANCE - v_ego), 1, distance_factor)
       t_follow = t_follow / t_follow_offset
 
+    # LongitudinalPlan variables for onroad driving insights
+    if self.status:
+      self.safe_obstacle_distance = int(np.mean(get_safe_obstacle_distance(v_ego, t_follow)))
+      self.safe_obstacle_distance_stock = int(np.mean(get_safe_obstacle_distance(v_ego, self.t_follow)))
+      self.stopped_equivalence_factor = int(np.mean(get_stopped_equivalence_factor(lead_xv_0[:,1])))
+    else:
+      self.safe_obstacle_distance = 0
+      self.safe_obstacle_distance_stock = 0
+      self.stopped_equivalence_factor = 0
+
     # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance
     # and then treat that as a stopped car/obstacle at this new distance.