Subaru: infrastructure to support the new angle based controllers (co…

…mmaai#1590)

board/safety/safety_subaru.h

@@ -13,6 +13,17 @@
 const SteeringLimits SUBARU_STEERING_LIMITS      = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
 const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40);
 
+const SteeringLimits SUBARU_ANGLE_STEERING_LIMITS = {
+  .angle_deg_to_can = 100,
+  .angle_rate_up_lookup = {
+    {0., 5., 15.},
+    {5., .8, .15}
+  },
+  .angle_rate_down_lookup = {
+    {0., 5., 15.},
+    {5., 3.5, .4}
+  },
+};
 
 const LongitudinalLimits SUBARU_LONG_LIMITS = {
   .min_gas = 808,       // appears to be engine braking
@@ -31,6 +42,7 @@ const LongitudinalLimits SUBARU_LONG_LIMITS = {
 #define MSG_SUBARU_Wheel_Speeds          0x13a
 
 #define MSG_SUBARU_ES_LKAS               0x122
+#define MSG_SUBARU_ES_LKAS_ANGLE         0x124
 #define MSG_SUBARU_ES_Brake              0x220
 #define MSG_SUBARU_ES_Distance           0x221
 #define MSG_SUBARU_ES_Status             0x222
@@ -42,8 +54,8 @@ const LongitudinalLimits SUBARU_LONG_LIMITS = {
 #define SUBARU_ALT_BUS  1
 #define SUBARU_CAM_BUS  2
 
-#define SUBARU_COMMON_TX_MSGS(alt_bus)              \
-  {MSG_SUBARU_ES_LKAS,         SUBARU_MAIN_BUS, 8}, \
+#define SUBARU_COMMON_TX_MSGS(alt_bus, lkas_msg)    \
+  {lkas_msg,                   SUBARU_MAIN_BUS, 8}, \
   {MSG_SUBARU_ES_Distance,     alt_bus,         8}, \
   {MSG_SUBARU_ES_DashStatus,   SUBARU_MAIN_BUS, 8}, \
   {MSG_SUBARU_ES_LKAS_State,   SUBARU_MAIN_BUS, 8}, \
@@ -61,18 +73,18 @@ const LongitudinalLimits SUBARU_LONG_LIMITS = {
   {.msg = {{MSG_SUBARU_CruiseControl,   alt_bus,         8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U}, { 0 }, { 0 }}}, \
 
 const CanMsg SUBARU_TX_MSGS[] = {
-  SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
+  SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
 };
 #define SUBARU_TX_MSGS_LEN (sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0]))
 
 const CanMsg SUBARU_LONG_TX_MSGS[] = {
-  SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS)
+  SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
   SUBARU_COMMON_LONG_TX_MSGS(SUBARU_MAIN_BUS)
 };
 #define SUBARU_LONG_TX_MSGS_LEN (sizeof(SUBARU_LONG_TX_MSGS) / sizeof(SUBARU_LONG_TX_MSGS[0]))
 
 const CanMsg SUBARU_GEN2_TX_MSGS[] = {
-  SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS)
+  SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
 };
 #define SUBARU_GEN2_TX_MSGS_LEN (sizeof(SUBARU_GEN2_TX_MSGS) / sizeof(SUBARU_GEN2_TX_MSGS[0]))
 
@@ -91,9 +103,11 @@ 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_LKAS_ANGLE = 4;
 
 bool subaru_gen2 = false;
 bool subaru_longitudinal = false;
+bool subaru_lkas_angle = false;
 
 
 static uint32_t subaru_get_checksum(CANPacket_t *to_push) {
@@ -191,6 +205,15 @@ static int subaru_tx_hook(CANPacket_t *to_send) {
     violation |= steer_torque_cmd_checks(desired_torque, -1, limits);
   }
 
+  if (addr == MSG_SUBARU_ES_LKAS_ANGLE) {
+    int desired_angle = (GET_BYTES(to_send, 5, 3) & 0x1FFFFU);
+    desired_angle = to_signed(desired_angle, 17);
+    bool lkas_request = GET_BIT(to_send, 12U);
+
+    const SteeringLimits limits = SUBARU_ANGLE_STEERING_LIMITS;
+    violation |= steer_angle_cmd_checks(desired_angle, lkas_request, limits);
+  }
+
   // check es_brake brake_pressure limits
   if (addr == MSG_SUBARU_ES_Brake) {
     int es_brake_pressure = GET_BYTES(to_send, 2, 2);
@@ -255,7 +278,8 @@ static const addr_checks* subaru_init(uint16_t param) {
   subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
 
 #ifdef ALLOW_DEBUG
-  subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL) && !subaru_gen2;
+  subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
+  subaru_lkas_angle = GET_FLAG(param, SUBARU_PARAM_LKAS_ANGLE);
 #endif
 
   if (subaru_gen2) {

python/__init__.py

@@ -249,6 +249,7 @@ class Panda:
 
   FLAG_SUBARU_GEN2 = 1
   FLAG_SUBARU_LONG = 2
+  FLAG_SUBARU_LKAS_ANGLE = 4
 
   FLAG_GM_HW_CAM = 1
   FLAG_GM_HW_CAM_LONG = 2

tests/safety/test_subaru.py

@@ -13,6 +13,7 @@
 MSG_SUBARU_Steering_Torque  = 0x119
 MSG_SUBARU_Wheel_Speeds     = 0x13a
 MSG_SUBARU_ES_LKAS          = 0x122
+MSG_SUBARU_ES_LKAS_ANGLE    = 0x124
 MSG_SUBARU_ES_Brake         = 0x220
 MSG_SUBARU_ES_Distance      = 0x221
 MSG_SUBARU_ES_Status        = 0x222
@@ -25,8 +26,8 @@
 SUBARU_CAM_BUS  = 2
 
 
-def lkas_tx_msgs(alt_bus):
-  return [[MSG_SUBARU_ES_LKAS,          SUBARU_MAIN_BUS],
+def lkas_tx_msgs(alt_bus, lkas_msg=MSG_SUBARU_ES_LKAS):
+  return [[lkas_msg,                    SUBARU_MAIN_BUS],
           [MSG_SUBARU_ES_Distance,      alt_bus],
           [MSG_SUBARU_ES_DashStatus,    SUBARU_MAIN_BUS],
           [MSG_SUBARU_ES_LKAS_State,    SUBARU_MAIN_BUS],
@@ -36,8 +37,8 @@ def long_tx_msgs():
   return [[MSG_SUBARU_ES_Brake,         SUBARU_MAIN_BUS],
           [MSG_SUBARU_ES_Status,        SUBARU_MAIN_BUS]]
 
-def fwd_blacklisted_addr():
-  return {SUBARU_CAM_BUS: [MSG_SUBARU_ES_LKAS, MSG_SUBARU_ES_DashStatus, MSG_SUBARU_ES_LKAS_State, MSG_SUBARU_ES_Infotainment]}
+def fwd_blacklisted_addr(lkas_msg=MSG_SUBARU_ES_LKAS):
+  return {SUBARU_CAM_BUS: [lkas_msg, MSG_SUBARU_ES_DashStatus, MSG_SUBARU_ES_LKAS_State, MSG_SUBARU_ES_Infotainment]}
 
 class TestSubaruSafetyBase(common.PandaSafetyTest, MeasurementSafetyTest):
   FLAGS = 0
@@ -151,6 +152,30 @@ def _torque_cmd_msg(self, torque, steer_req=1):
     return self.packer.make_can_msg_panda("ES_LKAS", SUBARU_MAIN_BUS, values)
 
 
+class TestSubaruAngleSafetyBase(TestSubaruSafetyBase, common.AngleSteeringSafetyTest):
+  TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
+  RELAY_MALFUNCTION_ADDR = MSG_SUBARU_ES_LKAS_ANGLE
+  FWD_BLACKLISTED_ADDRS = fwd_blacklisted_addr(MSG_SUBARU_ES_LKAS_ANGLE)
+
+  FLAGS = Panda.FLAG_SUBARU_LKAS_ANGLE
+
+  ANGLE_RATE_BP = [0, 5, 15]
+  ANGLE_RATE_UP = [5, 0.8, 0.15]
+  ANGLE_RATE_DOWN = [5, 3.5, 0.4]
+
+  def _angle_cmd_msg(self, angle, enabled=1):
+    values = {"LKAS_Output": angle, "LKAS_Request": enabled}
+    return self.packer.make_can_msg_panda("ES_LKAS_ANGLE", 0, values)
+
+  def _angle_meas_msg(self, angle):
+    values = {"Steering_Angle": angle}
+    return self.packer.make_can_msg_panda("Steering_Torque", 0, values)
+
+  def _pcm_status_msg(self, enable):
+    values = {"Cruise_Activated": enable}
+    return self.packer.make_can_msg_panda("ES_Status", self.ALT_CAM_BUS, values)
+
+
 class TestSubaruGen1TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruTorqueSafetyBase):
   FLAGS = 0
   TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS)