Merge branch 'hkg-can-jerk' into lt

# Conflicts:
#	selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
#	selfdrive/controls/lib/longitudinal_planner.py

cereal/custom.capnp

@@ -16,6 +16,7 @@ enum LongitudinalPersonalitySP {
   moderate @1;
   standard @2;
   relaxed @3;
+  overtake @4;
 }
 
 enum AccelerationPersonality {
@@ -44,6 +45,7 @@ struct ControlsStateSP @0x81c2f05a394cf4af {
   personality @8 :LongitudinalPersonalitySP;
   dynamicPersonality @9 :Bool;
   accelPersonality @10 :AccelerationPersonality;
+  overtakingAccelerationAssist @11 :Bool;
 
   lateralControlState :union {
     indiState @1 :LateralINDIState;

common/params.cc

@@ -294,6 +294,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"OsmLocationUrl", PERSISTENT},
     {"OsmWayTest", PERSISTENT},
     {"OsmDownloadedDate", PERSISTENT},
+    {"OvertakingAccelerationAssist", PERSISTENT},
     {"PathOffset", PERSISTENT | BACKUP},
     {"PauseLateralSpeed", PERSISTENT | BACKUP},
     {"PCMVCruiseOverride", PERSISTENT | BACKUP},

selfdrive/car/hyundai/interface.py

@@ -102,15 +102,15 @@ class CarInterface(CarInterfaceBase):
     ret.pcmCruise = not ret.openpilotLongitudinalControl
 
     ret.stoppingControl = True
-    ret.startingState = True
     ret.vEgoStarting = 0.5
+    ret.startAccel = 1.0
     ret.stopAccel = -1.0
     ret.longitudinalActuatorDelay = 0.5
 
     if ret.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV):
-      ret.startAccel = 1.0
+      ret.startingState = False
     else:
-      ret.startAccel = 1.5
+      ret.startingState = True
 
     if DBC[ret.carFingerprint]["radar"] is None:
       if ret.spFlags & (HyundaiFlagsSP.SP_ENHANCED_SCC | HyundaiFlagsSP.SP_CAMERA_SCC_LEAD):

selfdrive/controls/controlsd.py

@@ -188,6 +188,7 @@ class Controls:
                                      self.custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution
 
     self.dynamic_personality = self.params.get_bool("DynamicPersonality")
+    self.overtaking_accel = self.params.get_bool("OvertakingAccelerationAssist")
 
     self.accel_personality = self.read_accel_personality_param()
 
@@ -816,11 +817,21 @@ class Controls:
 
     # Curvature & Steering angle
     lp = self.sm['liveParameters']
-    lp_mono_time_svs = 'lateralPlanDEPRECATED' if self.model_use_lateral_planner else 'modelV2'
+    dh = 'lateralPlanDEPRECATED' if self.model_use_lateral_planner else 'modelV2'
 
     steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
     curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)
 
+    lc_svs = self.sm[dh]
+    long_plan = self.sm['longitudinalPlan'].hasLead
+    dm_state = self.sm['driverMonitoringState']
+    overtaking_accel_allowed = ((lc_svs.laneChangeDirection == LaneChangeDirection.right and dm_state.isRHD) or
+                                (lc_svs.laneChangeDirection == LaneChangeDirection.left and not dm_state.isRHD)) and \
+                               (lc_svs.laneChangeState in (LaneChangeState.preLaneChange, LaneChangeState.laneChangeStarting))
+    overtaking_accel_engaged = self.overtaking_accel and overtaking_accel_allowed and \
+                               CS.vEgo > (60 * CV.KPH_TO_MS) if self.is_metric else (40 * CV.MPH_TO_MS) and long_plan.hasLead and \
+                               long_plan.aTarget > -0.2 and not (CS.leftBlinker and CS.rightBlinker)
+
     # controlsState
     dat = messaging.new_message('controlsState')
     dat.valid = CS.canValid
@@ -835,7 +846,7 @@ class Controls:
       controlsState.alertSound = current_alert.audible_alert
 
     controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
-    controlsState.lateralPlanMonoTime = self.sm.logMonoTime[lp_mono_time_svs]
+    controlsState.lateralPlanMonoTime = self.sm.logMonoTime[dh]
     controlsState.enabled = not (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) and (self.enabled or CS.cruiseState.enabled) and CS.gearShifter not in [GearShifter.park, GearShifter.reverse]
     controlsState.active = not (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) and (self.active or CS.cruiseState.enabled)
     controlsState.curvature = curvature
@@ -873,6 +884,7 @@ class Controls:
     controlsStateSP.personality = self.personality
     controlsStateSP.dynamicPersonality = self.dynamic_personality
     controlsStateSP.accelPersonality = self.accel_personality
+    controlsStateSP.overtakingAccelerationAssist = overtaking_accel_engaged
 
     if self.enable_nnff and lat_tuning == 'torque':
       controlsStateSP.lateralControlState.torqueState = self.LaC.pid_long_sp
@@ -940,6 +952,7 @@ class Controls:
 
       self.reverse_acc_change = self.params.get_bool("ReverseAccChange")
       self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
+      self.overtaking_accel = self.params.get_bool("OvertakingAccelerationAssist")
 
       if self.sm.frame % int(2.5 / DT_CTRL) == 0:
         self.live_torque = self.params.get_bool("LiveTorque")

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -66,6 +66,8 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
     return 0.8
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
     return 0.65
+  elif personality==custom.LongitudinalPersonalitySP.overtake:
+    return 0.1
   else:
     raise NotImplementedError("Longitudinal personality not supported")
 
@@ -79,6 +81,8 @@ def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard):
     return 1.25
   elif personality==custom.LongitudinalPersonalitySP.aggressive:
     return 1.0
+  elif personality==custom.LongitudinalPersonalitySP.overtake:
+    return 0.5
   else:
     raise NotImplementedError("Longitudinal personality not supported")
 
@@ -357,9 +361,11 @@ class LongitudinalMpc:
     self.cruise_min_a = min_a
     self.max_a = max_a
 
-  def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard, dynamic_personality=False):
+  def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard,
+             dynamic_personality=False, overtaking_acceleration_assist=False):
     v_ego = self.x0[1]
     t_follow = get_dynamic_personality(v_ego, personality) if dynamic_personality else get_T_FOLLOW(personality)
+    t_follow = get_T_FOLLOW(custom.LongitudinalPersonalitySP.overtake) if overtaking_acceleration_assist else t_follow
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
     lead_xv_0 = self.process_lead(radarstate.leadOne)

selfdrive/controls/lib/longitudinal_planner.py

@@ -3,7 +3,7 @@ import math
 import numpy as np
 from openpilot.common.numpy_fast import clip, interp
 from openpilot.common.params import Params
-from cereal import car, custom
+from cereal import car, log, custom
 
 import cereal.messaging as messaging
 from openpilot.common.conversions import Conversions as CV
@@ -83,7 +83,7 @@ class LongitudinalPlanner:
     self.dt = dt
 
     self.a_desired = init_a
-    v_ego_sec = 0.5 if CP.carName == "hyundai" else 2.0
+    v_ego_sec = 0.6 if CP.carName == "hyundai" else 2.0
     self.v_desired_filter = FirstOrderFilter(init_v, v_ego_sec, self.dt)
     self.v_model_error = 0.0
 
@@ -194,11 +194,13 @@ class LongitudinalPlanner:
     accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
     accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
 
-    self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsStateSP'].personality)
+    overtaking_accel_engaged = sm['controlsStateSP'].overtakingAccelerationAssist
+    self.mpc.set_weights(prev_accel_constraint, personality=custom.LongitudinalPersonalitySP.overtake if overtaking_accel_engaged else sm['controlsStateSP'].personality)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
-    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsStateSP'].personality, dynamic_personality=sm['controlsStateSP'].dynamicPersonality)
+    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsStateSP'].personality,
+                    dynamic_personality=sm['controlsStateSP'].dynamicPersonality, overtaking_acceleration_assist=overtaking_accel_engaged)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)

selfdrive/controls/plannerd.py

@@ -32,7 +32,7 @@ def plannerd_thread():
   pm = messaging.PubMaster(['longitudinalPlan', 'longitudinalPlanSP'] + lateral_planner_svs)
   sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2',
                             'longitudinalPlan', 'navInstruction', 'longitudinalPlanSP',
-                            'liveMapDataSP', 'e2eLongStateSP', 'controlsStateSP'] + lateral_planner_svs,
+                            'liveMapDataSP', 'e2eLongStateSP', 'controlsStateSP', 'driverMonitoringState'] + lateral_planner_svs,
                            poll='modelV2', ignore_avg_freq=['radarState'])
 
   while True:

selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_settings.cc

@@ -49,6 +49,12 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) {
       tr("While in Auto Lane, switch to Laneless for current/future curves."),
       "../assets/offroad/icon_blank.png",
     },
+    {
+      "OvertakingAccelerationAssist",
+      tr("Overtaking Acceleration Assist"),
+      tr("Overtaking Acceleration Assist will operate when the turn signal indicator is turned on to the left (left-hand drive) or turned on to the right (right-hand drive) while Smart Cruise Control is operating."),
+      "../assets/offroad/icon_blank.png",
+    },
     {
       "EnableSlc",
       tr("Speed Limit Control (SLC)"),
@@ -310,7 +316,7 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) {
       list->addItem(dlp_settings);
     }
 
-    if (param == "VisionCurveLaneless") {
+    if (param == "OvertakingAccelerationAssist") {
       list->addItem(laneChangeSettingsLayout);
       list->addItem(horizontal_line());
 
@@ -389,6 +395,7 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) {
   toggles["EnableMads"]->setConfirmation(true, false);
   toggles["EndToEndLongAlertLight"]->setConfirmation(true, false);
   toggles["CustomOffsets"]->showDescription();
+  toggles["OvertakingAccelerationAssist"]->setConfirmation(true, false);
 
   connect(toggles["EnableMads"], &ToggleControlSP::toggleFlipped, mads_settings, &MadsSettings::updateToggles);
   connect(toggles["EnableMads"], &ToggleControlSP::toggleFlipped, [=](bool state) {