prevent tesla to reverse

commaai · Sep 11, 2024 · be0ed49 · be0ed49
1 parent 709bd2c
commit be0ed49
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 0 deletions.
diff --git a/board/safety/safety_tesla.h b/board/safety/safety_tesla.h
@@ -139,6 +139,12 @@ static bool tesla_tx_hook(const CANPacket_t *to_send) {
       // Don't allow any acceleration limits above the safety limits
       int raw_accel_max = ((GET_BYTE(to_send, 6) & 0x1FU) << 4) | (GET_BYTE(to_send, 5) >> 4);
       int raw_accel_min = ((GET_BYTE(to_send, 5) & 0x0FU) << 5) | (GET_BYTE(to_send, 4) >> 3);
+
+      // Prevent both acceleration from being negative, as this could cause the car to reverse after coming to standstill
+      if (raw_accel_max < TESLA_LONG_LIMITS.inactive_accel && raw_accel_min < TESLA_LONG_LIMITS.inactive_accel){
+        violation = true;
+      }
+
       violation |= longitudinal_accel_checks(raw_accel_max, TESLA_LONG_LIMITS);
       violation |= longitudinal_accel_checks(raw_accel_min, TESLA_LONG_LIMITS);
     } else {

diff --git a/tests/safety/test_tesla.py b/tests/safety/test_tesla.py
@@ -136,6 +136,28 @@ def test_stock_aeb_passthrough(self):
     self.assertEqual(0, self.safety.safety_fwd_hook(2, aeb_msg_cam.addr))
     self.assertFalse(self._tx(no_aeb_msg))
 
+  def test_prevent_reverse(self):
+    # Note: Tesla can reverse while at a standstill if both accel_min and accel_max are negative.
+    self.safety.set_controls_allowed(True)
+
+    # accel_min and accel_max are positive
+    self.assertTrue(self._tx(self._long_control_msg(set_speed=10, accel_limits=(1.1, 0.8))))
+    self.assertTrue(self._tx(self._long_control_msg(set_speed=0, accel_limits=(1.1, 0.8))))
+
+    # accel_min and accel_max are both zero
+    self.assertTrue(self._tx(self._long_control_msg(set_speed=10, accel_limits=(0, 0))))
+    self.assertTrue(self._tx(self._long_control_msg(set_speed=0, accel_limits=(0, 0))))
+
+    # accel_min and accel_max have opposing signs
+    self.assertTrue(self._tx(self._long_control_msg(set_speed=10, accel_limits=(-0.8, 1.3))))
+    self.assertTrue(self._tx(self._long_control_msg(set_speed=0, accel_limits=(0.8, -1.3))))
+    self.assertTrue(self._tx(self._long_control_msg(set_speed=0, accel_limits=(0, -1.3))))
+
+    # accel_min and accel_max are negative
+    self.assertFalse(self._tx(self._long_control_msg(set_speed=10, accel_limits=(-1.1, -0.6))))
+    self.assertFalse(self._tx(self._long_control_msg(set_speed=0, accel_limits=(-0.6, -1.1))))
+    self.assertFalse(self._tx(self._long_control_msg(set_speed=0, accel_limits=(-0.1, -0.1))))
+
 
 if __name__ == "__main__":
   unittest.main()