safety: add safety param for gas interceptor (commaai#1735)

* some refactoring still needing to be done here

* 50hz

* set rx checks

* tx msgs

* test

* fix

* forgot to check this since it's static now

* enable_gas_interceptor

* more

* remove int funcs since are common

* generic!

* not generic

* need to set counter now

* set_enable_gas_interceptor not needed

* already reset in safety init

* fix test

* remove gas_interceptor_detected (partly)

* oof need for hjonda too

* edit structs

* more honda (still broken)

* fix typo

* shift these down

* can also make this extensible

* stash

* defining classes is much simpler, maybe in future we will have some sort of wrapper after some more thought

* remove create_interceptor_test now

* add lta interceptor

* clean up print

* clean up

* once

* type hint

* rm

* no interceptor with stock long + good test

* add interceptor counter to honda

* need alt interceptor + set rx checks for honda

* we were never testing button enable with nidec + interceptor!

honda almost all passing

* these fixes make sense

* clean up

* better

* clean up test cov

* clean up

* not needed

* fix tx msgs

* clean up

* cppcheck

* fix

* unnecessary

Author: sshane

board/health.h

@@ -1,6 +1,6 @@
 // When changing these structs, python/__init__.py needs to be kept up to date!
 
-#define HEALTH_PACKET_VERSION 14
+#define HEALTH_PACKET_VERSION 15
 struct __attribute__((packed)) health_t {
   uint32_t uptime_pkt;
   uint32_t voltage_pkt;
@@ -14,7 +14,6 @@ struct __attribute__((packed)) health_t {
   uint8_t ignition_line_pkt;
   uint8_t ignition_can_pkt;
   uint8_t controls_allowed_pkt;
-  uint8_t gas_interceptor_detected_pkt;
   uint8_t car_harness_status_pkt;
   uint8_t safety_mode_pkt;
   uint16_t safety_param_pkt;

board/main_comms.h

@@ -17,7 +17,6 @@ int get_health_pkt(void *dat) {
   health->ignition_can_pkt = (uint8_t)(ignition_can);
 
   health->controls_allowed_pkt = controls_allowed;
-  health->gas_interceptor_detected_pkt = gas_interceptor_detected;
   health->safety_tx_blocked_pkt = safety_tx_blocked;
   health->safety_rx_invalid_pkt = safety_rx_invalid;
   health->tx_buffer_overflow_pkt = tx_buffer_overflow;

board/safety.h

@@ -332,7 +332,7 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
   // reset state set by safety mode
   safety_mode_cnt = 0U;
   relay_malfunction = false;
-  gas_interceptor_detected = false;
+  enable_gas_interceptor = false;
   gas_interceptor_prev = 0;
   gas_pressed = false;
   gas_pressed_prev = false;

board/safety/safety_honda.h

@@ -1,4 +1,5 @@
-const CanMsg HONDA_N_TX_MSGS[] = {{0xE4, 0, 5}, {0x194, 0, 4}, {0x1FA, 0, 8}, {0x200, 0, 6}, {0x30C, 0, 8}, {0x33D, 0, 5}};
+const CanMsg HONDA_N_TX_MSGS[] = {{0xE4, 0, 5}, {0x194, 0, 4}, {0x1FA, 0, 8}, {0x30C, 0, 8}, {0x33D, 0, 5}};
+const CanMsg HONDA_N_INTERCEPTOR_TX_MSGS[] = {{0xE4, 0, 5}, {0x194, 0, 4}, {0x1FA, 0, 8}, {0x200, 0, 6}, {0x30C, 0, 8}, {0x33D, 0, 5}};
 const CanMsg HONDA_BOSCH_TX_MSGS[] = {{0xE4, 0, 5}, {0xE5, 0, 8}, {0x296, 1, 4}, {0x33D, 0, 5}, {0x33DA, 0, 5}, {0x33DB, 0, 8}};  // Bosch
 const CanMsg HONDA_BOSCH_LONG_TX_MSGS[] = {{0xE4, 1, 5}, {0x1DF, 1, 8}, {0x1EF, 1, 8}, {0x1FA, 1, 8}, {0x30C, 1, 8}, {0x33D, 1, 5}, {0x33DA, 1, 5}, {0x33DB, 1, 8}, {0x39F, 1, 8}, {0x18DAB0F1, 1, 8}};  // Bosch w/ gas and brakes
 const CanMsg HONDA_RADARLESS_TX_MSGS[] = {{0xE4, 0, 5}, {0x296, 2, 4}, {0x33D, 0, 8}};  // Bosch radarless
@@ -46,6 +47,11 @@ RxCheck honda_common_rx_checks[] = {
   HONDA_COMMON_RX_CHECKS(0)
 };
 
+RxCheck honda_common_interceptor_rx_checks[] = {
+  HONDA_COMMON_RX_CHECKS(0)
+  {.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+};
+
 RxCheck honda_common_alt_brake_rx_checks[] = {
   HONDA_COMMON_RX_CHECKS(0)
   HONDA_ALT_BRAKE_ADDR_CHECK(0)
@@ -56,6 +62,11 @@ RxCheck honda_nidec_alt_rx_checks[] = {
   HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(0)
 };
 
+RxCheck honda_nidec_alt_interceptor_rx_checks[] = {
+  HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(0)
+  {.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+};
+
 // Bosch has pt on bus 1, verified 0x1A6 does not exist
 RxCheck honda_bosch_rx_checks[] = {
   HONDA_COMMON_RX_CHECKS(1)
@@ -70,6 +81,7 @@ const uint16_t HONDA_PARAM_ALT_BRAKE = 1;
 const uint16_t HONDA_PARAM_BOSCH_LONG = 2;
 const uint16_t HONDA_PARAM_NIDEC_ALT = 4;
 const uint16_t HONDA_PARAM_RADARLESS = 8;
+const uint16_t HONDA_PARAM_GAS_INTERCEPTOR = 16;
 
 enum {
   HONDA_BTN_NONE = 0,
@@ -115,13 +127,22 @@ static uint32_t honda_compute_checksum(CANPacket_t *to_push) {
 }
 
 static uint8_t honda_get_counter(CANPacket_t *to_push) {
-  int counter_byte = GET_LEN(to_push) - 1U;
-  return ((uint8_t)(GET_BYTE(to_push, counter_byte)) >> 4U) & 0x3U;
+  int addr = GET_ADDR(to_push);
+
+  uint8_t cnt = 0U;
+  if (addr == 0x201) {
+    // Signal: COUNTER_PEDAL
+    cnt = GET_BYTE(to_push, 4) & 0x0FU;
+  } else {
+    int counter_byte = GET_LEN(to_push) - 1U;
+    cnt = (GET_BYTE(to_push, counter_byte) >> 4U) & 0x3U;
+  }
+  return cnt;
 }
 
 static void honda_rx_hook(CANPacket_t *to_push) {
   const bool pcm_cruise = ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) || \
-                          ((honda_hw == HONDA_NIDEC) && !gas_interceptor_detected);
+                          ((honda_hw == HONDA_NIDEC) && !enable_gas_interceptor);
   int pt_bus = honda_get_pt_bus();
 
   int addr = GET_ADDR(to_push);
@@ -198,14 +219,13 @@ static void honda_rx_hook(CANPacket_t *to_push) {
   }
 
   // length check because bosch hardware also uses this id (0x201 w/ len = 8)
-  if ((addr == 0x201) && (len == 6)) {
-    gas_interceptor_detected = 1;
+  if ((addr == 0x201) && (len == 6) && enable_gas_interceptor) {
     int gas_interceptor = HONDA_GET_INTERCEPTOR(to_push);
     gas_pressed = gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD;
     gas_interceptor_prev = gas_interceptor;
   }
 
-  if (!gas_interceptor_detected) {
+  if (!enable_gas_interceptor) {
     if (addr == 0x17C) {
       gas_pressed = GET_BYTE(to_push, 0) != 0U;
     }
@@ -359,12 +379,21 @@ static safety_config honda_nidec_init(uint16_t param) {
   honda_alt_brake_msg = false;
   honda_bosch_long = false;
   honda_bosch_radarless = false;
+  enable_gas_interceptor = GET_FLAG(param, HONDA_PARAM_GAS_INTERCEPTOR);
 
   safety_config ret;
   if (GET_FLAG(param, HONDA_PARAM_NIDEC_ALT)) {
-    ret = BUILD_SAFETY_CFG(honda_nidec_alt_rx_checks, HONDA_N_TX_MSGS);
+    enable_gas_interceptor ? SET_RX_CHECKS(honda_nidec_alt_interceptor_rx_checks, ret) : \
+                             SET_RX_CHECKS(honda_nidec_alt_rx_checks, ret);
+  } else {
+    enable_gas_interceptor ? SET_RX_CHECKS(honda_common_interceptor_rx_checks, ret) : \
+                             SET_RX_CHECKS(honda_common_rx_checks, ret);
+  }
+
+  if (enable_gas_interceptor) {
+    SET_TX_MSGS(HONDA_N_INTERCEPTOR_TX_MSGS, ret);
   } else {
-    ret= BUILD_SAFETY_CFG(honda_common_rx_checks, HONDA_N_TX_MSGS);
+    SET_TX_MSGS(HONDA_N_TX_MSGS, ret);
   }
   return ret;
 }

board/safety/safety_toyota.h

@@ -43,10 +43,19 @@ const LongitudinalLimits TOYOTA_LONG_LIMITS = {
 const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805;
 #define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks
 
-const CanMsg TOYOTA_TX_MSGS[] = {{0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8},  // DSU bus 0
-                                 {0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4},  // DSU bus 1
-                                 {0x2E4, 0, 5}, {0x191, 0, 8}, {0x411, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8},  // LKAS + ACC
-                                 {0x200, 0, 6}};  // gas interceptor
+#define TOYOTA_COMMON_TX_MSGS                                                                                                               \
+  {0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8},  /* DSU bus 0 */  \
+  {0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4},  /* DSU bus 1 */                                               \
+  {0x2E4, 0, 5}, {0x191, 0, 8}, {0x411, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8},  /* LKAS + ACC */                               \
+
+const CanMsg TOYOTA_TX_MSGS[] = {
+  TOYOTA_COMMON_TX_MSGS
+};
+
+const CanMsg TOYOTA_INTERCEPTOR_TX_MSGS[] = {
+  TOYOTA_COMMON_TX_MSGS
+  {0x200, 0, 6},  // gas interceptor
+};
 
 #define TOYOTA_COMMON_RX_CHECKS(lta)                                                                        \
   {.msg = {{ 0xaa, 0, 8, .check_checksum = false, .frequency = 83U}, { 0 }, { 0 }}},                        \
@@ -59,18 +68,29 @@ RxCheck toyota_lka_rx_checks[] = {
   TOYOTA_COMMON_RX_CHECKS(false)
 };
 
+RxCheck toyota_lka_interceptor_rx_checks[] = {
+  TOYOTA_COMMON_RX_CHECKS(false)
+  {.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+};
+
+// Check the quality flag for angle measurement when using LTA, since it's not set on TSS-P cars
 RxCheck toyota_lta_rx_checks[] = {
-  // Check the quality flag for angle measurement when using LTA, since it's not set on TSS-P cars
   TOYOTA_COMMON_RX_CHECKS(true)
 };
 
+RxCheck toyota_lta_interceptor_rx_checks[] = {
+  TOYOTA_COMMON_RX_CHECKS(true)
+  {.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+};
+
 // safety param flags
 // first byte is for EPS factor, second is for flags
 const uint32_t TOYOTA_PARAM_OFFSET = 8U;
 const uint32_t TOYOTA_EPS_FACTOR = (1U << TOYOTA_PARAM_OFFSET) - 1U;
 const uint32_t TOYOTA_PARAM_ALT_BRAKE = 1U << TOYOTA_PARAM_OFFSET;
 const uint32_t TOYOTA_PARAM_STOCK_LONGITUDINAL = 2U << TOYOTA_PARAM_OFFSET;
 const uint32_t TOYOTA_PARAM_LTA = 4U << TOYOTA_PARAM_OFFSET;
+const uint32_t TOYOTA_PARAM_GAS_INTERCEPTOR = 8U << TOYOTA_PARAM_OFFSET;
 
 bool toyota_alt_brake = false;
 bool toyota_stock_longitudinal = false;
@@ -92,6 +112,17 @@ static uint32_t toyota_get_checksum(CANPacket_t *to_push) {
   return (uint8_t)(GET_BYTE(to_push, checksum_byte));
 }
 
+static uint8_t toyota_get_counter(CANPacket_t *to_push) {
+  int addr = GET_ADDR(to_push);
+
+  uint8_t cnt = 0U;
+  if (addr == 0x201) {
+    // Signal: COUNTER_PEDAL
+    cnt = GET_BYTE(to_push, 4) & 0x0FU;
+  }
+  return cnt;
+}
+
 static bool toyota_get_quality_flag_valid(CANPacket_t *to_push) {
   int addr = GET_ADDR(to_push);
 
@@ -144,7 +175,7 @@ static void toyota_rx_hook(CANPacket_t *to_push) {
       pcm_cruise_check(cruise_engaged);
 
       // sample gas pedal
-      if (!gas_interceptor_detected) {
+      if (!enable_gas_interceptor) {
         gas_pressed = GET_BIT(to_push, 4U) == 0U;
       }
     }
@@ -170,8 +201,7 @@ static void toyota_rx_hook(CANPacket_t *to_push) {
     }
 
     // sample gas interceptor
-    if (addr == 0x201) {
-      gas_interceptor_detected = 1;
+    if ((addr == 0x201) && enable_gas_interceptor) {
       int gas_interceptor = TOYOTA_GET_INTERCEPTOR(to_push);
       gas_pressed = gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD;
 
@@ -304,13 +334,21 @@ static safety_config toyota_init(uint16_t param) {
   toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE);
   toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL);
   toyota_lta = GET_FLAG(param, TOYOTA_PARAM_LTA);
+  enable_gas_interceptor = GET_FLAG(param, TOYOTA_PARAM_GAS_INTERCEPTOR);
   toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR;
 
+  // Gas interceptor should not be used if openpilot is not controlling longitudinal
+  if (toyota_stock_longitudinal) {
+    enable_gas_interceptor = false;
+  }
+
   safety_config ret;
   if (toyota_lta) {
-    ret = BUILD_SAFETY_CFG(toyota_lta_rx_checks, TOYOTA_TX_MSGS);
+    ret = enable_gas_interceptor ? BUILD_SAFETY_CFG(toyota_lta_interceptor_rx_checks, TOYOTA_INTERCEPTOR_TX_MSGS) : \
+                                   BUILD_SAFETY_CFG(toyota_lta_rx_checks, TOYOTA_TX_MSGS);
   } else {
-    ret = BUILD_SAFETY_CFG(toyota_lka_rx_checks, TOYOTA_TX_MSGS);
+    ret = enable_gas_interceptor ? BUILD_SAFETY_CFG(toyota_lka_interceptor_rx_checks, TOYOTA_INTERCEPTOR_TX_MSGS) : \
+                                   BUILD_SAFETY_CFG(toyota_lka_rx_checks, TOYOTA_TX_MSGS);
   }
   return ret;
 }
@@ -345,5 +383,6 @@ const safety_hooks toyota_hooks = {
   .fwd = toyota_fwd_hook,
   .get_checksum = toyota_get_checksum,
   .compute_checksum = toyota_compute_checksum,
+  .get_counter = toyota_get_counter,
   .get_quality_flag_valid = toyota_get_quality_flag_valid,
 };

board/safety_declarations.h

@@ -212,7 +212,7 @@ void safety_tick(const safety_config *safety_config);
 // This can be set by the safety hooks
 bool controls_allowed = false;
 bool relay_malfunction = false;
-bool gas_interceptor_detected = false;
+bool enable_gas_interceptor = false;
 int gas_interceptor_prev = 0;
 bool gas_pressed = false;
 bool gas_pressed_prev = false;

drivers/windows/panda_shared/panda.h

@@ -90,7 +90,6 @@ namespace panda {
 		uint8_t ignition_line;
 		uint8_t ignition_can;
 		uint8_t controls_allowed;
-		uint8_t gas_interceptor_detected;
 		uint8_t car_harness_status;
 		uint8_t usb_power_mode;
 		uint8_t safety_mode;

python/__init__.py

@@ -167,9 +167,9 @@ class Panda:
   HW_TYPE_TRES = b'\x09'
 
   CAN_PACKET_VERSION = 4
-  HEALTH_PACKET_VERSION = 14
+  HEALTH_PACKET_VERSION = 15
   CAN_HEALTH_PACKET_VERSION = 5
-  HEALTH_STRUCT = struct.Struct("<IIIIIIIIIBBBBBBHBBBHfBBHBHHB")
+  HEALTH_STRUCT = struct.Struct("<IIIIIIIIIBBBBBHBBBHfBBHBHHB")
   CAN_HEALTH_STRUCT = struct.Struct("<BIBBBBBBBBIIIIIIIHHBBBIIII")
 
   F2_DEVICES = [HW_TYPE_PEDAL, ]
@@ -193,11 +193,13 @@ class Panda:
   FLAG_TOYOTA_ALT_BRAKE = (1 << 8)
   FLAG_TOYOTA_STOCK_LONGITUDINAL = (2 << 8)
   FLAG_TOYOTA_LTA = (4 << 8)
+  FLAG_TOYOTA_GAS_INTERCEPTOR = (8 << 8)
 
   FLAG_HONDA_ALT_BRAKE = 1
   FLAG_HONDA_BOSCH_LONG = 2
   FLAG_HONDA_NIDEC_ALT = 4
   FLAG_HONDA_RADARLESS = 8
+  FLAG_HONDA_GAS_INTERCEPTOR = 16
 
   FLAG_HYUNDAI_EV_GAS = 1
   FLAG_HYUNDAI_HYBRID_GAS = 2
@@ -591,22 +593,21 @@ def health(self):
       "ignition_line": a[9],
       "ignition_can": a[10],
       "controls_allowed": a[11],
-      "gas_interceptor_detected": a[12],
-      "car_harness_status": a[13],
-      "safety_mode": a[14],
-      "safety_param": a[15],
-      "fault_status": a[16],
-      "power_save_enabled": a[17],
-      "heartbeat_lost": a[18],
-      "alternative_experience": a[19],
-      "interrupt_load": a[20],
-      "fan_power": a[21],
-      "safety_rx_checks_invalid": a[22],
-      "spi_checksum_error_count": a[23],
-      "fan_stall_count": a[24],
-      "sbu1_voltage_mV": a[25],
-      "sbu2_voltage_mV": a[26],
-      "som_reset_triggered": a[27],
+      "car_harness_status": a[12],
+      "safety_mode": a[13],
+      "safety_param": a[14],
+      "fault_status": a[15],
+      "power_save_enabled": a[16],
+      "heartbeat_lost": a[17],
+      "alternative_experience": a[18],
+      "interrupt_load": a[19],
+      "fan_power": a[20],
+      "safety_rx_checks_invalid": a[21],
+      "spi_checksum_error_count": a[22],
+      "fan_stall_count": a[23],
+      "sbu1_voltage_mV": a[24],
+      "sbu2_voltage_mV": a[25],
+      "som_reset_triggered": a[26],
     }
 
   @ensure_can_health_packet_version

tests/libpanda/safety_helpers.h

@@ -31,10 +31,6 @@ void set_relay_malfunction(bool c){
   relay_malfunction = c;
 }
 
-void set_gas_interceptor_detected(bool c){
-  gas_interceptor_detected = c;
-}
-
 bool get_controls_allowed(void){
   return controls_allowed;
 }
@@ -47,10 +43,6 @@ bool get_relay_malfunction(void){
   return relay_malfunction;
 }
 
-bool get_gas_interceptor_detected(void){
-  return gas_interceptor_detected;
-}
-
 int get_gas_interceptor_prev(void){
   return gas_interceptor_prev;
 }

tests/libpanda/safety_helpers.py

@@ -10,8 +10,6 @@ def setup_safety_helpers(ffi):
   int get_alternative_experience(void);
   void set_relay_malfunction(bool c);
   bool get_relay_malfunction(void);
-  void set_gas_interceptor_detected(bool c);
-  bool get_gas_interceptor_detected(void);
   int get_gas_interceptor_prev(void);
   bool get_gas_pressed_prev(void);
   bool get_brake_pressed_prev(void);
@@ -62,8 +60,6 @@ def set_alternative_experience(self, mode: int) -> None: ...
   def get_alternative_experience(self) -> int: ...
   def set_relay_malfunction(self, c: bool) -> None: ...
   def get_relay_malfunction(self) -> bool: ...
-  def set_gas_interceptor_detected(self, c: bool) -> None: ...
-  def get_gas_interceptor_detected(self) -> bool: ...
   def get_gas_interceptor_prev(self) -> int: ...
   def get_gas_pressed_prev(self) -> bool: ...
   def get_brake_pressed_prev(self) -> bool: ...