From f27249ea94f89353b59cdc8e2a5d67c0068ea18e Mon Sep 17 00:00:00 2001
From: Willem Melching <willem.melching@gmail.com>
Date: Wed, 24 Jul 2019 16:39:26 -0700
Subject: [PATCH] Add fields for LQR lateral control

---
 car.capnp | 15 +++++++++++++++
 log.capnp |  8 ++++++++
 2 files changed, 23 insertions(+)

diff --git a/car.capnp b/car.capnp
index 9a03b8d225698d..e6c9df5dd049a9 100644
--- a/car.capnp
+++ b/car.capnp
@@ -324,6 +324,7 @@ struct CarParams {
   lateralTuning :union {
     pid @26 :LateralPIDTuning;
     indi @27 :LateralINDITuning;
+    lqr @40 :LateralLQRTuning;
   }
 
   steerLimitAlert @28 :Bool;
@@ -366,6 +367,20 @@ struct CarParams {
     actuatorEffectiveness @3 :Float32;
   }
 
+  struct LateralLQRTuning {
+    scale @0 :Float32;
+    ki @1 :Float32;
+    dcGain @2 :Float32;
+
+    # State space system
+    a @3 :List(Float32);
+    b @4 :List(Float32);
+    c @5 :List(Float32);
+
+    k @6 :List(Float32);  # LQR gain
+    l @7 :List(Float32);  # Kalman gain
+  }
+
 
   enum SafetyModel {
     # does NOT match board setting
diff --git a/log.capnp b/log.capnp
index 94fbe5ddd485a5..3b2e00dd213a2d 100644
--- a/log.capnp
+++ b/log.capnp
@@ -454,6 +454,7 @@ struct ControlsState @0x97ff69c53601abf1 {
   lateralControlState :union {
     indiState @52 :LateralINDIState;
     pidState @53 :LateralPIDState;
+    lqrState @55 :LateralLQRState;
   }
 
   enum OpenpilotState @0xdbe58b96d2d1ac61 {
@@ -508,6 +509,13 @@ struct ControlsState @0x97ff69c53601abf1 {
     saturated @8 :Bool;
    }
 
+  struct LateralLQRState {
+    active @0 :Bool;
+    steerAngle @1 :Float32;
+    i @2 :Float32;
+    output @3 :Float32;
+   }
+
 }
 
 struct LiveEventData {