init

dragonpilot-community · Jul 18, 2024 · d88c18f · d88c18f
1 parent 4e94257
commit d88c18f
Show file tree

Hide file tree

Showing 10 changed files with 361 additions and 0 deletions.
diff --git a/board/safety.h b/board/safety.h
@@ -327,6 +327,9 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
   // reset state set by safety mode
   safety_mode_cnt = 0U;
   relay_malfunction = false;
+  // dp - gas interceptor
+  enable_gas_interceptor = false;
+  gas_interceptor_prev = 0;
   gas_pressed = false;
   gas_pressed_prev = false;
   brake_pressed = false;
@@ -541,6 +544,11 @@ bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limit
   return violation;
 }
 
+// dp - gas interceptor
+bool longitudinal_interceptor_checks(const CANPacket_t *to_send) {
+  return !get_longitudinal_allowed() && (GET_BYTE(to_send, 0) || GET_BYTE(to_send, 1));
+}
+
 // Safety checks for torque-based steering commands
 bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLimits limits) {
   bool violation = false;

diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h
@@ -1,9 +1,16 @@
 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
 const CanMsg HONDA_RADARLESS_LONG_TX_MSGS[] = {{0xE4, 0, 5}, {0x33D, 0, 8}, {0x1C8, 0, 8}, {0x30C, 0, 8}};  // Bosch radarless w/ gas and brakes
 
+// panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches
+// If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state
+// Threshold calculated from DBC gains: round(((83.3 / 0.253984064) + (83.3 / 0.126992032)) / 2) = 492
+const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 492;
+#define HONDA_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 LongitudinalLimits HONDA_BOSCH_LONG_LIMITS = {
   .max_accel = 200,   // accel is used for brakes
   .min_accel = -350,
@@ -40,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)
@@ -50,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)
@@ -64,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,
@@ -110,12 +128,32 @@ static uint32_t honda_compute_checksum(const CANPacket_t *to_push) {
 }
 
 static uint8_t honda_get_counter(const CANPacket_t *to_push) {
+  #if 0
   int counter_byte = GET_LEN(to_push) - 1U;
   return (GET_BYTE(to_push, counter_byte) >> 4U) & 0x3U;
+  #endif
+  // dp - w/ gas interceptor
+  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(const CANPacket_t *to_push) {
+  #if 0
   const bool pcm_cruise = ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) || (honda_hw == HONDA_NIDEC);
+  #endif
+  // dp - w/ gas interceptor
+  int len = GET_LEN(to_push);
+  const bool pcm_cruise = ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) || \
+                            ((honda_hw == HONDA_NIDEC) && !enable_gas_interceptor);
   int pt_bus = honda_get_pt_bus();
 
   int addr = GET_ADDR(to_push);
@@ -190,9 +228,24 @@ static void honda_rx_hook(const CANPacket_t *to_push) {
     }
   }
 
+  #if 0
   if (addr == 0x17C) {
     gas_pressed = GET_BYTE(to_push, 0) != 0U;
   }
+  #endif
+  // dp - w/ interceptor
+  // length check because bosch hardware also uses this id (0x201 w/ len = 8)
+  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 (!enable_gas_interceptor) {
+    if (addr == 0x17C) {
+      gas_pressed = GET_BYTE(to_push, 0) != 0U;
+    }
+  }
 
   // disable stock Honda AEB in alternative experience
   if (!(alternative_experience & ALT_EXP_DISABLE_STOCK_AEB)) {
@@ -309,6 +362,14 @@ static bool honda_tx_hook(const CANPacket_t *to_send) {
     }
   }
 
+  // dp - w/ gas interceptor
+  // GAS: safety check (interceptor)
+  if (addr == 0x200) {
+    if (longitudinal_interceptor_checks(to_send)) {
+      tx = false;
+    }
+  }
+
   // FORCE CANCEL: safety check only relevant when spamming the cancel button in Bosch HW
   // ensuring that only the cancel button press is sent (VAL 2) when controls are off.
   // This avoids unintended engagements while still allowing resume spam
@@ -336,17 +397,35 @@ static safety_config honda_nidec_init(uint16_t param) {
   honda_alt_brake_msg = false;
   honda_bosch_long = false;
   honda_bosch_radarless = false;
+  // dp - gas interceptor
+  enable_gas_interceptor = GET_FLAG(param, HONDA_PARAM_GAS_INTERCEPTOR);
 
   safety_config ret;
 
   bool enable_nidec_alt = GET_FLAG(param, HONDA_PARAM_NIDEC_ALT);
 
+  #if 0
   if (enable_nidec_alt) {
     SET_RX_CHECKS(honda_nidec_alt_rx_checks, ret);
   } else {
     SET_RX_CHECKS(honda_common_rx_checks, ret);
   }
   SET_TX_MSGS(HONDA_N_TX_MSGS, ret);
+  #endif
+  // dp - w/ gas interceptor
+  if (enable_nidec_alt) {
+    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 {
+    SET_TX_MSGS(HONDA_N_TX_MSGS, ret);
+  }
 
   return ret;
 }

diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h
@@ -37,6 +37,13 @@ const LongitudinalLimits TOYOTA_LONG_LIMITS = {
   .min_accel = -3500,  // -3.5 m/s2
 };
 
+// dp - gas interceptor
+// panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches
+// If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state
+// Threshold calculated from DBC gains: round((((15 + 75.555) / 0.159375) + ((15 + 151.111) / 0.159375)) / 2) = 805
+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
+
 // Stock longitudinal
 #define TOYOTA_COMMON_TX_MSGS                                                                                     \
   {0x2E4, 0, 5}, {0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8},  /* LKAS + LTA + ACC & PCM cancel cmds */  \
@@ -56,6 +63,12 @@ const CanMsg TOYOTA_LONG_TX_MSGS[] = {
   TOYOTA_COMMON_LONG_TX_MSGS
 };
 
+// dp - gas interceptor
+const CanMsg TOYOTA_INTERCEPTOR_TX_MSGS[] = {
+  TOYOTA_COMMON_LONG_TX_MSGS
+  {0x200, 0, 6},  // gas interceptor
+};
+
 #define TOYOTA_COMMON_RX_CHECKS(lta)                                                                        \
   {.msg = {{ 0xaa, 0, 8, .check_checksum = false, .frequency = 83U}, { 0 }, { 0 }}},                        \
   {.msg = {{0x260, 0, 8, .check_checksum = true, .quality_flag = (lta), .frequency = 50U}, { 0 }, { 0 }}},  \
@@ -67,18 +80,32 @@ RxCheck toyota_lka_rx_checks[] = {
   TOYOTA_COMMON_RX_CHECKS(false)
 };
 
+// dp - gas interceptor
+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[] = {
   TOYOTA_COMMON_RX_CHECKS(true)
 };
 
+// dp - gas interceptor
+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 = (1UL << TOYOTA_PARAM_OFFSET) - 1U;
 const uint32_t TOYOTA_PARAM_ALT_BRAKE = 1UL << TOYOTA_PARAM_OFFSET;
 const uint32_t TOYOTA_PARAM_STOCK_LONGITUDINAL = 2UL << TOYOTA_PARAM_OFFSET;
 const uint32_t TOYOTA_PARAM_LTA = 4UL << TOYOTA_PARAM_OFFSET;
+// dp - gas interceptor
+const uint32_t TOYOTA_PARAM_GAS_INTERCEPTOR = 8UL << TOYOTA_PARAM_OFFSET;
 
 bool toyota_alt_brake = false;
 bool toyota_stock_longitudinal = false;
@@ -100,6 +127,18 @@ static uint32_t toyota_get_checksum(const CANPacket_t *to_push) {
   return (uint8_t)(GET_BYTE(to_push, checksum_byte));
 }
 
+// dp - gas interceptor
+static uint8_t toyota_get_counter(const 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(const CANPacket_t *to_push) {
   int addr = GET_ADDR(to_push);
 
@@ -152,7 +191,13 @@ static void toyota_rx_hook(const CANPacket_t *to_push) {
       pcm_cruise_check(cruise_engaged);
 
       // sample gas pedal
+      #if 0
       gas_pressed = !GET_BIT(to_push, 4U);
+      #endif
+      // dp - w/ gas interceptor
+      if (!enable_gas_interceptor) {
+        gas_pressed = !GET_BIT(to_push, 4U);
+      }
     }
 
     // sample speed
@@ -175,6 +220,16 @@ static void toyota_rx_hook(const CANPacket_t *to_push) {
       brake_pressed = GET_BIT(to_push, bit);
     }
 
+    // dp - gas interceptor
+    // sample gas interceptor
+    if ((addr == 0x201) && enable_gas_interceptor) {
+      int gas_interceptor = TOYOTA_GET_INTERCEPTOR(to_push);
+      gas_pressed = gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD;
+
+      // TODO: remove this, only left in for gas_interceptor_prev test
+      gas_interceptor_prev = gas_interceptor;
+    }
+
     #if 0
     bool stock_ecu_detected = addr == 0x2E4;  // STEERING_LKA
     if (!toyota_stock_longitudinal && (addr == 0x343)) {
@@ -194,6 +249,15 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) {
 
   // Check if msg is sent on BUS 0
   if (bus == 0) {
+
+    // dp - gas interceptor
+    // GAS PEDAL: safety check
+    if (addr == 0x200) {
+      if (longitudinal_interceptor_checks(to_send)) {
+        tx = false;
+      }
+    }
+
     // ACCEL: safety check on byte 1-2
     if (addr == 0x343) {
       int desired_accel = (GET_BYTE(to_send, 0) << 8) | GET_BYTE(to_send, 1);
@@ -317,8 +381,15 @@ static safety_config toyota_init(uint16_t param) {
   toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL);
   toyota_lta = GET_FLAG(param, TOYOTA_PARAM_LTA);
   toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR;
+  // dp - gas interceptor
+  enable_gas_interceptor = GET_FLAG(param, TOYOTA_PARAM_GAS_INTERCEPTOR);
+  // Gas interceptor should not be used if openpilot is not controlling longitudinal
+  if (toyota_stock_longitudinal) {
+    enable_gas_interceptor = false;
+  }
 
   safety_config ret;
+  #if 0
   if (toyota_stock_longitudinal) {
     SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
   } else {
@@ -327,6 +398,22 @@ static safety_config toyota_init(uint16_t param) {
 
   toyota_lta ? SET_RX_CHECKS(toyota_lta_rx_checks, ret) : \
                SET_RX_CHECKS(toyota_lka_rx_checks, ret);
+  #endif
+  // dp - with gas interceptor
+  if (toyota_stock_longitudinal) {
+    SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
+  } else {
+    enable_gas_interceptor ? SET_TX_MSGS(TOYOTA_INTERCEPTOR_TX_MSGS, ret) : \
+                             SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret);
+  }
+  if (enable_gas_interceptor) {
+    toyota_lta ? SET_RX_CHECKS(toyota_lta_interceptor_rx_checks, ret) : \
+                 SET_RX_CHECKS(toyota_lka_interceptor_rx_checks, ret);
+  } else {
+    toyota_lta ? SET_RX_CHECKS(toyota_lta_rx_checks, ret) : \
+                 SET_RX_CHECKS(toyota_lka_rx_checks, ret);
+  }
+
   return ret;
 }
 
@@ -362,5 +449,7 @@ const safety_hooks toyota_hooks = {
   .fwd = toyota_fwd_hook,
   .get_checksum = toyota_get_checksum,
   .compute_checksum = toyota_compute_checksum,
+  // dp - for gas interceptor (?)
+  .get_counter = toyota_get_counter,
   .get_quality_flag_valid = toyota_get_quality_flag_valid,
 };
diff --git a/board/safety_declarations.h b/board/safety_declarations.h
@@ -207,9 +207,16 @@ void pcm_cruise_check(bool cruise_engaged);
 
 void safety_tick(const safety_config *safety_config);
 
+// dp - gas interceptor
+bool longitudinal_interceptor_checks(const CANPacket_t *to_send);
+
 // This can be set by the safety hooks
 bool controls_allowed = false;
 bool relay_malfunction = false;
+// dp - gas interceptor
+bool enable_gas_interceptor = false;
+int gas_interceptor_prev = 0;
+
 bool gas_pressed = false;
 bool gas_pressed_prev = false;
 bool brake_pressed = false;

diff --git a/python/__init__.py b/python/__init__.py
@@ -192,11 +192,15 @@ class Panda:
   FLAG_TOYOTA_ALT_BRAKE = (1 << 8)
   FLAG_TOYOTA_STOCK_LONGITUDINAL = (2 << 8)
   FLAG_TOYOTA_LTA = (4 << 8)
+  # dp - gas interceptor
+  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
+  # dp - gas interceptor
+  FLAG_HONDA_GAS_INTERCEPTOR = 16
 
   FLAG_HYUNDAI_EV_GAS = 1
   FLAG_HYUNDAI_HYBRID_GAS = 2

diff --git a/tests/libpanda/safety_helpers.h b/tests/libpanda/safety_helpers.h
@@ -43,6 +43,11 @@ bool get_relay_malfunction(void){
   return relay_malfunction;
 }
 
+// dp - gas interceptor
+int get_gas_interceptor_prev(void){
+  return gas_interceptor_prev;
+}
+
 bool get_gas_pressed_prev(void){
   return gas_pressed_prev;
 }