mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-06 20:43:46 +08:00
firestar5683
e4f3d6661c
Implement acceleration profiles, FollowJerk, force-stop and lead-departure owners. Keep selected acceleration limits and hazard arbitration explicit at their shared boundaries. Co-authored-by: whoisdomi <dcantu83@gmail.com>
74 lines
3.7 KiB
Python
74 lines
3.7 KiB
Python
"""Lead departure and inside-gap responses, bounded by the native planner."""
|
|
|
|
import math
|
|
|
|
from openpilot.starpilot.longitudinal.experimental_release import valid_key, SOURCES, MAX_SOURCE_AGE_NS
|
|
|
|
|
|
def current_leads(sm, cp, now_ns, key):
|
|
try:
|
|
if not valid_key(key):
|
|
return None
|
|
if not all(sm.valid[name] and sm.alive[name] and key[2] <= sm.logMonoTime[name] <= now_ns and
|
|
now_ns - sm.logMonoTime[name] <= MAX_SOURCE_AGE_NS for name in SOURCES):
|
|
return None
|
|
car = sm['carState']
|
|
if (not cp.openpilotLongitudinalControl or cp.passive or cp.dashcamOnly or cp.notCar or
|
|
not sm['carControl'].longActive or not sm['selfdriveState'].enabled or
|
|
not car.canValid or car.canTimeout or car.brakePressed or car.gasPressed or sm['controlsState'].forceDecel):
|
|
return None
|
|
leads = tuple(lead for lead in (sm['radarState'].leadOne, sm['radarState'].leadTwo) if lead.present)
|
|
for lead in leads:
|
|
if not all(math.isfinite(float(getattr(lead, field))) for field in ('dRel', 'vLead', 'aLeadK', 'yRel', 'modelProb')):
|
|
return None
|
|
return leads
|
|
except (AttributeError, KeyError, TypeError, ValueError, OverflowError):
|
|
return None
|
|
|
|
|
|
def inside_gap_cap(lead, speed, follow_seconds, accel_min):
|
|
"""Dom's modest closing-speed response once inside the selected following gap."""
|
|
if (speed < 8.0 or lead.vLead < 5.0 or abs(lead.yRel) > 1.75 or
|
|
not lead.radar and lead.modelProb < .95 or speed - lead.vLead < .5):
|
|
return None
|
|
desired_gap = speed * follow_seconds + 6.0 + (speed ** 2 - max(lead.vLead, 0.) ** 2) / 5.0
|
|
deficit = desired_gap - lead.dRel
|
|
if deficit <= max(3.0, .15 * desired_gap):
|
|
return None
|
|
brake_deficit = .25 * desired_gap
|
|
deficit_factor = max(0., min(1., (deficit - brake_deficit) / max(brake_deficit, 1.)))
|
|
closing_factor = max(0., min(1., (speed - lead.vLead - 1.) / 1.5))
|
|
return max(accel_min, -min(.65, .45 * deficit_factor + .20 * closing_factor))
|
|
|
|
|
|
def departure_floor(lead, speed, model_accel):
|
|
"""Dom's positive-model departure assist; native lead/braking targets still win."""
|
|
if (speed > 2.0 or lead.vLead < .6 or lead.vLead - speed < .5 or lead.dRel < 3.5 or
|
|
lead.aLeadK < -.2 or model_accel < .12 or abs(lead.yRel) > 1.75 or
|
|
not lead.radar and lead.modelProb < .85):
|
|
return None
|
|
gap_factor = max(0., min(1., (lead.dRel - 3.5) / 2.5))
|
|
lead_factor = max(0., min(1., (lead.vLead - .6) / 1.6))
|
|
cap = .25 + .30 * max(0., min(1., .55 * lead_factor + .45 * gap_factor))
|
|
return min(cap, max(model_accel + .10, .25))
|
|
|
|
|
|
def adjust(*, target, sm, cp, now_ns, key, follow_seconds, mpc_target, cruise_target, model_target,
|
|
stopping, force_stop, traffic_mode, accel_min):
|
|
leads = current_leads(sm, cp, now_ns, key)
|
|
if not leads or traffic_mode is not False or force_stop or stopping or not .5 <= follow_seconds <= 3.:
|
|
return target
|
|
speed = float(sm['carState'].vEgo)
|
|
if not all(math.isfinite(float(value)) for value in
|
|
(target, speed, follow_seconds, mpc_target, cruise_target, model_target, accel_min)):
|
|
return target
|
|
# A departure may only recover a conservative model acceleration. It cannot
|
|
# override either native MPC lead braking, cruise limits, stop or another lead.
|
|
if (sm['selfdriveState'].experimentalMode and not stopping and
|
|
not sm['modelV2'].action.shouldStop and all(lead.vLead > speed and lead.dRel >= 3.5 for lead in leads)):
|
|
floors = [floor for lead in leads if (floor := departure_floor(lead, speed, model_target)) is not None]
|
|
if floors:
|
|
target = max(target, min(min(floors), mpc_target, cruise_target))
|
|
caps = [cap for lead in leads if (cap := inside_gap_cap(lead, speed, follow_seconds, accel_min)) is not None]
|
|
return min(target, min(caps)) if caps else target
|