Subaru: Stop and Go autoresume (#19)

* Subaru: Stop and Go autoresume * sync with subaru-community * fix panda * cleanup * use int flag and safety param, only block message when sng is allowed * block tx if sng is not allowed on certain platforms
sunnypilot · Sep 29, 2023 · a83841a · a83841a
1 parent 235217b
commit a83841a
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 3 deletions.
diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
@@ -37,6 +37,7 @@ const LongitudinalLimits SUBARU_LONG_LIMITS = {
 #define MSG_SUBARU_Throttle              0x40
 #define MSG_SUBARU_Steering_Torque       0x119
 #define MSG_SUBARU_Wheel_Speeds          0x13a
+#define MSG_SUBARU_Brake_Pedal           0x139
 
 #define MSG_SUBARU_ES_LKAS               0x122
 #define MSG_SUBARU_ES_LKAS_ANGLE         0x124
@@ -63,6 +64,8 @@ const LongitudinalLimits SUBARU_LONG_LIMITS = {
   {MSG_SUBARU_ES_DashStatus,     SUBARU_MAIN_BUS, 8}, \
   {MSG_SUBARU_ES_LKAS_State,     SUBARU_MAIN_BUS, 8}, \
   {MSG_SUBARU_ES_Infotainment,   SUBARU_MAIN_BUS, 8}, \
+  {MSG_SUBARU_Throttle,          SUBARU_CAM_BUS,  8}, \
+  {MSG_SUBARU_Brake_Pedal,       SUBARU_CAM_BUS,  8}, \
 
 #define SUBARU_COMMON_LONG_TX_MSGS(alt_bus)           \
   {MSG_SUBARU_ES_Brake,          alt_bus,         8}, \
@@ -121,10 +124,12 @@ addr_checks subaru_gen2_rx_checks = {subaru_gen2_addr_checks, SUBARU_GEN2_ADDR_C
 const uint16_t SUBARU_PARAM_GEN2 = 1;
 const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
 const uint16_t SUBARU_PARAM_MAX_STEER_2018 = 4;
+const uint16_t SUBARU_PARAM_SNG = 1024;
 
 bool subaru_gen2 = false;
 bool subaru_longitudinal = false;
 bool subaru_max_steer_2018_crosstrek = false;
+bool subaru_sng = false;
 
 
 static uint32_t subaru_get_checksum(CANPacket_t *to_push) {
@@ -272,6 +277,10 @@ static int subaru_tx_hook(CANPacket_t *to_send) {
     violation |= !(is_tester_present || is_button_rdbi);
   }
 
+  if ((addr == MSG_SUBARU_Throttle) || (addr == MSG_SUBARU_Brake_Pedal)) {
+    violation |= !subaru_sng;
+  }
+
   if (violation){
     tx = 0;
   }
@@ -282,7 +291,10 @@ static int subaru_fwd_hook(int bus_num, int addr) {
   int bus_fwd = -1;
 
   if (bus_num == SUBARU_MAIN_BUS) {
-    bus_fwd = SUBARU_CAM_BUS;  // to the eyesight camera
+    bool block_msg = subaru_sng && ((addr == MSG_SUBARU_Throttle) || (addr == MSG_SUBARU_Brake_Pedal));
+    if (!block_msg) {
+      bus_fwd = SUBARU_CAM_BUS;  // to the eyesight camera
+    }
   }
 
   if (bus_num == SUBARU_CAM_BUS) {
@@ -308,6 +320,7 @@ static int subaru_fwd_hook(int bus_num, int addr) {
 static const addr_checks* subaru_init(uint16_t param) {
   subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
   subaru_max_steer_2018_crosstrek = GET_FLAG(param, SUBARU_PARAM_MAX_STEER_2018);
+  subaru_sng = GET_FLAG(param, SUBARU_PARAM_SNG);
 
 #ifdef ALLOW_DEBUG
   subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);

diff --git a/board/safety/safety_subaru_preglobal.h b/board/safety/safety_subaru_preglobal.h
@@ -26,7 +26,8 @@ const SteeringLimits SUBARU_PG_STEERING_LIMITS = {
 
 const CanMsg SUBARU_PG_TX_MSGS[] = {
   {MSG_SUBARU_PG_ES_Distance, SUBARU_PG_MAIN_BUS, 8},
-  {MSG_SUBARU_PG_ES_LKAS,     SUBARU_PG_MAIN_BUS, 8}
+  {MSG_SUBARU_PG_ES_LKAS,     SUBARU_PG_MAIN_BUS, 8},
+  {MSG_SUBARU_PG_Throttle,    SUBARU_PG_CAM_BUS,  8},
 };
 #define SUBARU_PG_TX_MSGS_LEN (sizeof(SUBARU_PG_TX_MSGS) / sizeof(SUBARU_PG_TX_MSGS[0]))
 
@@ -40,7 +41,9 @@ AddrCheckStruct subaru_preglobal_addr_checks[] = {
 addr_checks subaru_preglobal_rx_checks = {subaru_preglobal_addr_checks, SUBARU_PG_ADDR_CHECK_LEN};
 
 const uint16_t SUBARU_L_PARAM_FLIP_DRIVER_TORQUE = 1;
+const uint16_t SUBARU_L_PARAM_SNG = 1024;
 bool subaru_l_flip_driver_torque = false;
+bool subaru_l_sng = false;
 
 static int subaru_preglobal_rx_hook(CANPacket_t *to_push) {
 
@@ -108,14 +111,24 @@ static int subaru_preglobal_tx_hook(CANPacket_t *to_send) {
     }
 
   }
+
+  if (addr == MSG_SUBARU_PG_Throttle) {
+    if (!subaru_l_sng) {
+      tx = 0;
+    }
+  }
+
   return tx;
 }
 
 static int subaru_preglobal_fwd_hook(int bus_num, int addr) {
   int bus_fwd = -1;
 
   if (bus_num == SUBARU_PG_MAIN_BUS) {
-    bus_fwd = SUBARU_PG_CAM_BUS;  // Camera CAN
+    bool block_msg = subaru_l_sng && (addr == MSG_SUBARU_PG_Throttle);
+    if (!block_msg) {
+      bus_fwd = SUBARU_PG_CAM_BUS;  // Camera CAN
+    }
   }
 
   if (bus_num == SUBARU_PG_CAM_BUS) {
@@ -131,6 +144,7 @@ static int subaru_preglobal_fwd_hook(int bus_num, int addr) {
 static const addr_checks* subaru_preglobal_init(uint16_t param) {
   // Checking for flip driver torque from safety parameter
   subaru_l_flip_driver_torque = GET_FLAG(param, SUBARU_L_PARAM_FLIP_DRIVER_TORQUE);
+  subaru_l_sng = GET_FLAG(param, SUBARU_L_PARAM_SNG);
   return &subaru_preglobal_rx_checks;
 }
 

diff --git a/python/__init__.py b/python/__init__.py
@@ -246,6 +246,8 @@ class Panda:
 
   FLAG_SUBARU_LEGACY_FLIP_DRIVER_TORQUE = 1
 
+  FLAG_SUBARU_SNG = 1024
+
   FLAG_NISSAN_ALT_EPS_BUS = 1
 
   FLAG_GM_HW_CAM = 1