StarPilot/openpilot/starpilot/controllers/coast.py

import math
from dataclasses import dataclass


@dataclass(frozen=True)
class CoastPlan:
  force_decel: bool = False
  brake_floor: float | None = None
  speed_ceiling_mps: float | None = None
  pulse_coasting: bool = False


class CoastOwner:
  def __init__(self):
    self.force_coast = False
    self.pulse_glide = False
    self.coasting = False
    self.hill_paused = False

  def reset(self):
    self.__init__()

  def toggle(self, action, *, long_active):
    if action == 2:
      self.force_coast = not self.force_coast
      return True
    if action == 14 and (long_active or self.pulse_glide):
      self.pulse_glide = not self.pulse_glide
      return True
    return False

  def sample(self, *, admitted, active, gas, brake, speed_mps, target_mps, delta_mps, pitch,
             lead_relevant, stop_context):
    values = (speed_mps, target_mps, delta_mps)
    if (not admitted or not all(type(value) in (int, float) and math.isfinite(value) for value in values) or
        speed_mps < 0 or delta_mps < 0 or type(active) is not bool or type(gas) is not bool or
        type(brake) is not bool or type(lead_relevant) is not bool or type(stop_context) is not bool):
      self.reset()
      return CoastPlan()
    self.force_coast &= not (gas or brake)
    if not active:
      return CoastPlan()
    ceiling = None
    if not self.pulse_glide:
      self.coasting = False
      self.hill_paused = False
    else:
      if type(pitch) in (int, float) and math.isfinite(pitch):
        if self.hill_paused:
          if abs(pitch) <= math.radians(2.5):
            self.hill_paused = False
        elif abs(pitch) >= math.radians(3.0):
          self.hill_paused = True
      lower = target_mps - delta_mps
      if (self.hill_paused or target_mps <= 5.0 or lower < 3.0 or delta_mps <= 0 or
          lead_relevant or stop_context):
        self.coasting = False
      elif self.coasting:
        if speed_mps <= lower + 0.25:
          self.coasting = False
      elif speed_mps >= target_mps - 0.25:
        self.coasting = True
      if self.coasting:
        ceiling = lower
    return CoastPlan(self.force_coast, -0.6 if self.force_coast else -0.03 if self.coasting else None,
                     ceiling, self.coasting)


class CoastRuntime:
  def __init__(self):
    from openpilot.starpilot.controllers.wheel_actions import WheelConsumer
    self.consumer = WheelConsumer()
    self.owner = CoastOwner()
    self.session = None
    self.drive = 0
    self.traffic_toggles = 0
    self.cp_signature = None
    self.conditional_stop = False

  def receive(self, event, params, cp, sm, *, now_ns):
    command = self.consumer.accept(event, params, cp, sm, now_ns=now_ns)
    if command is None:
      return False
    if command.session != self.session or command.drive_id != self.drive:
      self.owner.reset()
      self.session, self.drive = command.session, command.drive_id
    from openpilot.starpilot.controllers.wheel_actions import cp_fingerprint
    self.cp_signature = cp_fingerprint(cp)
    if command.action in (2, 6, 14):
      from openpilot.starpilot.conditional_mode.ui_action import current_authority
      from openpilot.starpilot.saved_source import read_saved
      active_required = command.action == 6 or command.action == 14 and not self.owner.pulse_glide
      if (not cp.openpilotLongitudinalControl or
          read_saved(params, "SafeMode", 8) not in ((None, True), (b"0", True)) or
          active_required and not current_authority(sm, cp, command.drive_id, now_ns)):
        return False
      if command.action == 6:
        self.traffic_toggles += 1
        return True
      return self.owner.toggle(command.action, long_active=bool(sm["carControl"].longActive))
    return False

  def sample(self, params, cp, sm, *, now_ns, target_mps, lead_relevant, stop_context):
    from openpilot.starpilot.controllers.wheel_actions import eligible, LIFETIME_NS, cp_fingerprint, WheelConsumer
    from openpilot.starpilot.conditional_mode.ui_action import fresh_service
    from openpilot.starpilot.saved_source import read_saved
    # A zero accepted-drive/heartbeat cannot satisfy the admission below.
    if (type(self) is CoastRuntime and type(getattr(self, 'owner', None)) is CoastOwner and
        type(getattr(self, 'consumer', None)) is WheelConsumer and 'sample' not in vars(self.owner) and
        type(getattr(self, 'drive', None)) is int and type(getattr(self.consumer, 'last_heartbeat', None)) is int and
        (self.drive == 0 or self.consumer.last_heartbeat == 0) and
        type(stop_context) is bool and type(getattr(self, 'conditional_stop', None)) is bool):
      from capnp.lib.capnp import _DynamicStructReader
      from opendbc.car.structs import car
      from openpilot.cereal import log, messaging
      from openpilot.common.params import Params
      if (type(params) is Params and type(sm) is messaging.SubMaster and type(getattr(sm, 'data', None)) is dict and
          type(cp) is _DynamicStructReader and cp.schema == car.CarParams.schema):
        device = sm.data.get('deviceState')
        state = sm.data.get('carState')
        control = sm.data.get('carControl')
        if (type(device) is _DynamicStructReader and device.schema == log.DeviceState.schema and
            type(state) is _DynamicStructReader and state.schema == car.CarState.schema and
            type(control) is _DynamicStructReader and control.schema == car.CarControl.schema):
          self.owner.reset()
          return CoastPlan()
    drive = int(sm["deviceState"].startedMonoTime)
    raw, readable = read_saved(params, "PulseGlideSpeedDelta", 32)
    units, unit_readable = read_saved(params, "IsMetric", 8)
    try:
      delta = 5.0 if raw is None else float(raw)
    except (ValueError, TypeError, OverflowError):
      delta = math.nan
    admitted = bool(eligible(cp) and self.cp_signature == cp_fingerprint(cp) and cp.openpilotLongitudinalControl and sm["deviceState"].started and
                    drive == self.drive and 0 < drive < self.consumer.last_heartbeat <= now_ns and
                    now_ns - self.consumer.last_heartbeat <= LIFETIME_NS and
                    all(fresh_service(sm, name, drive, now_ns) for name in ("carState", "carControl")) and
                    sm["carState"].canValid and not sm["carState"].canTimeout and
                    read_saved(params, "SafeMode", 8) in ((None, True), (b"0", True)) and
                    readable and unit_readable and units in (None, b"0", b"1") and
                    math.isfinite(delta) and 0.5 <= delta <= 30.0)
    state, control = sm["carState"], sm["carControl"]
    pitch = control.orientationNED[1] if len(control.orientationNED) == 3 else None
    return self.owner.sample(admitted=admitted, active=bool(control.enabled and control.longActive),
                             gas=bool(state.gasPressed), brake=bool(state.brakePressed), speed_mps=float(state.vEgo),
                             target_mps=target_mps, delta_mps=delta * (1 / 3.6 if units == b"1" else 0.44704),
                             pitch=pitch, lead_relevant=lead_relevant, stop_context=bool(stop_context or self.conditional_stop))