openpilot_comma/selfdrive/controls/lib/speed_smoother.py

import numpy as np


def get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin):

  tDelta = 0.
  if aEgo > aMax:
    tDelta = (aMax - aEgo) / jMin
  elif aEgo < aMin:
    tDelta = (aMin - aEgo) / jMax

  return tDelta


def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts):

  dV = vT - vEgo

  # recover quickly if dV is positive and aEgo is negative or viceversa
  if dV > 0. and aEgo < 0.:
    jMax *= 3.
  elif dV < 0. and aEgo > 0.:
    jMin *= 3.

  tDelta = get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin)

  if (ts <= tDelta):
    if (aEgo < aMin):
      vEgo += ts * aEgo + 0.5 * ts**2 * jMax
      aEgo += ts * jMax
      return vEgo, aEgo
    elif (aEgo > aMax):
      vEgo += ts * aEgo + 0.5 * ts**2 * jMin
      aEgo += ts * jMin
      return vEgo, aEgo

  if aEgo > aMax:
    dV -= 0.5 * (aMax**2 - aEgo**2) / jMin
    vEgo += 0.5 * (aMax**2 - aEgo**2) / jMin
    aEgo += tDelta * jMin
  elif aEgo < aMin:
    dV -= 0.5 * (aMin**2 - aEgo**2) / jMax
    vEgo += 0.5 * (aMin**2 - aEgo**2) / jMax
    aEgo += tDelta * jMax

  ts -= tDelta

  jLim = jMin if aEgo >= 0 else jMax
  # if we reduce the accel to zero immediately, how much delta speed we generate?
  dv_min_shift = - 0.5 * aEgo**2 / jLim

  # flip signs so we can consider only one case
  flipped = False
  if dV < dv_min_shift:
    flipped = True
    dV *= -1
    vEgo *= -1
    aEgo *= -1
    aMax = -aMin
    jMaxcopy = -jMin
    jMin = -jMax
    jMax = jMaxcopy

  # small addition needed to avoid numerical issues with sqrt of ~zero
  aPeak = np.sqrt((0.5 * aEgo**2 / jMax + dV + 1e-9) / (0.5 / jMax - 0.5 / jMin))

  if aPeak > aMax:
    aPeak = aMax
    t1 = (aPeak - aEgo) / jMax
    if aPeak <= 0:  # there is no solution, so stop after t1
      t2 = t1 + ts + 1e-9
      t3 = t2
    else:
      vChange = dV - 0.5 * (aPeak**2 - aEgo**2) / jMax + 0.5 * aPeak**2 / jMin
      if vChange < aPeak * ts:
        t2 = t1 + vChange / aPeak
      else:
        t2 = t1 + ts
      t3 = t2 - aPeak / jMin
  else:
    t1 = (aPeak - aEgo) / jMax
    t2 = t1
    t3 = t2 - aPeak / jMin

  dt1 = min(ts, t1)
  dt2 = max(min(ts, t2) - t1, 0.)
  dt3 = max(min(ts, t3) - t2, 0.)

  if ts > t3:
    vEgo += dV
    aEgo = 0.
  else:
    vEgo += aEgo * dt1 + 0.5 * dt1**2 * jMax + aPeak * dt2 + aPeak * dt3 + 0.5 * dt3**2 * jMin
    aEgo += jMax * dt1 + dt3 * jMin

  vEgo *= -1 if flipped else 1
  aEgo *= -1 if flipped else 1

  return float(vEgo), float(aEgo)