diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index ad70c025ce..13f3366333 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -223,6 +223,9 @@ static bool ford_tx_hook(const CANPacket_t *to_send) { // Signal: CmbbDeny_B_Actl bool cmbb_deny = GET_BIT(to_send, 37U); + // Signal: AccBrkPrchg_B_Rq & AccBrkDecel_B_Rq + bool brake_actuation = GET_BIT(to_send, 54U) || GET_BIT(to_send, 55U); + bool violation = false; violation |= longitudinal_accel_checks(accel, FORD_LONG_LIMITS); violation |= longitudinal_gas_checks(gas, FORD_LONG_LIMITS); @@ -231,6 +234,8 @@ static bool ford_tx_hook(const CANPacket_t *to_send) { // Safety check for stock AEB violation |= cmbb_deny; // do not prevent stock AEB actuation + violation |= !get_longitudinal_allowed() && brake_actuation; + if (violation) { tx = false; } diff --git a/tests/safety/test_ford.py b/tests/safety/test_ford.py index 5ac2832703..ab6c4cd440 100755 --- a/tests/safety/test_ford.py +++ b/tests/safety/test_ford.py @@ -406,12 +406,14 @@ def setUpClass(cls): raise unittest.SkipTest # ACC command - def _acc_command_msg(self, gas: float, brake: float, cmbb_deny: bool = False): + def _acc_command_msg(self, gas: float, brake: float, brake_actuation: bool, cmbb_deny: bool = False): values = { - "AccPrpl_A_Rq": gas, # [-5|5.23] m/s^2 - "AccPrpl_A_Pred": gas, # [-5|5.23] m/s^2 - "AccBrkTot_A_Rq": brake, # [-20|11.9449] m/s^2 - "CmbbDeny_B_Actl": 1 if cmbb_deny else 0, # [0|1] deny AEB actuation + "AccPrpl_A_Rq": gas, # [-5|5.23] m/s^2 + "AccPrpl_A_Pred": gas, # [-5|5.23] m/s^2 + "AccBrkTot_A_Rq": brake, # [-20|11.9449] m/s^2 + "AccBrkPrchg_B_Rq": 1 if brake_actuation else 0, # Pre-charge brake request: 0=No, 1=Yes + "AccBrkDecel_B_Rq": 1 if brake_actuation else 0, # Deceleration request: 0=Inactive, 1=Active + "CmbbDeny_B_Actl": 1 if cmbb_deny else 0, # [0|1] deny AEB actuation } return self.packer.make_can_msg_panda("ACCDATA", 0, values) @@ -421,9 +423,9 @@ def test_stock_aeb(self): self.safety.set_controls_allowed(controls_allowed) for cmbb_deny in (True, False): should_tx = not cmbb_deny - self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.INACTIVE_GAS, self.INACTIVE_ACCEL, cmbb_deny))) + self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.INACTIVE_GAS, self.INACTIVE_ACCEL, controls_allowed, cmbb_deny))) should_tx = controls_allowed and not cmbb_deny - self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.MAX_GAS, self.MAX_ACCEL, cmbb_deny))) + self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.MAX_GAS, self.MAX_ACCEL, controls_allowed, cmbb_deny))) def test_gas_safety_check(self): for controls_allowed in (True, False): @@ -431,15 +433,17 @@ def test_gas_safety_check(self): for gas in np.concatenate((np.arange(self.MIN_GAS - 2, self.MAX_GAS + 2, 0.05), [self.INACTIVE_GAS])): gas = round(gas, 2) # floats might not hit exact boundary conditions without rounding should_tx = (controls_allowed and self.MIN_GAS <= gas <= self.MAX_GAS) or gas == self.INACTIVE_GAS - self.assertEqual(should_tx, self._tx(self._acc_command_msg(gas, self.INACTIVE_ACCEL))) + self.assertEqual(should_tx, self._tx(self._acc_command_msg(gas, self.INACTIVE_ACCEL, controls_allowed))) def test_brake_safety_check(self): for controls_allowed in (True, False): self.safety.set_controls_allowed(controls_allowed) - for brake in np.arange(self.MIN_ACCEL - 2, self.MAX_ACCEL + 2, 0.05): - brake = round(brake, 2) # floats might not hit exact boundary conditions without rounding - should_tx = (controls_allowed and self.MIN_ACCEL <= brake <= self.MAX_ACCEL) or brake == self.INACTIVE_ACCEL - self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.INACTIVE_GAS, brake))) + for brake_actuation in (True, False): + for brake in np.arange(self.MIN_ACCEL - 2, self.MAX_ACCEL + 2, 0.05): + brake = round(brake, 2) # floats might not hit exact boundary conditions without rounding + should_tx = (controls_allowed and self.MIN_ACCEL <= brake <= self.MAX_ACCEL) or brake == self.INACTIVE_ACCEL + should_tx = should_tx and (controls_allowed or not brake_actuation) + self.assertEqual(should_tx, self._tx(self._acc_command_msg(self.INACTIVE_GAS, brake, brake_actuation))) class TestFordLongitudinalSafety(TestFordLongitudinalSafetyBase):