Add fields for LQR lateral control

emmertex · Jul 24, 2019 · f27249e · f27249e
1 parent 654860c
commit f27249e
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 0 deletions.
diff --git 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
@@ -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 {