better

openpilot/cereal/custom.capnp

@@ -389,6 +389,44 @@ struct CarControlSP @0xa5cd762cd951a455 {
   leadOne @2 :LeadData;
   leadTwo @3 :LeadData;
   intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
+  smartCruiseControl @5 :SmartCruiseControl;
+  speedLimit @6 :SpeedLimit;
+  laneChange @7 :LaneChange;
+  driverMonitoring @8 :DriverMonitoring;
+  leadFollowing @9 :Bool;
+
+  struct SmartCruiseControl {
+    visionState @0 :LongitudinalPlanSP.SmartCruiseControl.VisionState;
+    mapState @1 :LongitudinalPlanSP.SmartCruiseControl.MapState;
+  }
+
+  struct SpeedLimit {
+    assistState @0 :LongitudinalPlanSP.SpeedLimit.AssistState;
+    speedLimitFinalLast @1 :Float32;
+  }
+
+  struct DriverMonitoring {
+    alertLevel @0 :UInt8;
+    visionPolicy @1 :Bool;
+  }
+
+  struct LaneChange {
+    state @0 :State;
+    direction @1 :Direction;
+
+    enum State {
+      off @0;
+      preLaneChange @1;
+      laneChangeStarting @2;
+      laneChangeFinishing @3;
+    }
+
+    enum Direction {
+      none @0;
+      left @1;
+      right @2;
+    }
+  }
 
   struct Param {
     key @0 :Text;

openpilot/selfdrive/car/helpers.py

@@ -63,5 +63,9 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct
   struct_dataclass.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement(
     **remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {}))
   )
+  struct_dataclass.smartCruiseControl = structs.CarControlSP.SmartCruiseControl(**struct_dict.get('smartCruiseControl', {}))
+  struct_dataclass.speedLimit = structs.CarControlSP.SpeedLimit(**struct_dict.get('speedLimit', {}))
+  struct_dataclass.laneChange = structs.CarControlSP.LaneChange(**struct_dict.get('laneChange', {}))
+  struct_dataclass.driverMonitoring = structs.CarControlSP.DriverMonitoring(**struct_dict.get('driverMonitoring', {}))
 
   return struct_dataclass

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -57,6 +57,14 @@ COMFORT_BRAKE = 2.5
 STOP_DISTANCE = 6.0
 MIN_X_LEAD_FACTOR = 0.5
 
+# aggressive follows close to stock Hyundai SCC closest setting: 2.4 m stopped (stock 2.4), 16.3 m @ 31 mph (stock 14.8), 22 m @ 40 (20.2), 27.5 m @ 48 (25.2)
+# the MPC settles ~0.8 m short of its target at standstill, so a 3.2 m target stops at ~2.4 m
+# the short stop gap fades out by 10 m/s and moving gaps are ~0.1 s over stock: exact stock gaps crash in sim when the lead brakes 2 m/s^2 to a stop
+AGGRESSIVE_STOP_DISTANCE_BP = [0., 3., 10.]
+AGGRESSIVE_STOP_DISTANCE_V = [3.2, 3.2, STOP_DISTANCE]
+AGGRESSIVE_T_FOLLOW_BP = [0., 10., 14., 16., 18., 20., 21.3, 30.]
+AGGRESSIVE_T_FOLLOW_V = [0.74, 0.74, 0.74, 0.81, 0.90, 0.945, 1.0, 1.1]
+
 def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
   if personality==log.LongitudinalPersonality.relaxed:
     return 1.0
@@ -309,6 +317,11 @@ class LongitudinalMpc:
 
   def update(self, radarstate, personality=log.LongitudinalPersonality.standard):
     t_follow = get_T_FOLLOW(personality)
+    stop_distance_offset = 0.
+    if personality == log.LongitudinalPersonality.aggressive:
+      t_follow = float(np.interp(self.x0[1], AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V))
+      # STOP_DISTANCE is compiled into the solver, so move the lead forward instead
+      stop_distance_offset = STOP_DISTANCE - float(np.interp(self.x0[1], AGGRESSIVE_STOP_DISTANCE_BP, AGGRESSIVE_STOP_DISTANCE_V))
 
     lead_xv_0 = self.process_lead(radarstate.leadOne)
     lead_xv_1 = self.process_lead(radarstate.leadTwo)
@@ -316,8 +329,8 @@ class LongitudinalMpc:
     # 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.
-    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
-    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
+    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) + stop_distance_offset
+    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + stop_distance_offset
 
     x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle])
     self.source = MPC_SOURCES[np.argmin(x_obstacles[0])]

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -2,6 +2,7 @@
 import numpy as np
 
 import openpilot.cereal.messaging as messaging
+from openpilot.cereal import log
 from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.common.constants import CV
 from openpilot.common.filter_simple import FirstOrderFilter
@@ -20,6 +21,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import Lon
 J_CRUISE_VALS = [1.6, 1.2, 0.8, 0.6]
 J_CRUISE_BP = [0., 10.0, 25., 40.]
 A_CRUISE_MIN = -0.5
+AGGRESSIVE_LEAD_ONLY_DIST = 40.  # m
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 
 
@@ -114,6 +116,10 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
                   (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop),
                   (output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e)]
+    # aggressive: with a close radar lead, follow it like stock SCC instead of the model's larger stop gap
+    lead = sm['radarState'].leadOne
+    if sm['selfdriveState'].personality == log.LongitudinalPersonality.aggressive and lead.radar and lead.dRel < AGGRESSIVE_LEAD_ONLY_DIST:
+      candidates.pop()
 
     output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
     self.output_should_stop = any(should_stop for _, _, should_stop in candidates)

openpilot/selfdrive/controls/tests/test_following_distance.py

@@ -4,7 +4,9 @@ from openpilot.common.parameterized import parameterized_class
 
 from openpilot.cereal import log
 
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
+import numpy as np
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW, \
+  STOP_DISTANCE, AGGRESSIVE_STOP_DISTANCE_BP, AGGRESSIVE_STOP_DISTANCE_V, AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V
 from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 
 
@@ -38,7 +40,12 @@ class TestFollowingDistance(OpenpilotTestCase):
   def test_following_distance(self):
     v_lead = float(self.speed)
     simulation_steady_state = run_following_distance_simulation(v_lead, personality=self.personality)
-    correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
+    if self.personality == log.LongitudinalPersonality.aggressive:
+      t_follow = float(np.interp(v_lead, AGGRESSIVE_T_FOLLOW_BP, AGGRESSIVE_T_FOLLOW_V))
+      stop_distance = float(np.interp(v_lead, AGGRESSIVE_STOP_DISTANCE_BP, AGGRESSIVE_STOP_DISTANCE_V))
+      correct_steady_state = desired_follow_distance(v_lead, v_lead, t_follow) - (STOP_DISTANCE - stop_distance)
+    else:
+      correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
     abs_err_margin = 0.5 if v_lead > 0.0 else 1.15
     self.assertAlmostEqual(simulation_steady_state, correct_steady_state,
                            delta=0.2 * correct_steady_state + abs_err_margin)

openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -31,7 +31,7 @@ class ControlsExt(ModelStateBase):
     self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP)
     cloudlog.info("controlsd_ext got CarParamsSP")
 
-    self.sm_services_ext = ['radarState', 'selfdriveStateSP']
+    self.sm_services_ext = ['radarState', 'selfdriveStateSP', 'longitudinalPlanSP']
     self.pm_services_ext = ['carControlSP']
 
   def initialize_lateral_control(self, lac, CI, dt):
@@ -104,6 +104,31 @@ class ControlsExt(ModelStateBase):
     CC_SP.intelligentCruiseButtonManagement.sendButton = icbm_src.sendButton
     CC_SP.intelligentCruiseButtonManagement.vTarget = icbm_src.vTarget
 
+    # Smart Cruise Control state
+    scc_src = sm['longitudinalPlanSP'].smartCruiseControl
+    CC_SP.smartCruiseControl.visionState = scc_src.vision.state
+    CC_SP.smartCruiseControl.mapState = scc_src.map.state
+
+    # Speed Limit Assist state
+    sl_src = sm['longitudinalPlanSP'].speedLimit
+    CC_SP.speedLimit.assistState = sl_src.assist.state
+    CC_SP.speedLimit.speedLimitFinalLast = sl_src.resolver.speedLimitFinalLast
+
+    # Lane change state
+    meta = sm['modelV2'].meta
+    CC_SP.laneChange.state = str(meta.laneChangeState)
+    CC_SP.laneChange.direction = str(meta.laneChangeDirection)
+
+    # Driver monitoring alert
+    dm = sm['driverMonitoringState']
+    CC_SP.driverMonitoring.alertLevel = dm.alertLevel.raw
+    CC_SP.driverMonitoring.visionPolicy = dm.activePolicy == 'vision'
+
+    # Planner following a lead, the model path (e2e) wins often while following so count it when there is a lead
+    long_plan = sm['longitudinalPlan']
+    CC_SP.leadFollowing = str(long_plan.longitudinalPlanSource) in ('lead0', 'lead1', 'lead2') or \
+                          (str(long_plan.longitudinalPlanSource) == 'e2e' and long_plan.hasLead)
+
     return CC_SP
 
   @staticmethod

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -1349,17 +1349,7 @@
               "key": "TorqueBar",
               "widget": "toggle",
               "title": "Steering Arc",
-              "description": "Display steering arc on the driving screen when lateral control is enabled.",
-              "visibility": [
-                {
-                  "type": "not",
-                  "condition": {
-                    "type": "capability",
-                    "field": "device_type",
-                    "equals": "mici"
-                  }
-                }
-              ]
+              "description": "Display steering arc on the driving screen when lateral control is enabled."
             },
             {
               "key": "ShowTurnSignals",

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/visuals.yaml

@@ -20,8 +20,6 @@ sections:
     widget: toggle
     title: Steering Arc
     description: Display steering arc on the driving screen when lateral control is enabled.
-    visibility:
-    - $ref: '#/macros/hide_on_mici'
   - key: ShowTurnSignals
     widget: toggle
     title: Display Turn Signals