diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index c8bf8f779..09478b6db 100644 Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ diff --git a/cereal/car.capnp b/cereal/car.capnp index 475f789f1..976a7ac91 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -86,6 +86,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { internetConnectivityNeeded @61; manualSteeringRequired @62; manualSteeringRequiredBlinkersOn @63; + leadCarMoving @64; } } diff --git a/common/params.py b/common/params.py index ea7950b2f..8549f7e87 100755 --- a/common/params.py +++ b/common/params.py @@ -139,6 +139,7 @@ keys = { "DragonCarModel": [TxType.PERSISTENT], "DragonCarVIN": [TxType.PERSISTENT], "DragonEnableSlowOnCurve": [TxType.PERSISTENT], + "DragonEnableLeadCarMovingAlert": [TxType.PERSISTENT], } diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8f220dadc..05e256285 100755 --- a/selfdrive/controls/controlsd.py +++ b/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 = \ diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 2ffaf562c..eb2e26036 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/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), ] diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 4004fbaf4..59960e469 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -49,6 +49,7 @@ default_conf = { 'DragonCarModel': '', 'DragonCarVIN': '', 'DragonEnableSlowOnCurve': '1', + 'DragonEnableLeadCarMovingAlert': '0', } deprecated_conf = {