Subaru Long Accel

segments = [
  "d9df6f87e8feff94|2023-03-28--17-41-10/1:12"
]
platform = "SUBARU_OUTBACK"

import copy
import numpy as np

from opendbc.can.parser import CANParser
from opendbc.car.subaru.values import DBC

from openpilot.selfdrive.pandad import can_capnp_to_list
from openpilot.tools.lib.logreader import LogReader

"""
In this example, we plot the relationship between Cruise_Brake and Acceleration for stock eyesight.
"""

for segment in segments:
  lr = LogReader(segment)

  messages = [
    ("ES_Distance", 20),
    ("ES_Brake", 20),
    ("ES_Status", 20),
  ]

  cp = CANParser(DBC[platform]["pt"], messages, 1)

  es_distance_history = []
  es_status_history = []
  es_brake_history = []
  acceleration_history = []

  last_acc = 0

  for msg in lr:
    if msg.which() == "can":
      cp.update_strings(can_capnp_to_list([msg.as_builder().to_bytes()]))
      es_distance_history.append(copy.copy(cp.vl["ES_Distance"]))
      es_brake_history.append(copy.copy(cp.vl["ES_Brake"]))
      es_status_history.append(copy.copy(cp.vl["ES_Status"]))

      acceleration_history.append(last_acc)

    if msg.which() == "carState":
      last_acc = msg.carState.aEgo

def process(history, func):
  return np.array([func(h) for h in history])

cruise_activated = process(es_status_history, lambda es_status: es_status["Cruise_Activated"])
cruise_throttle = process(es_distance_history, lambda es_distance: es_distance["Cruise_Throttle"])
cruise_rpm = process(es_status_history, lambda es_status: es_status["Cruise_RPM"])
cruise_brake = process(es_brake_history, lambda es_brake: es_brake["Brake_Pressure"])
acceleration = process(acceleration_history, lambda acc: acc)

import matplotlib.pyplot as plt

valid_brake = (cruise_activated==1) & (cruise_brake>0) # only when cruise is activated and eyesight is braking

ax = plt.figure().add_subplot()

ax.set_title("Brake_Pressure vs Acceleration")
ax.set_xlabel("Brake_Pessure")
ax.set_ylabel("Acceleration")
ax.scatter(cruise_brake[valid_brake], -acceleration[valid_brake])