Pedal safety logic

board/safety/safety_gm.h

@@ -27,14 +27,19 @@ const LongitudinalLimits *gm_long_limits;
 
 const int GM_STANDSTILL_THRSLD = 10;  // 0.311kph
 
+// 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
+const int GM_GAS_INTERCEPTOR_THRESHOLD = 515; // (610 + 306.25) / 2 ratio between offset and gain from dbc file
+#define GM_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 GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, {0x2CB, 0, 8}, {0x370, 0, 6},  // pt bus
                                   {0xA1, 1, 7}, {0x306, 1, 8}, {0x308, 1, 7}, {0x310, 1, 2},   // obs bus
                                   {0x315, 2, 5}};  // ch bus
 
 const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4},  // pt bus
                                  {0x1E1, 2, 7}, {0x184, 2, 8}};  // camera bus
 
-const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8}, {0x370, 0, 6},  // pt bus
+const CanMsg GM_CAM_LONG_TX_MSGS[] = {{0x180, 0, 4}, {0x315, 0, 5}, {0x2CB, 0, 8}, {0x370, 0, 6}, {0x200, 0, 6},  // pt bus
                                       {0x184, 2, 8}};  // camera bus
 
 // TODO: do checksum and counter checks. Add correct timestep, 0.1s for now.
@@ -51,6 +56,8 @@ RxCheck gm_rx_checks[] = {
 
 const uint16_t GM_PARAM_HW_CAM = 1;
 const uint16_t GM_PARAM_HW_CAM_LONG = 2;
+const uint16_t GM_PARAM_NO_ACC = 4;
+const uint16_t GM_PARAM_PEDAL_INTERCEPTOR = 8;
 
 enum {
   GM_BTN_UNPRESS = 1,
@@ -66,6 +73,7 @@ typedef enum {
 GmHardware gm_hw = GM_ASCM;
 bool gm_cam_long = false;
 bool gm_pcm_cruise = false;
+bool gm_has_acc = true;
 
 static void gm_rx_hook(const CANPacket_t *to_push) {
   if (GET_BUS(to_push) == 0U) {
@@ -115,19 +123,35 @@ static void gm_rx_hook(const CANPacket_t *to_push) {
     }
 
     if (addr == 0x1C4) {
-      gas_pressed = GET_BYTE(to_push, 5) != 0U;
+      if (!enable_gas_interceptor) {
+        gas_pressed = GET_BYTE(to_push, 5) != 0U;
+      }
 
       // enter controls on rising edge of ACC, exit controls when ACC off
-      if (gm_pcm_cruise) {
+      if (gm_pcm_cruise && gm_has_acc) {
         bool cruise_engaged = (GET_BYTE(to_push, 1) >> 5) != 0U;
         pcm_cruise_check(cruise_engaged);
       }
     }
 
+    // Cruise check for CC only cars
+    if ((addr == 0x3D1) && !gm_has_acc) {
+      bool cruise_engaged = (GET_BYTE(to_push, 4) >> 7) != 0U;
+      cruise_engaged_prev = cruise_engaged;
+    }
+
     if (addr == 0xBD) {
       regen_braking = (GET_BYTE(to_push, 0) >> 4) != 0U;
     }
 
+    // Pedal Interceptor
+    if ((addr == 0x201) && enable_gas_interceptor) {
+      int gas_interceptor = GM_GET_INTERCEPTOR(to_push);
+      gas_pressed = gas_interceptor > GM_GAS_INTERCEPTOR_THRESHOLD;
+      gas_interceptor_prev = gas_interceptor;
+//      gm_pcm_cruise = false;
+    }
+
     bool stock_ecu_detected = (addr == 0x180);  // ASCMLKASteeringCmd
 
     // Check ASCMGasRegenCmd only if we're blocking it
@@ -163,6 +187,13 @@ static bool gm_tx_hook(const CANPacket_t *to_send) {
     }
   }
 
+  // GAS: safety check (interceptor)
+  if (addr == 0x200) {
+    if (longitudinal_interceptor_checks(to_send)) {
+      tx = 0;
+    }
+  }
+
   // GAS/REGEN: safety check
   if (addr == 0x2CB) {
     bool apply = GET_BIT(to_send, 0U);
@@ -229,6 +260,8 @@ static safety_config gm_init(uint16_t param) {
 
 #ifdef ALLOW_DEBUG
   gm_cam_long = GET_FLAG(param, GM_PARAM_HW_CAM_LONG);
+  gm_has_acc = !GET_FLAG(param, GM_PARAM_NO_ACC);
+  enable_gas_interceptor = GET_FLAG(param, GM_PARAM_PEDAL_INTERCEPTOR);
 #endif
   gm_pcm_cruise = (gm_hw == GM_CAM) && !gm_cam_long;
 

python/__init__.py

@@ -233,6 +233,8 @@ class Panda:
 
   FLAG_GM_HW_CAM = 1
   FLAG_GM_HW_CAM_LONG = 2
+  FLAG_GM_NO_ACC = 4
+  FLAG_GM_GAS_INTERCEPTOR = 8
 
   FLAG_FORD_LONG_CONTROL = 1
   FLAG_FORD_CANFD = 2

tests/libpanda/safety_helpers.py

@@ -61,7 +61,7 @@ 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 get_gas_interceptor_prev(self) -> int: ...
+  def get_gas_interceptor_prev(self) -> bool: ...
   def get_gas_pressed_prev(self) -> bool: ...
   def get_brake_pressed_prev(self) -> bool: ...
   def get_regen_braking_prev(self) -> bool: ...

tests/safety/test_gm.py

@@ -223,5 +223,90 @@ def setUp(self):
     self.safety.init_tests()
 
 
+##### OPGM TESTS #####
+
+def interceptor_msg(gas, addr):
+  to_send = common.make_msg(0, addr, 6)
+  to_send[0].data[0] = (gas & 0xFF00) >> 8
+  to_send[0].data[1] = gas & 0xFF
+  to_send[0].data[2] = (gas & 0xFF00) >> 8
+  to_send[0].data[3] = gas & 0xFF
+  return to_send
+
+
+class TestGmInterceptorSafety(common.GasInterceptorSafetyTest, TestGmCameraSafety):
+  INTERCEPTOR_THRESHOLD = 515
+
+  def setUp(self):
+    self.packer = CANPackerPanda("gm_global_a_powertrain_generated")
+    self.packer_chassis = CANPackerPanda("gm_global_a_chassis")
+    self.safety = libpanda_py.libpanda
+    self.safety.set_safety_hooks(
+      Panda.SAFETY_GM,
+      Panda.FLAG_GM_HW_CAM | Panda.FLAG_GM_NO_ACC | Panda.FLAG_GM_PEDAL_LONG | Panda.FLAG_GM_GAS_INTERCEPTOR)
+    self.safety.init_tests()
+
+  def test_pcm_sets_cruise_engaged(self):
+    for enabled in [True, False]:
+      self._rx(self._pcm_status_msg(enabled))
+      self.assertEqual(enabled, self.safety.get_cruise_engaged_prev())
+
+  def test_no_pcm_enable(self):
+    self._rx(self._interceptor_user_gas(0))
+    self.safety.set_controls_allowed(0)
+    self.assertFalse(self.safety.get_controls_allowed())
+    self._rx(self._pcm_status_msg(True))
+    self.assertFalse(self.safety.get_controls_allowed())
+    self.assertTrue(self.safety.get_cruise_engaged_prev())
+
+  def test_no_response_to_acc_pcm_message(self):
+    self._rx(self._interceptor_user_gas(0))
+    def _acc_pcm_msg(enable):
+      values = {"CruiseState": enable}
+      return self.packer.make_can_msg_panda("AcceleratorPedal2", 0, values)
+    for enable in [True, False]:
+      self.safety.set_controls_allowed(enable)
+      self._rx(_acc_pcm_msg(True))
+      self.assertEqual(enable, self.safety.get_controls_allowed())
+      self._rx(_acc_pcm_msg(False))
+      self.assertEqual(enable, self.safety.get_controls_allowed())
+
+  def test_buttons(self):
+    self._rx(self._interceptor_user_gas(0))
+    # Only CANCEL button is allowed while cruise is enabled
+    self.safety.set_controls_allowed(0)
+    for btn in range(8):
+      self.assertFalse(self._tx(self._button_msg(btn)))
+
+    self.safety.set_controls_allowed(1)
+    for btn in range(8):
+      self.assertFalse(self._tx(self._button_msg(btn)))
+
+    self.safety.set_controls_allowed(1)
+    for enabled in (True, False):
+      self._rx(self._pcm_status_msg(enabled))
+      self.assertEqual(enabled, self._tx(self._button_msg(Buttons.CANCEL)))
+      self.assertTrue(self.safety.get_controls_allowed())
+
+  def test_fwd_hook(self):
+    pass
+
+  def test_disable_control_allowed_from_cruise(self):
+    pass
+
+  def test_enable_control_allowed_from_cruise(self):
+    pass
+
+  def _interceptor_gas_cmd(self, gas):
+    return interceptor_msg(gas, 0x200)
+
+  def _interceptor_user_gas(self, gas):
+    return interceptor_msg(gas, 0x201)
+
+  def _pcm_status_msg(self, enable):
+    values = {"CruiseActive": enable}
+    return self.packer.make_can_msg_panda("ECMCruiseControl", 0, values)
+
+
 if __name__ == "__main__":
   unittest.main()