Add Lead Car Moving Alert

cereal/car.capnp

@@ -86,6 +86,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     internetConnectivityNeeded @61;
     manualSteeringRequired @62;
     manualSteeringRequiredBlinkersOn @63;
+    leadCarMoving @64;
   }
 }
 

common/params.py

@@ -139,6 +139,7 @@ keys = {
   "DragonCarModel": [TxType.PERSISTENT],
   "DragonCarVIN": [TxType.PERSISTENT],
   "DragonEnableSlowOnCurve": [TxType.PERSISTENT],
+  "DragonEnableLeadCarMovingAlert": [TxType.PERSISTENT],
 }
 
 

selfdrive/controls/controlsd.py

@@ -521,6 +521,9 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
   dragon_toyota_stock_dsu = False
   dragon_lat_control = True
   dragon_display_steering_limit_alert = True
+  dragon_stopped_has_lead_count = 0
+  dragon_lead_car_moving_alert = False
+  dragon_send_lead_car_moving_alert = False
 
   while True:
     # dragonpilot, don't check for param too often as it's a kernel call
@@ -529,6 +532,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
       dragon_toyota_stock_dsu = True if params.get("DragonToyotaStockDSU", encoding='utf8') == "1" else False
       dragon_lat_control = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True
       dragon_display_steering_limit_alert = False if params.get("DragonDisplaySteeringLimitAlert", encoding='utf8') == "0" else True
+      dragon_lead_car_moving_alert = True if params.get("DragonEnableLeadCarMovingAlert", encoding='utf8') == "1" else False
       ts_last_check = ts
 
     start_time = sec_since_boot()
@@ -567,6 +571,25 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
       if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
         events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
 
+    if dragon_lead_car_moving_alert:
+      # when car has a lead and is standstill and lead is barely moving, we start counting
+      if not CP.radarOffCan and sm['plan'].hasLead and CS.vEgo < 0.1 and 0.1 > sm['plan'].vTarget >= 0:
+        dragon_stopped_has_lead_count += 1
+      else:
+        dragon_stopped_has_lead_count = 0
+
+      # when we detect lead car over a sec and the lead car is started moving, we are ready to send alerts
+      # once the condition is triggered, we want to keep the trigger
+      if dragon_stopped_has_lead_count >= 100 and sm['plan'].vTargetFuture >= 0.1:
+        dragon_send_lead_car_moving_alert = True
+
+      # we remove alert once our car is moving
+      if CS.vEgo >= 0.1:
+        dragon_send_lead_car_moving_alert = False
+
+      if dragon_send_lead_car_moving_alert:
+        events.append(create_event('leadCarMoving', [ET.WARNING]))
+
     if not read_only:
       # update control state
       state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \

selfdrive/controls/lib/alerts.py

@@ -731,4 +731,10 @@ ALERTS = [
     "",
     AlertStatus.normal, AlertSize.small,
     Priority.LOW, VisualAlert.none, AudibleAlert.none, .0, .1, .1, alert_rate=0.25),
+  Alert(
+    "leadCarMoving",
+    "LEAD CAR IS MOVING",
+    "Resume Driving Manually",
+    AlertStatus.userPrompt, AlertSize.mid,
+    Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1),
 ]

selfdrive/dragonpilot/dragonconf/__init__.py

@@ -49,6 +49,7 @@ default_conf = {
   'DragonCarModel': '',
   'DragonCarVIN': '',
   'DragonEnableSlowOnCurve': '1',
+  'DragonEnableLeadCarMovingAlert': '0',
 }
 
 deprecated_conf = {