diff --git a/cereal/car.capnp b/cereal/car.capnp
index fdcdfdd82191231..df1aa60fcec856b 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -428,6 +428,7 @@ struct CarParams {
     pid @26 :LateralPIDTuning;
     indi @27 :LateralINDITuning;
     lqr @40 :LateralLQRTuning;
+    torque @66 :LateralTorqueTuning;
   }
 
   steerLimitAlert @28 :Bool;
@@ -476,11 +477,20 @@ struct CarParams {
     kf @4 :Float32;
   }
 
+  struct LateralTorqueTuning {
+    useSteeringAngle @0 :Bool;
+    kp @1 :Float32;
+    ki @2 :Float32;
+    friction @3 :Float32;
+    kf @4 :Float32;
+  }
+
   struct LongitudinalPIDTuning {
     kpBP @0 :List(Float32);
     kpV @1 :List(Float32);
     kiBP @2 :List(Float32);
     kiV @3 :List(Float32);
+    kf @6 :Float32;
     deadzoneBP @4 :List(Float32);
     deadzoneV @5 :List(Float32);
   }
diff --git a/cereal/log.capnp b/cereal/log.capnp
index 6f4f439394d254b..80aca0f42f27170 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -579,6 +579,7 @@ struct ControlsState @0x97ff69c53601abf1 {
     lqrState @55 :LateralLQRState;
     angleState @58 :LateralAngleState;
     debugState @59 :LateralDebugState;
+    torqueState @60 :LateralTorqueState;
   }
 
   enum OpenpilotState @0xdbe58b96d2d1ac61 {
@@ -630,6 +631,18 @@ struct ControlsState @0x97ff69c53601abf1 {
     steeringAngleDesiredDeg @9 :Float32;
    }
 
+   struct LateralTorqueState {
+     active @0 :Bool;
+     error @1 :Float32;
+     errorRate @8 :Float32;
+     p @2 :Float32;
+     i @3 :Float32;
+     d @4 :Float32;
+     f @5 :Float32;
+     output @6 :Float32;
+     saturated @7 :Bool;
+    }
+
   struct LateralLQRState {
     active @0 :Bool;
     steeringAngleDeg @1 :Float32;
diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h
index 648d10d768f4bd3..d1a2922f84e1629 100644
--- a/panda/board/safety/safety_toyota.h
+++ b/panda/board/safety/safety_toyota.h
@@ -3,13 +3,13 @@ const int TOYOTA_MAX_TORQUE = 1500;       // max torque cmd allowed ever
 
 // rate based torque limit + stay within actually applied
 // packet is sent at 100hz, so this limit is 1000/sec
-const int TOYOTA_MAX_RATE_UP = 10;        // ramp up slow
-const int TOYOTA_MAX_RATE_DOWN = 25;      // ramp down fast
+const int TOYOTA_MAX_RATE_UP = 15;        // ramp up slow
+const int TOYOTA_MAX_RATE_DOWN = 25;      // ramp down fast (we do not use 50 just allow throuht ~ kumar)
 const int TOYOTA_MAX_TORQUE_ERROR = 350;  // max torque cmd in excess of torque motor
 
 // real time torque limit to prevent controls spamming
 // the real time limit is 1500/sec
-const int TOYOTA_MAX_RT_DELTA = 375;      // max delta torque allowed for real time checks
+const int TOYOTA_MAX_RT_DELTA = 450;      // max delta torque allowed for real time checks
 const uint32_t TOYOTA_RT_INTERVAL = 250000;    // 250ms between real time checks
 
 // longitudinal limits
diff --git a/release/files_common b/release/files_common
index 67e4c0e31cd0365..f91389e4f121966 100644
--- a/release/files_common
+++ b/release/files_common
@@ -239,6 +239,7 @@ selfdrive/controls/lib/lane_planner.py
 selfdrive/controls/lib/latcontrol_angle.py
 selfdrive/controls/lib/latcontrol_indi.py
 selfdrive/controls/lib/latcontrol_lqr.py
+selfdrive/controls/lib/latcontrol_torque.py
 selfdrive/controls/lib/latcontrol_pid.py
 selfdrive/controls/lib/latcontrol.py
 selfdrive/controls/lib/lateral_planner.py
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index 09402119c9771e2..630b23724e5d1a8 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -100,6 +100,7 @@ def get_std_params(candidate, fingerprint):
     ret.stoppingControl = True
     ret.longitudinalTuning.deadzoneBP = [0.]
     ret.longitudinalTuning.deadzoneV = [0.]
+    ret.longitudinalTuning.kf = 1.
     ret.longitudinalTuning.kpBP = [0.]
     ret.longitudinalTuning.kpV = [1.]
     ret.longitudinalTuning.kiBP = [0.]
diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py
index 930619ee6753def..0a5e78e1d5e93e2 100755
--- a/selfdrive/car/tests/test_car_interfaces.py
+++ b/selfdrive/car/tests/test_car_interfaces.py
@@ -39,8 +39,8 @@ def test_car_interfaces(self, car_name):
       tuning = car_params.lateralTuning.which()
       if tuning == 'pid':
         self.assertTrue(len(car_params.lateralTuning.pid.kpV))
-      elif tuning == 'lqr':
-        self.assertTrue(len(car_params.lateralTuning.lqr.a))
+      elif tuning == 'torque':
+        self.assertTrue(car_params.lateralTuning.torque.kf > 0)
       elif tuning == 'indi':
         self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV))
 
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 70af9887737fcd9..dc48de1c27128b3 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -36,7 +36,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       ret.steerRatio = 15.74   # unknown end-to-end spec
       tire_stiffness_factor = 0.6371   # hand-tune
       ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
-
       set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS)
       ret.steerActuatorDelay = 0.3
 
@@ -46,7 +45,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       ret.steerRatio = 17.4
       tire_stiffness_factor = 0.5533
       ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
-      set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06)
 
     elif candidate in (CAR.RAV4, CAR.RAV4H):
       stop_and_go = True if (candidate in CAR.RAV4H) else False
@@ -54,7 +53,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       ret.steerRatio = 16.88   # 14.5 is spec end-to-end
       tire_stiffness_factor = 0.5533
       ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
-      set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06)
 
     elif candidate == CAR.COROLLA:
       ret.wheelbase = 2.70
@@ -87,10 +86,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       tire_stiffness_factor = 0.7933
       ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
       if Params().get_bool('LqrTune'):
-        set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS_TSS2)
-        ret.steerActuatorDelay = 0.3
-        ret.steerRateCost = 1.25
-        ret.steerLimitTimer = 0.5
+        set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.4, FRICTION=0.05)
       else:
         set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
 
@@ -123,13 +119,13 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       ret.steerRatio = 14.3
       tire_stiffness_factor = 0.7933
       ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG  # Average between ICE and Hybrid
-      set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_TORQUE=2.5, FRICTION=0.06)
 
       # 2019+ Rav4 TSS2 uses two different steering racks and specific tuning seems to be necessary.
       # See https://github.com/commaai/openpilot/pull/21429#issuecomment-873652891
       for fw in car_fw:
         if fw.ecu == "eps" and (fw.fwVersion.startswith(b'\x02') or fw.fwVersion in [b'8965B42181\x00\x00\x00\x00\x00\x00']):
-          set_lat_tune(ret.lateralTuning, LatTunes.PID_I)
+          set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_TORQUE=2.5, FRICTION=0.06)
           break
 
     elif candidate in (CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2):
@@ -138,7 +134,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       ret.steerRatio = 13.9
       tire_stiffness_factor = 0.444  # not optimized yet
       ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
-      set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.0, FRICTION=0.06)
 
     elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH):
       stop_and_go = True
@@ -186,10 +182,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       tire_stiffness_factor = 0.6371   # hand-tune
       ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
       if Params().get_bool('LqrTune'):
-        set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS_TSS2)
-        ret.steerActuatorDelay = 0.3
-        ret.steerRateCost = 1.25
-        ret.steerLimitTimer = 0.5
+        set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.0, FRICTION=0.06)
       else:
         set_lat_tune(ret.lateralTuning, LatTunes.PID_N)
 
diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index 44a732685a0f078..115ea77a5403545 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -25,7 +25,8 @@ class LatTunes(Enum):
   PID_M = 14
   PID_N = 15
   INDI_PRIUS_TSS2 = 16
-  
+  INDI_RAV4_TSS2 = 17
+  TORQUE = 18
 
 
 ###### LONG ######
@@ -51,8 +52,15 @@ def set_long_tune(tune, name):
 
 
 ###### LAT ######
-def set_lat_tune(tune, name):
-  if name == LatTunes.INDI_PRIUS:
+def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1):
+  if name == LatTunes.TORQUE:
+    tune.init('torque')
+    tune.torque.useSteeringAngle = True
+    tune.torque.kp = 1.0 / MAX_LAT_ACCEL
+    tune.torque.kf = 1.0 / MAX_LAT_ACCEL
+    tune.torque.ki = 0.1 / MAX_LAT_ACCEL
+    tune.torque.friction = FRICTION
+  elif name == LatTunes.INDI_PRIUS:
     tune.init('indi')
     tune.indi.innerLoopGainBP = [0.]
     tune.indi.innerLoopGainV = [4.0]
@@ -75,16 +83,18 @@ def set_lat_tune(tune, name):
     tune.indi.actuatorEffectivenessV = [2, 3]
 
 
-  elif name == LatTunes.LQR_RAV4:
-    tune.init('lqr')
-    tune.lqr.scale = 1500.0
-    tune.lqr.ki = 0.05
-    tune.lqr.a = [0., 1., -0.22619643, 1.21822268]
-    tune.lqr.b = [-1.92006585e-04, 3.95603032e-05]
-    tune.lqr.c = [1., 0.]
-    tune.lqr.k = [-110.73572306, 451.22718255]
-    tune.lqr.l = [0.3233671, 0.3185757]
-    tune.lqr.dcGain = 0.002237852961363602
+  elif name == LatTunes.INDI_RAV4_TSS2:
+    tune.init('indi')
+    tune.indi.innerLoopGainBP = [5.0,  8.3,   11.1,  12.10,  13.9,  16.7,  18,    20,    25,    30]
+    tune.indi.innerLoopGainV = [3.13,  5.2,   6.66,  7.8,    8.8,   10,    10.8,  12,    15,  15]
+    tune.indi.outerLoopGainBP = [5.0,  8.3,   11.1,  12.10,  13.9,  16.7,  18,    20,    25,    30]
+    tune.indi.outerLoopGainV = [2.9, 4.97,   6.51,  7.65,  8.58,  9.78,  10.58, 11.83, 14.9, 14.998]
+     # inner - outer difference 0.23, 0.23,  0.15,  0.15,  0.22,  0.22 , 0.22,  0.16,  0.1,  0.002
+    tune.indi.timeConstantBP = [5.0,  8.33,   11.1,  13.9,   16.7,  17.5,  18,    20,  22.5, 23.88, 23.89,  30,  40]
+    tune.indi.timeConstantV = [0.1461, 0.2427, 0.27871, 0.3, 0,3,   0.3,   0.5, 0.6250, 0.85, 1.1,  2.2,   2.6,  3.2]
+    #tune.indi.timeConstantV = [0.1461, 0.2427, 0.27871, 0.312, 0,315, 0.32, 0.525, 0.6250, 0.85, 1.1, 1.2, 2.3, 2.6]
+    tune.indi.actuatorEffectivenessBP = [10, 13, 14, 25, 33]
+    tune.indi.actuatorEffectivenessV = [15, 13, 13, 15, 15]
 
   elif 'PID' in str(name):
     tune.init('pid')
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index 66be8e2c6a49a19..50d02f648c87304 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -15,11 +15,10 @@ class CarControllerParams:
   ACCEL_MIN = -3.5  # m/s2
 
   STEER_MAX = 1500
-  STEER_DELTA_UP = 10       # 1.5s time to peak torque
+  STEER_DELTA_UP = 15       # 1.5s time to peak torque
   STEER_DELTA_DOWN = 25     # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
   STEER_ERROR_MAX = 350     # max delta between torque cmd and torque motor
 
-
 class ToyotaFlags(IntFlag):
   HYBRID = 1
 
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index f9260762ede231b..e7ab62ab69b6bdf 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -21,6 +21,7 @@
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
 from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from selfdrive.controls.lib.events import Events, ET
 from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
 from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -133,6 +134,8 @@ def __init__(self, sm=None, pm=None, can_sock=None):
       self.LaC = LatControlINDI(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'lqr':
       self.LaC = LatControlLQR(self.CP, self.CI)
+    elif self.CP.lateralTuning.which() == 'torque':
+      self.LaC = LatControlTorque(self.CP, self.CI)
 
     self.initialized = False
     self.state = State.disabled
@@ -501,7 +504,7 @@ def state_control(self, CS):
                                                                              lat_plan.curvatures,
                                                                              lat_plan.curvatureRates)
       actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(lat_active, CS, self.CP, self.VM, params, self.last_actuators,
-                                                                             desired_curvature, desired_curvature_rate)
+                                                                             desired_curvature, desired_curvature_rate, self.sm['liveLocationKalman'])
     else:
       lac_log = log.ControlsState.LateralDebugState.new_message()
       if self.sm.rcv_frame['testJoystick'] > 0 and self.active:
@@ -666,6 +669,8 @@ def publish_logs(self, CS, start_time, actuators, lac_log):
       controlsState.lateralControlState.pidState = lac_log
     elif lat_tuning == 'lqr':
       controlsState.lateralControlState.lqrState = lac_log
+    elif lat_tuning == 'torque':
+      controlsState.lateralControlState.torqueState = lac_log
     elif lat_tuning == 'indi':
       controlsState.lateralControlState.indiState = lac_log
 
diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py
index eb16aca2e8c65fe..bc3b3103ba5b5e9 100644
--- a/selfdrive/controls/lib/latcontrol.py
+++ b/selfdrive/controls/lib/latcontrol.py
@@ -13,7 +13,7 @@ def __init__(self, CP, CI):
     self.sat_count = 0.
 
   @abstractmethod
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
     pass
 
   def reset(self):
diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py
index c9353563113a28b..8d09432b6a6cd96 100644
--- a/selfdrive/controls/lib/latcontrol_angle.py
+++ b/selfdrive/controls/lib/latcontrol_angle.py
@@ -7,7 +7,7 @@
 
 
 class LatControlAngle(LatControl):
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
     angle_log = log.ControlsState.LateralAngleState.new_message()
 
     if CS.vEgo < MIN_STEER_SPEED or not active:
diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py
index dc1b31bad9def98..9fcadefb5207cf2 100644
--- a/selfdrive/controls/lib/latcontrol_indi.py
+++ b/selfdrive/controls/lib/latcontrol_indi.py
@@ -65,7 +65,7 @@ def reset(self):
     self.steer_filter.x = 0.
     self.speed = 0.
 
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
     self.speed = CS.vEgo
     # Update Kalman filter
     y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py
index f5ff5a95e596930..41eb09fa13581b6 100644
--- a/selfdrive/controls/lib/latcontrol_pid.py
+++ b/selfdrive/controls/lib/latcontrol_pid.py
@@ -1,6 +1,6 @@
 import math
 
-from selfdrive.controls.lib.pid import PIController
+from selfdrive.controls.lib.pid import PIDController
 from selfdrive.controls.lib.drive_helpers import get_steer_max
 from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
 from cereal import log
@@ -9,7 +9,7 @@
 class LatControlPID(LatControl):
   def __init__(self, CP, CI):
     super().__init__(CP, CI)
-    self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
+    self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
                             (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
                             k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0)
     self.get_steer_feedforward = CI.get_steer_feedforward_function()
@@ -18,16 +18,17 @@ def reset(self):
     super().reset()
     self.pid.reset()
 
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
     pid_log = log.ControlsState.LateralPIDState.new_message()
     pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
     pid_log.steeringRateDeg = float(CS.steeringRateDeg)
 
     angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
     angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
+    error = angle_steers_des - CS.steeringAngleDeg
 
     pid_log.steeringAngleDesiredDeg = angle_steers_des
-    pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
+    pid_log.angleError = error
     if CS.vEgo < MIN_STEER_SPEED or not active:
       output_steer = 0.0
       pid_log.active = False
@@ -40,10 +41,8 @@ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature,
       # offset does not contribute to resistive torque
       steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
 
-      deadzone = 0.0
-
-      output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, override=CS.steeringPressed,
-                                     feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
+      output_steer = self.pid.update(error, override=CS.steeringPressed,
+                                     feedforward=steer_feedforward, speed=CS.vEgo)
       pid_log.active = True
       pid_log.p = self.pid.p
       pid_log.i = self.pid.i
diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py
new file mode 100644
index 000000000000000..2816b20149794bb
--- /dev/null
+++ b/selfdrive/controls/lib/latcontrol_torque.py
@@ -0,0 +1,79 @@
+import math
+from selfdrive.controls.lib.pid import PIDController
+from common.numpy_fast import interp
+from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
+from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
+from cereal import log
+
+# At higher speeds (25+mph) we can assume:
+# Lateral acceleration achieved by a specific car correlates to
+# torque applied to the steering rack. It does not correlate to
+# wheel slip, or to speed.
+
+# This controller applies torque to achieve desired lateral
+# accelerations. To compensate for the low speed effects we
+# use a LOW_SPEED_FACTOR in the error. Additionally there is
+# friction in the steering wheel that needs to be overcome to
+# move it at all, this is compensated for too.
+
+
+LOW_SPEED_FACTOR = 200
+JERK_THRESHOLD = 0.2
+
+
+class LatControlTorque(LatControl):
+  def __init__(self, CP, CI):
+    super().__init__(CP, CI)
+    self.pid = PIDController(CP.lateralTuning.torque.kp, CP.lateralTuning.torque.ki,
+                            k_f=CP.lateralTuning.torque.kf, pos_limit=1.0, neg_limit=-1.0)
+    self.get_steer_feedforward = CI.get_steer_feedforward_function()
+    self.steer_max = 1.0
+    self.pid.pos_limit = self.steer_max
+    self.pid.neg_limit = -self.steer_max
+    self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle
+    self.friction = CP.lateralTuning.torque.friction
+
+  def reset(self):
+    super().reset()
+    self.pid.reset()
+
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
+    pid_log = log.ControlsState.LateralTorqueState.new_message()
+
+    if CS.vEgo < MIN_STEER_SPEED or not active:
+      output_torque = 0.0
+      pid_log.active = False
+      self.pid.reset()
+    else:
+      if self.use_steering_angle:
+        actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+      else:
+        actual_curvature = llk.angularVelocityCalibrated.value[2] / CS.vEgo
+      desired_lateral_accel = desired_curvature * CS.vEgo**2
+      desired_lateral_jerk = desired_curvature_rate * CS.vEgo**2
+      actual_lateral_accel = actual_curvature * CS.vEgo**2
+
+      setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature
+      measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature
+      error = setpoint - measurement
+      pid_log.error = error
+
+      ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
+      output_torque = self.pid.update(error,
+                                      override=CS.steeringPressed, feedforward=ff,
+                                      speed=CS.vEgo,
+                                      freeze_integrator=CS.steeringRateLimited)
+
+      friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction])
+      output_torque += friction_compensation
+
+      pid_log.active = True
+      pid_log.p = self.pid.p
+      pid_log.i = self.pid.i
+      pid_log.d = self.pid.d
+      pid_log.f = self.pid.f
+      pid_log.output = -output_torque
+      pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS)
+
+    #TODO left is positive in this convention
+    return -output_torque, 0.0, pid_log
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 21c34aa2d643b4e..41fe5c14b78a31f 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -1,7 +1,7 @@
 from cereal import car
 from common.numpy_fast import clip, interp
 from common.realtime import DT_CTRL
-from selfdrive.controls.lib.pid import PIController
+from selfdrive.controls.lib.pid import PIDController
 from selfdrive.controls.lib.drive_helpers import CONTROL_N
 from selfdrive.modeld.constants import T_IDXS
 
@@ -11,6 +11,14 @@
 ACCEL_MIN_ISO = -3.5  # m/s^2
 ACCEL_MAX_ISO = 2.0  # m/s^2
 
+def apply_deadzone(error, deadzone):
+  if error > deadzone:
+    error -= deadzone
+  elif error < - deadzone:
+    error += deadzone
+  else:
+    error = 0.
+  return error
 
 def long_control_state_trans(CP, active, long_control_state, v_ego, v_target_future,
                              brake_pressed, cruise_standstill):
@@ -42,9 +50,9 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, v_target_fut
 class LongControl():
   def __init__(self, CP):
     self.long_control_state = LongCtrlState.off  # initialized to off
-    self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
-                            (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
-                            rate=1 / DT_CTRL)
+    self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
+                             (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
+                             k_f = CP.longitudinalTuning.kf, rate=1 / DT_CTRL)
     self.v_pid = 0.0
     self.last_output_accel = 0.0
 
@@ -99,7 +107,9 @@ def update(self, active, CS, CP, long_plan, accel_limits):
       deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
       freeze_integrator = prevent_overshoot
 
-      output_accel = self.pid.update(self.v_pid, CS.vEgo, speed=CS.vEgo, deadzone=deadzone, feedforward=a_target, freeze_integrator=freeze_integrator)
+      error = self.v_pid - CS.vEgo
+      error_deadzone = apply_deadzone(error, deadzone)
+      output_accel = self.pid.update(error_deadzone, speed=CS.vEgo, feedforward=a_target, freeze_integrator=freeze_integrator)
 
       if prevent_overshoot:
         output_accel = min(output_accel, 0.0)
diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py
index 28c6438191f58d3..b5ef735680b0cd8 100644
--- a/selfdrive/controls/lib/pid.py
+++ b/selfdrive/controls/lib/pid.py
@@ -3,24 +3,19 @@
 
 from common.numpy_fast import clip, interp
 
-def apply_deadzone(error, deadzone):
-  if error > deadzone:
-    error -= deadzone
-  elif error < - deadzone:
-    error += deadzone
-  else:
-    error = 0.
-  return error
-
-class PIController():
-  def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100):
+
+class PIDController():
+  def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=None, neg_limit=None, rate=100):
     self._k_p = k_p  # proportional gain
     self._k_i = k_i  # integral gain
+    self._k_d = k_d   # feedforward gain
     self.k_f = k_f   # feedforward gain
     if isinstance(self._k_p, Number):
       self._k_p = [[0], [self._k_p]]
     if isinstance(self._k_i, Number):
       self._k_i = [[0], [self._k_i]]
+    if isinstance(self._k_d, Number):
+      self._k_d = [[0], [self._k_d]]
 
     self.pos_limit = pos_limit
     self.neg_limit = neg_limit
@@ -38,24 +33,29 @@ def k_p(self):
   def k_i(self):
     return interp(self.speed, self._k_i[0], self._k_i[1])
 
+  @property
+  def k_d(self):
+    return interp(self.speed, self._k_d[0], self._k_d[1])
+
   def reset(self):
     self.p = 0.0
     self.i = 0.0
+    self.d = 0.0
     self.f = 0.0
     self.control = 0
 
-  def update(self, setpoint, measurement, speed=0.0, override=False, feedforward=0., deadzone=0., freeze_integrator=False):
+  def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False):
     self.speed = speed
 
-    error = float(apply_deadzone(setpoint - measurement, deadzone))
-    self.p = error * self.k_p
+    self.p = float(error) * self.k_p
     self.f = feedforward * self.k_f
+    self.d = error_rate * self.k_d
 
     if override:
       self.i -= self.i_unwind_rate * float(np.sign(self.i))
     else:
       i = self.i + error * self.k_i * self.i_rate
-      control = self.p + self.f + i
+      control = self.p + i + self.d + self.f
 
       # Update when changing i will move the control away from the limits
       # or when i will move towards the sign of the error
@@ -64,7 +64,7 @@ def update(self, setpoint, measurement, speed=0.0, override=False, feedforward=0
          not freeze_integrator:
         self.i = i
 
-    control = self.p + self.f + self.i
+    control = self.p + self.i + self.d + self.f
 
     self.control = clip(control, self.neg_limit, self.pos_limit)
     return self.control
diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py
index b56961b1cd242f8..96f692d2e896e93 100755
--- a/selfdrive/thermald/thermald.py
+++ b/selfdrive/thermald/thermald.py
@@ -19,7 +19,7 @@
 from common.params import Params
 from common.realtime import DT_TRML, sec_since_boot
 from selfdrive.controls.lib.alertmanager import set_offroad_alert
-from selfdrive.controls.lib.pid import PIController
+from selfdrive.controls.lib.pid import PIDController
 from selfdrive.hardware import EON, HARDWARE, PC, TICI
 from selfdrive.loggerd.config import get_available_percent
 from selfdrive.statsd import statlog
@@ -140,9 +140,9 @@ def handle_fan_tici(controller, max_cpu_temp, fan_speed, ignition):
   if ignition != last_ignition:
     controller.reset()
 
+  error = 70 - max_cpu_temp
   fan_pwr_out = -int(controller.update(
-                     setpoint=75,
-                     measurement=max_cpu_temp,
+                     error=error,
                      feedforward=interp(max_cpu_temp, [60.0, 100.0], [0, -80])
                   ))
 
@@ -240,7 +240,7 @@ def thermald_thread(end_event, hw_queue):
   thermal_config = HARDWARE.get_thermal_config()
 
   # TODO: use PI controller for UNO
-  controller = PIController(k_p=0, k_i=2e-3, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML))
+  controller = PIDController(k_p=0, k_i=4e-3, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML))
 
   while not end_event.is_set():
     sm.update(PANDA_STATES_TIMEOUT)
diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc
index 2bb14cfdaf22309..1229cff264efdc7 100644
--- a/selfdrive/ui/qt/offroad/settings.cc
+++ b/selfdrive/ui/qt/offroad/settings.cc
@@ -73,8 +73,8 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
     },
     {
       "LqrTune",
-      "Use LQR/INDI Tune",
-      "Use LQR tuning values for select Hondas. Use INDI for select Toyotas..",
+      "Use LQR/Torque Tune",
+      "Use LQR controller for select Hondas & Use Torque controller for select Toyotas..",
       "../assets/offroad/icon_openpilot.png",
     },
 #ifdef ENABLE_MAPS