allow alt lkas

martinl · Jul 20, 2023 · 2ed2d02 · 2ed2d02
1 parent db5b407
commit 2ed2d02
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 1 deletion.
diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
@@ -65,6 +65,12 @@ const CanMsg SUBARU_GEN2_TX_MSGS[] = {
 };
 #define SUBARU_GEN2_TX_MSGS_LEN (sizeof(SUBARU_GEN2_TX_MSGS) / sizeof(SUBARU_GEN2_TX_MSGS[0]))
 
+const CanMsg SUBARU_LKAS_ALT_TX_MSGS[] = {
+  SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
+  {MSG_SUBARU_ES_LKAS_ALT,         SUBARU_MAIN_BUS, 8}, \
+};
+#define SUBARU_LKAS_ALT_TX_MSGS_LEN (sizeof(SUBARU_LKAS_ALT_TX_MSGS) / sizeof(SUBARU_LKAS_ALT_TX_MSGS[0]))
+
 AddrCheckStruct subaru_addr_checks[] = {
   SUBARU_COMMON_ADDR_CHECKS(SUBARU_MAIN_BUS)
   SUBARU_GEN12_ADDR_CHECKS(SUBARU_MAIN_BUS)
@@ -166,6 +172,8 @@ static int subaru_tx_hook(CANPacket_t *to_send) {
 
   if (subaru_gen2) {
     tx = msg_allowed(to_send, SUBARU_GEN2_TX_MSGS, SUBARU_GEN2_TX_MSGS_LEN);
+  } else if(lkas_alt) {
+    tx = msg_allowed(to_send, SUBARU_LKAS_ALT_TX_MSGS, SUBARU_LKAS_ALT_TX_MSGS_LEN);
   } else {
     tx = msg_allowed(to_send, SUBARU_TX_MSGS, SUBARU_TX_MSGS_LEN);
   }
@@ -181,6 +189,18 @@ static int subaru_tx_hook(CANPacket_t *to_send) {
     }
   }
 
+  // only allow zero steer and no steer request for now
+  if ((addr == MSG_SUBARU_ES_LKAS_ALT)) {
+    int desired_angle = ((GET_BYTES(to_send, 4, 4) >> 8) & 0x3FFFFU);
+    desired_angle = -1 * to_signed(desired_angle, 17);
+
+    int request_angle = GET_BIT(to_send, 12);
+
+    if(desired_angle != 0 || request_angle != 0){
+      tx = 0;
+    }
+  }
+
   return tx;
 }
 

diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py
@@ -108,7 +108,7 @@ class TestSubaruGen2Safety(TestSubaruGen2SafetyBase):
   TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS)
 
 class TestSubaruForester2022Safety(TestSubaruSafetyBase):
-  TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS)
+  TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS) + [[MSG_SUBARU_ES_LKAS_ALT, SUBARU_MAIN_BUS]]
   RELAY_MALFUNCTION_ADDR = MSG_SUBARU_ES_LKAS_ALT
   FWD_BLACKLISTED_ADDRS = fwd_blacklisted_addr(MSG_SUBARU_ES_LKAS_ALT)
 
@@ -118,6 +118,15 @@ def _pcm_status_msg(self, enable):
     values = {"Cruise_Activated": enable}
     return self.packer.make_can_msg_panda("ES_Status", 2, values)
 
+  def _angle_cmd_msg(self, angle, steer_req=1):
+    values = {"LKAS_Output": angle, "LKAS_Request": steer_req}
+    return self.packer.make_can_msg_panda("ES_LKAS_ALT", 0, values)
+
+  def test_alt_lkas_msg(self):
+    self.assertTrue(self._tx(self._angle_cmd_msg(0, 0)))
+    self.assertFalse(self._tx(self._angle_cmd_msg(0, 1)))
+    self.assertFalse(self._tx(self._angle_cmd_msg(10, 0)))
+    self.assertFalse(self._tx(self._angle_cmd_msg(-10, 0)))
 
 if __name__ == "__main__":
   unittest.main()