From 3517306113834d77a77552f522b53dfddfce5786 Mon Sep 17 00:00:00 2001
From: Adeeb <8762862+quillford@users.noreply.github.com>
Date: Tue, 28 Apr 2020 10:47:26 -0700
Subject: [PATCH] Subaru: fix steer torque scaling (#501)

* Subaru: fix steer torque scaling

* fix legacy test
---
 board/safety/safety_subaru.h | 5 +++--
 tests/safety/test_subaru.py  | 6 +++---
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
index b65b8d0..17bd699 100644
--- a/board/safety/safety_subaru.h
+++ b/board/safety/safety_subaru.h
@@ -70,10 +70,11 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       int torque_driver_new;
       if (subaru_global) {
         torque_driver_new = ((GET_BYTES_04(to_push) >> 16) & 0x7FF);
+        torque_driver_new = -1 * to_signed(torque_driver_new, 11);
       } else {
         torque_driver_new = (GET_BYTE(to_push, 3) >> 5) + (GET_BYTE(to_push, 4) << 3);
+        torque_driver_new = to_signed(torque_driver_new, 11);
       }
-      torque_driver_new = to_signed(torque_driver_new, 11);
       update_sample(&torque_driver, torque_driver_new);
     }
 
@@ -148,7 +149,7 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
     int desired_torque = ((GET_BYTES_04(to_send) >> bit_shift) & 0x1FFF);
     bool violation = 0;
     uint32_t ts = TIM2->CNT;
-    desired_torque = to_signed(desired_torque, 13);
+    desired_torque = -1 * to_signed(desired_torque, 13);
 
     if (controls_allowed) {
 
diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py
index 078e4b3..08bb25b 100644
--- a/tests/safety/test_subaru.py
+++ b/tests/safety/test_subaru.py
@@ -42,7 +42,7 @@ class TestSubaruSafety(common.PandaSafetyTest):
     self.safety.set_rt_torque_last(t)
 
   def _torque_driver_msg(self, torque):
-    values = {"Steer_Torque_Sensor": -torque, "counter": self.cnt_torque_driver % 4}
+    values = {"Steer_Torque_Sensor": torque, "counter": self.cnt_torque_driver % 4}
     self.__class__.cnt_torque_driver += 1
     return self.packer.make_can_msg_panda("Steering_Torque", 0, values)
 
@@ -59,7 +59,7 @@ class TestSubaruSafety(common.PandaSafetyTest):
     return self.packer.make_can_msg_panda("Brake_Pedal", 0, values)
 
   def _torque_msg(self, torque):
-    values = {"LKAS_Output": -torque}
+    values = {"LKAS_Output": torque}
     return self.packer.make_can_msg_panda("ES_LKAS", 0, values)
 
   def _gas_msg(self, gas):
@@ -194,7 +194,7 @@ class TestSubaruLegacySafety(TestSubaruSafety):
     return self.packer.make_can_msg_panda("Steering_Torque", 0, values)
 
   def _torque_msg(self, torque):
-    values = {"LKAS_Command": -torque}
+    values = {"LKAS_Command": torque}
     return self.packer.make_can_msg_panda("ES_LKAS", 0, values)
 
   def _gas_msg(self, gas):