Controls: Neural Network Lateral Control (NNLC) for Torque Lateral Accel Control (#667)

* init

* more init

* keep it alive

* fixes

* more fixes

* more fix

* new submodule for nn data

* bump submodule

* update path to submodule

* spacing???

* update submodule path

* update submodule path

* bump

* dump

* bump

* introduce params

* Add Neural Network Lateral Control toggle to developer panel

This introduces a new toggle for enabling Neural Network Lateral Control (NNLC), providing detailed descriptions of its functionality and compatibility. It includes UI integration, car compatibility checks, and feedback links for unsupported vehicles.

* decouple even more

* static

* codespell

* remove debug

* in structs

* fix import

* convert to capnp

* fixes

* debug

* only initialize if NNLC is enabled or allow to enable

* oops

* fix initialization

* only allow engage if nnlc is off

* fix toggle param

* fix tests

* lint

* fix more test

* capnp test

* try this out

* validate if it's not None

* make it 33 to match

* align

* share the same friction input calculation

* return stock values if not enabled

* unused

* split base and child

* space

* rename

* NeuralNetworkFeedForwardModel

* less

* just use file name

* try this

* more explicit

* rename

* move it

* child class for additional controllers

* rename

* time to split out custom lateral acceleration

* move around

* space

* fix

* TODO-SP

* TODO-SP

* update regardless, it's an extension now

* update name and expose toggle

* ui: sunnypilot Panel -> Steering Panel

* Update selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h

* merge

* move to steering panel

* no need for this

* live params in a thread

* no live for now

* new structs

* more ui

* more flexible

* more ui

* no longer needed

* another ui

* cereal changes

* bump opendbc

* simplify checks

* all in one place

* split Enhanced Lat Accel

---------

Co-authored-by: DevTekVE <devtekve@gmail.com>

.gitmodules

@@ -16,3 +16,6 @@
 [submodule "tinygrad"]
   path = tinygrad_repo
   url = https://github.com/commaai/tinygrad.git
+[submodule "sunnypilot/neural_network_data"]
+  path = sunnypilot/neural_network_data
+  url = https://github.com/sunnypilot/neural-network-data.git

cereal/custom.capnp

@@ -138,6 +138,18 @@ struct OnroadEventSP @0xda96579883444c35 {
 struct CarParamsSP @0x80ae746ee2596b11 {
   flags @0 :UInt32;        # flags for car specific quirks in sunnypilot
   safetyParam @1 : Int16;  # flags for sunnypilot's custom safety flags
+
+  neuralNetworkLateralControl @2 :NeuralNetworkLateralControl;
+
+  struct NeuralNetworkLateralControl {
+    model @0 :Model;
+    fuzzyFingerprint @1 :Bool;
+
+    struct Model {
+      path @0 :Text;
+      name @1 :Text;
+    }
+  }
 }
 
 struct CarControlSP @0xa5cd762cd951a455 {

common/params_keys.h

@@ -141,6 +141,9 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"ModelManager_LastSyncTime", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
     {"ModelManager_ModelsCache", PERSISTENT | BACKUP},
 
+    // Neural Network Lateral Control
+    {"NeuralNetworkLateralControl", PERSISTENT | BACKUP},
+
     // sunnylink params
     {"EnableSunnylinkUploader", PERSISTENT | BACKUP},
     {"LastSunnylinkPingTime", CLEAR_ON_MANAGER_START},

selfdrive/ui/sunnypilot/SConscript

@@ -36,6 +36,7 @@ qt_src = [
 
 lateral_panel_qt_src = [
   "sunnypilot/qt/offroad/settings/lateral/mads_settings.cc",
+  "sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc",
 ]
 
 network_src = [

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc

@@ -0,0 +1,63 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h"
+
+NeuralNetworkLateralControl::NeuralNetworkLateralControl() :
+  ParamControl("NeuralNetworkLateralControl", tr("Neural Network Lateral Control (NNLC)"),  "", "") {
+  setConfirmation(true, false);
+  updateToggle();
+}
+
+void NeuralNetworkLateralControl::showEvent(QShowEvent *event) {
+  updateToggle();
+}
+
+void NeuralNetworkLateralControl::updateToggle() {
+  QString statusInitText = "<font color='yellow'>" + STATUS_CHECK_COMPATIBILITY + "</font>";
+  QString notLoadedText = "<font color='yellow'>" + STATUS_NOT_LOADED + "</font>";
+  QString loadedText = "<font color=#00ff00>" + STATUS_LOADED + "</font>";
+
+  auto cp_bytes = params.get("CarParamsPersistent");
+  auto cp_sp_bytes = params.get("CarParamsSPPersistent");
+  if (!cp_bytes.empty() && !cp_sp_bytes.empty()) {
+    AlignedBuffer aligned_buf;
+    AlignedBuffer aligned_buf_sp;
+    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
+    capnp::FlatArrayMessageReader cmsg_sp(aligned_buf_sp.align(cp_sp_bytes.data(), cp_sp_bytes.size()));
+    cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
+    cereal::CarParamsSP::Reader CP_SP = cmsg_sp.getRoot<cereal::CarParamsSP>();
+
+    if (CP.getSteerControlType() == cereal::CarParams::SteerControlType::ANGLE) {
+      params.remove("NeuralNetworkLateralControl");
+      setDescription(nnffDescriptionBuilder(STATUS_NOT_AVAILABLE));
+      setEnabled(false);
+    } else {
+      QString nn_model_name = QString::fromStdString(CP_SP.getNeuralNetworkLateralControl().getModel().getName());
+      QString nn_fuzzy = CP_SP.getNeuralNetworkLateralControl().getFuzzyFingerprint() ?
+                         STATUS_MATCH_FUZZY : STATUS_MATCH_EXACT;
+
+      if (nn_model_name.isEmpty()) {
+        setDescription(nnffDescriptionBuilder(statusInitText));
+      } else if (nn_model_name == "MOCK") {
+        setDescription(nnffDescriptionBuilder(
+          notLoadedText + "<br>" + buildSupportText(SUPPORT_DONATE_LOGS)
+        ));
+      } else {
+        QString statusText = loadedText + " | " + STATUS_MATCH + " = " + nn_fuzzy + " | " + nn_model_name;
+        QString explanationText = EXPLANATION_MATCH + " " + buildSupportText(SUPPORT_ISSUES);
+        setDescription(nnffDescriptionBuilder(statusText + "<br><br>" + explanationText));
+      }
+    }
+  } else {
+    setDescription(nnffDescriptionBuilder(statusInitText));
+  }
+
+  if (getDescription() != getBaseDescription()) {
+    showDescription();
+  }
+}

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h

@@ -0,0 +1,64 @@
+/**
+ * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+ *
+ * This file is part of sunnypilot and is licensed under the MIT License.
+ * See the LICENSE.md file in the root directory for more details.
+ */
+
+#pragma once
+
+#include <map>
+
+#include "selfdrive/ui/sunnypilot/ui.h"
+#include "selfdrive/ui/qt/util.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
+
+class NeuralNetworkLateralControl : public ParamControl {
+  Q_OBJECT
+
+public:
+  NeuralNetworkLateralControl();
+  void showEvent(QShowEvent *event) override;
+
+public slots:
+  void updateToggle();
+
+private:
+  Params params;
+
+  void refresh();
+
+  // Status messages
+  const QString STATUS_NOT_AVAILABLE = tr("NNLC is currently not available on this platform.");
+  const QString STATUS_CHECK_COMPATIBILITY = tr("Start the car to check car compatibility");
+  const QString STATUS_NOT_LOADED = tr("NNLC Not Loaded");
+  const QString STATUS_LOADED = tr("NNLC Loaded");
+  const QString STATUS_MATCH = tr("Match");
+  const QString STATUS_MATCH_EXACT = tr("Exact");
+  const QString STATUS_MATCH_FUZZY = tr("Fuzzy");
+
+  // Explanations
+  const QString EXPLANATION_MATCH = tr("Match: \"Exact\" is ideal, but \"Fuzzy\" is fine too.");
+  const QString EXPLANATION_FEATURE = tr("Formerly known as <b>\"NNFF\"</b>, this replaces the lateral <b>\"torque\"</b> controller, "
+                                            "with one using a neural network trained on each car's (actually, each separate EPS firmware) driving data for increased controls accuracy.");
+
+  // Support information
+  const QString SUPPORT_CHANNEL = "<font color='white'><b>#tuning-nnlc</b></font>";
+  const QString SUPPORT_REACH_OUT = tr("Reach out to the sunnypilot team in the following channel at the sunnypilot Discord server");
+  const QString SUPPORT_FEEDBACK = tr("with feedback, or to provide log data for your car if your car is currently unsupported:");
+  const QString SUPPORT_ISSUES = tr("if there are any issues:");
+  const QString SUPPORT_DONATE_LOGS = tr("and donate logs to get NNLC loaded for your car:");
+
+  // Description builders
+  QString buildSupportText(const QString& context) const {
+    return SUPPORT_REACH_OUT + " " + context + " " + SUPPORT_CHANNEL;
+  }
+
+  QString nnffDescriptionBuilder(const QString &custom_description) const {
+    return "<b>" + custom_description + "</b><br><br>" + getBaseDescription();
+  }
+
+  QString getBaseDescription() const {
+    return EXPLANATION_FEATURE + "<br><br>" + buildSupportText(SUPPORT_FEEDBACK);
+  }
+};

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc

@@ -42,8 +42,21 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
   });
   list->addItem(madsSettingsButton);
 
+  nnlcToggle = new NeuralNetworkLateralControl();
+  list->addItem(nnlcToggle);
+
+  QObject::connect(nnlcToggle, &ParamControl::toggleFlipped, [=](bool state) {
+    if (state) {
+      nnlcToggle->showDescription();
+    } else {
+      nnlcToggle->hideDescription();
+    }
+
+    nnlcToggle->updateToggle();
+  });
+
   toggleOffroadOnly = {
-    madsToggle,
+    madsToggle, nnlcToggle,
   };
   QObject::connect(uiState(), &UIState::offroadTransition, this, &LateralPanel::updateToggles);
 

selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h

@@ -12,6 +12,7 @@
 
 #include "selfdrive/ui/sunnypilot/ui.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h"
 #include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
 #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
@@ -37,4 +38,5 @@ private:
   ParamControl *madsToggle;
   PushButtonSP *madsSettingsButton;
   MadsSettings *madsWidget = nullptr;
+  NeuralNetworkLateralControl *nnlcToggle = nullptr;
 };

sunnypilot/selfdrive/car/interfaces.py

@@ -8,9 +8,12 @@ See the LICENSE.md file in the root directory for more details.
 from opendbc.car import Bus, structs
 from opendbc.car.can_definitions import CanRecvCallable, CanSendCallable
 from opendbc.car.car_helpers import can_fingerprint
+from opendbc.car.interfaces import CarInterfaceBase
 from opendbc.car.hyundai.radar_interface import RADAR_START_ADDR
 from opendbc.car.hyundai.values import HyundaiFlags, DBC as HYUNDAI_DBC
 from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
+from openpilot.common.swaglog import cloudlog
+from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import get_nn_model_path
 
 import openpilot.system.sentry as sentry
 
@@ -22,6 +25,25 @@ def log_fingerprint(CP: structs.CarParams) -> None:
     sentry.capture_fingerprint(CP.carFingerprint, CP.brand)
 
 
+def initialize_neural_network_lateral_control(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params, enabled: bool = False) -> None:
+  nnlc_model_path, nnlc_model_name, exact_match = get_nn_model_path(CP)
+
+  if nnlc_model_path is None:
+    cloudlog.error({"nnlc event": "car doesn't match any Neural Network model"})
+    nnlc_model_path = "MOCK"
+    nnlc_model_name = "MOCK"
+
+  if nnlc_model_path != "MOCK" and CP.steerControlType != structs.CarParams.SteerControlType.angle:
+    enabled = params.get_bool("NeuralNetworkLateralControl")
+
+  if enabled:
+    CarInterfaceBase.configure_torque_tune(CP.carFingerprint, CP.lateralTuning)
+
+  CP_SP.neuralNetworkLateralControl.model.path = nnlc_model_path
+  CP_SP.neuralNetworkLateralControl.model.name = nnlc_model_name
+  CP_SP.neuralNetworkLateralControl.fuzzyFingerprint = not exact_match
+
+
 def setup_car_interface_sp(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params):
   if CP.brand == 'hyundai':
     if CP.flags & HyundaiFlags.MANDO_RADAR and CP.radarUnavailable:
@@ -32,6 +54,8 @@ def setup_car_interface_sp(CP: structs.CarParams, CP_SP: structs.CarParamsSP, pa
         if params.get_bool("HyundaiRadarTracks"):
           CP.radarUnavailable = False
 
+  initialize_neural_network_lateral_control(CP, CP_SP, params)
+
 
 def initialize_car_interface_sp(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params, can_recv: CanRecvCallable,
                                 can_send: CanSendCallable):

sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py

@@ -4,10 +4,11 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
 This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
-from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base import LatControlTorqueExtBase
+
+from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.nnlc import NeuralNetworkLateralControl
 
 
-class LatControlTorqueExt(LatControlTorqueExtBase):
+class LatControlTorqueExt(NeuralNetworkLateralControl):
   def __init__(self, lac_torque, CP, CP_SP):
     super().__init__(lac_torque, CP, CP_SP)
 
@@ -22,5 +23,6 @@ class LatControlTorqueExt(LatControlTorqueExtBase):
     self._actual_lateral_accel = actual_lateral_accel
 
     self.update_calculations(CS, VM, desired_lateral_accel)
+    self.update_neural_network_feedforward(CS, params, calibrated_pose)
 
     return self._ff, self._pid_log

sunnypilot/selfdrive/controls/lib/nnlc/helpers.py

@@ -0,0 +1,58 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import os
+from difflib import SequenceMatcher
+
+from opendbc.car import structs
+from openpilot.common.basedir import BASEDIR
+
+TORQUE_NN_MODEL_PATH = os.path.join(BASEDIR, "sunnypilot", "neural_network_data", "neural_network_lateral_control")
+
+
+def similarity(s1: str, s2: str) -> float:
+  return SequenceMatcher(None, s1, s2).ratio()
+
+
+def get_nn_model_path(CP: structs.CarParams) -> tuple[str | None, str, bool]:
+  exact_match = True
+  car_fingerprint = CP.carFingerprint
+  eps_fw = str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), ""))
+  model_name = ""
+
+  def check_nn_path(nn_candidate):
+    _model_path = None
+    _max_similarity = -1.0
+    for f in os.listdir(TORQUE_NN_MODEL_PATH):
+      if f.endswith(".json"):
+        model = os.path.splitext(f)[0]
+        similarity_score = similarity(model, nn_candidate)
+        if similarity_score > _max_similarity:
+          _max_similarity = similarity_score
+          _model_path = os.path.join(TORQUE_NN_MODEL_PATH, f)
+    return _model_path, _max_similarity
+
+  if len(eps_fw) > 3:
+    eps_fw = eps_fw.replace("\\", "")
+    nn_candidate = f"{car_fingerprint} {eps_fw}"
+    model_path, max_similarity = check_nn_path(nn_candidate)
+
+    if model_path is not None and car_fingerprint in model_path and max_similarity >= 0.9:
+      model_name = os.path.splitext(os.path.basename(model_path))[0]
+      exact_match = max_similarity >= 0.99
+      return model_path, model_name, exact_match
+
+  nn_candidate = car_fingerprint
+  model_path, max_similarity = check_nn_path(nn_candidate)
+
+  if model_path is None or car_fingerprint not in model_path or max_similarity < 0.9:
+    model_path = None
+
+  if model_path is not None:
+    model_name = os.path.splitext(os.path.basename(model_path))[0]
+    exact_match = max_similarity >= 0.99
+
+  return model_path, model_name, exact_match

sunnypilot/selfdrive/controls/lib/nnlc/model.py

@@ -0,0 +1,81 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from json import load
+import numpy as np
+
+# dict used to rename activation functions whose names aren't valid python identifiers
+ACTIVATION_FUNCTION_NAMES = {'σ': 'sigmoid'}
+
+
+class NNTorqueModel:
+  def __init__(self, params_file, zero_bias=False):
+    with open(params_file) as f:
+      params = load(f)
+
+    self.input_size = params["input_size"]
+    self.output_size = params["output_size"]
+    self.input_mean = np.array(params["input_mean"], dtype=np.float32).T
+    self.input_std = np.array(params["input_std"], dtype=np.float32).T
+    self.layers = []
+    self.friction_override = False
+
+    for layer_params in params["layers"]:
+      W = np.array(layer_params[next(key for key in layer_params.keys() if key.endswith('_W'))], dtype=np.float32).T
+      b = np.array(layer_params[next(key for key in layer_params.keys() if key.endswith('_b'))], dtype=np.float32).T
+      if zero_bias:
+        b = np.zeros_like(b)
+      activation = layer_params["activation"]
+      for k, v in ACTIVATION_FUNCTION_NAMES.items():
+        activation = activation.replace(k, v)
+      self.layers.append((W, b, activation))
+
+    self.validate_layers()
+    self.check_for_friction_override()
+
+  # Begin activation functions.
+  # These are called by name using the keys in the model json file
+  @staticmethod
+  def sigmoid(x):
+    return 1 / (1 + np.exp(-x))
+
+  @staticmethod
+  def identity(x):
+    return x
+  # End activation functions
+
+  def forward(self, x):
+    for W, b, activation in self.layers:
+      x = getattr(self, activation)(x.dot(W) + b)
+    return x
+
+  def evaluate(self, input_array):
+    in_len = len(input_array)
+    if in_len != self.input_size:
+      # If the input is length 2-4, then it's a simplified evaluation.
+      # In that case, need to add on zeros to fill out the input array to match the correct length.
+      if 2 <= in_len:
+        input_array = input_array + [0] * (self.input_size - in_len)
+      else:
+        raise ValueError(f"Input array length {len(input_array)} must be length 2 or greater")
+
+    input_array = np.array(input_array, dtype=np.float32)
+
+    # Rescale the input array using the input_mean and input_std
+    input_array = (input_array - self.input_mean) / self.input_std
+
+    output_array = self.forward(input_array)
+
+    return float(output_array[0, 0])
+
+  def validate_layers(self):
+    for _, _, activation in self.layers:
+      if not hasattr(self, activation):
+        raise ValueError(f"Unknown activation: {activation}")
+
+  def check_for_friction_override(self):
+    y = self.evaluate([10.0, 0.0, 0.2])
+    self.friction_override = (y < 0.1)

sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py

@@ -0,0 +1,106 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from collections import deque
+import math
+import numpy as np
+
+from opendbc.car.interfaces import LatControlInputs
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.params import Params
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base import LatControlTorqueExtBase
+from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.model import NNTorqueModel
+
+
+# At a given roll, if pitch magnitude increases, the
+# gravitational acceleration component starts pointing
+# in the longitudinal direction, decreasing the lateral
+# acceleration component. Here we do the same thing
+# to the roll value itself, then passed to nnff.
+def roll_pitch_adjust(roll, pitch):
+  return roll * math.cos(pitch)
+
+
+class NeuralNetworkLateralControl(LatControlTorqueExtBase):
+  def __init__(self, lac_torque, CP, CP_SP):
+    super().__init__(lac_torque, CP, CP_SP)
+    self.params = Params()
+    self.enabled = self.params.get_bool("NeuralNetworkLateralControl")
+
+    # NN model takes current v_ego, lateral_accel, lat accel/jerk error, roll, and past/future/planned data
+    # of lat accel and roll
+    # Past value is computed using previous desired lat accel and observed roll
+    # Only initialize NNTorqueModel if enabled
+    self.model = NNTorqueModel(CP_SP.neuralNetworkLateralControl.model.path) if self.enabled else None
+
+    self.pitch = FirstOrderFilter(0.0, 0.5, 0.01)
+    self.pitch_last = 0.0
+
+    # setup future time offsets
+    self.nn_time_offset = CP.steerActuatorDelay + 0.2
+    future_times = [0.3, 0.6, 1.0, 1.5] # seconds in the future
+    self.nn_future_times = [i + self.nn_time_offset for i in future_times]
+
+    # setup past time offsets
+    self.past_times = [-0.3, -0.2, -0.1]
+    history_check_frames = [int(abs(i)*100) for i in self.past_times]
+    self.history_frame_offsets = [history_check_frames[0] - i for i in history_check_frames]
+    self.lateral_accel_desired_deque = deque(maxlen=history_check_frames[0])
+    self.roll_deque = deque(maxlen=history_check_frames[0])
+    self.error_deque = deque(maxlen=history_check_frames[0])
+    self.past_future_len = len(self.past_times) + len(self.nn_future_times)
+
+  def update_neural_network_feedforward(self, CS, params, calibrated_pose):
+    if not self.enabled or not self.model_valid:
+      return
+
+    # update past data
+    roll = params.roll
+    if calibrated_pose is not None:
+      pitch = self.pitch.update(calibrated_pose.orientation.pitch)
+      roll = roll_pitch_adjust(roll, pitch)
+      self.pitch_last = pitch
+    self.roll_deque.append(roll)
+    self.lateral_accel_desired_deque.append(self._desired_lateral_accel)
+
+    # prepare past and future values
+    # adjust future times to account for longitudinal acceleration
+    adjusted_future_times = [t + 0.5 * CS.aEgo * (t / max(CS.vEgo, 1.0)) for t in self.nn_future_times]
+    past_rolls = [self.roll_deque[min(len(self.roll_deque) - 1, i)] for i in self.history_frame_offsets]
+    future_rolls = [roll_pitch_adjust(np.interp(t, ModelConstants.T_IDXS, self.model_v2.orientation.x) + roll,
+                                      np.interp(t, ModelConstants.T_IDXS, self.model_v2.orientation.y) + self.pitch_last) for t in
+                    adjusted_future_times]
+    past_lateral_accels_desired = [self.lateral_accel_desired_deque[min(len(self.lateral_accel_desired_deque) - 1, i)]
+                                   for i in self.history_frame_offsets]
+    future_planned_lateral_accels = [np.interp(t, ModelConstants.T_IDXS, self.model_v2.acceleration.y) for t in
+                                     adjusted_future_times]
+
+    # compute NNFF error response
+    nnff_setpoint_input = [CS.vEgo, self._setpoint, self.lateral_jerk_setpoint, roll] \
+                          + [self._setpoint] * self.past_future_len \
+                          + past_rolls + future_rolls
+    # past lateral accel error shouldn't count, so use past desired like the setpoint input
+    nnff_measurement_input = [CS.vEgo, self._measurement, self.lateral_jerk_measurement, roll] \
+                             + [self._measurement] * self.past_future_len \
+                             + past_rolls + future_rolls
+    torque_from_setpoint = self.model.evaluate(nnff_setpoint_input)
+    torque_from_measurement = self.model.evaluate(nnff_measurement_input)
+    self._pid_log.error = torque_from_setpoint - torque_from_measurement
+
+    # compute feedforward (same as nn setpoint output)
+    friction_input = self.update_friction_input(self._setpoint, self._measurement)
+    nn_input = [CS.vEgo, self._desired_lateral_accel, friction_input, roll] \
+               + past_lateral_accels_desired + future_planned_lateral_accels \
+               + past_rolls + future_rolls
+    self._ff = self.model.evaluate(nn_input)
+
+    # apply friction override for cars with low NN friction response
+    if self.model.friction_override:
+      self._pid_log.error += self.torque_from_lateral_accel(LatControlInputs(0.0, 0.0, CS.vEgo, CS.aEgo), self.torque_params,
+                                                            friction_input,
+                                                            self._lateral_accel_deadzone, friction_compensation=True,
+                                                            gravity_adjusted=False)

system/manager/manager.py

@@ -49,7 +49,8 @@ def manager_init() -> None:
     ("MadsSteeringMode", "0"),
     ("MadsUnifiedEngagementMode", "1"),
     ("ModelManager_LastSyncTime", "0"),
-    ("ModelManager_ModelsCache", "")
+    ("ModelManager_ModelsCache", ""),
+    ("NeuralNetworkLateralControl", "0"),
   ]
 
   if params.get_bool("RecordFrontLock"):