mawei/panda-meb
1
0
Fork 0
mirror of https://github.com/infiniteCable2/panda.git synced 2026-02-18 17:23:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

Toyota: stock longitudinal safety (#927)

Browse Source 
* safety for non-longitudinal

* flip around param for easily replaying old logs on new panda

* fix misra: a function parameter should not be modified

* safety

* the toyota tests all share common messages

* fixup tests

* update comment

* same as tesla test

* check if subset

* update comments

update comments
This commit is contained in:
Shane Smiskol
2022-04-28 22:29:29 -07:00
committed by GitHub
parent cf8fb0b883
commit 326cc2a8db
4 changed files with 54 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
board/safety/safety_toyota.h
View File

@@ -43,9 +43,11 @@ addr_checks toyota_rx_checks = {toyota_addr_checks, TOYOTA_ADDR_CHECKS_LEN};
// 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_ALT_BRAKE = 1U << TOYOTA_PARAM_OFFSET;
const uint32_t TOYOTA_PARAM_ALT_BRAKE = 1U << TOYOTA_PARAM_OFFSET;
const uint32_t TOYOTA_PARAM_STOCK_LONGITUDINAL = 2U << TOYOTA_PARAM_OFFSET;
bool toyota_alt_brake = false;
bool toyota_stock_longitudinal = false;
int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file
static uint8_t toyota_compute_checksum(CANPacket_t *to_push) {
@@ -164,11 +166,20 @@ static int toyota_tx_hook(CANPacket_t *to_send, bool longitudinal_allowed) {
if (addr == 0x343) {
int desired_accel = (GET_BYTE(to_send, 0) << 8) | GET_BYTE(to_send, 1);
desired_accel = to_signed(desired_accel, 16);
if (!longitudinal_allowed) {
if (!longitudinal_allowed || toyota_stock_longitudinal) {
if (desired_accel != 0) {
tx = 0;
}
}
// only ACC messages that cancel are allowed when openpilot is not controlling longitudinal
if (toyota_stock_longitudinal) {
bool cancel_req = GET_BIT(to_send, 24U) != 0U;
if (!cancel_req) {
tx = 0;
}
}
bool violation = max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL);
if (violation) {
@@ -256,7 +267,8 @@ static const addr_checks* toyota_init(uint32_t param) {
controls_allowed = 0;
relay_malfunction_reset();
gas_interceptor_detected = 0;
toyota_alt_brake = GET_FLAG(param, TOYOTA_ALT_BRAKE);
toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE);
toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL);
toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR;
return &toyota_rx_checks;
}

3
python/__init__.py
View File

@@ -180,7 +180,9 @@ class Panda(object):
CLOCK_SOURCE_MODE_DISABLED = 0
CLOCK_SOURCE_MODE_FREE_RUNNING = 1
# first byte is for EPS scaling factor
FLAG_TOYOTA_ALT_BRAKE = (1 << 8)
FLAG_TOYOTA_STOCK_LONGITUDINAL = (2 << 8)
FLAG_HONDA_ALT_BRAKE = 1
FLAG_HONDA_BOSCH_LONG = 2
@@ -189,6 +191,7 @@ class Panda(object):
FLAG_HYUNDAI_EV_GAS = 1
FLAG_HYUNDAI_HYBRID_GAS = 2
FLAG_HYUNDAI_LONG = 4
FLAG_TESLA_POWERTRAIN = 1
FLAG_TESLA_LONG_CONTROL = 2

2
tests/safety/common.py
View File

@@ -450,7 +450,7 @@ class PandaSafetyTest(PandaSafetyTestBase):
if 'Tesla' in attr and 'Tesla' in current_test:
continue
if {attr, current_test} == {'TestToyotaSafety', 'TestToyotaAltBrakeSafety'}:
if {attr, current_test}.issubset({'TestToyotaSafety', 'TestToyotaAltBrakeSafety', 'TestToyotaStockLongitudinal'}):
continue
# TODO: Temporary, should be fixed in panda firmware, safety_honda.h

45
tests/safety/test_toyota.py
View File

@@ -1,7 +1,8 @@
#!/usr/bin/env python3
import unittest
import numpy as np
import random
import unittest
from panda import Panda
from panda.tests.safety import libpandasafety_py
import panda.tests.safety.common as common
@@ -40,17 +41,17 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest,
MAX_RT_DELTA = 450
RT_INTERVAL = 250000
MAX_TORQUE_ERROR = 350
TORQUE_MEAS_TOLERANCE = 1 # toyota safety adds one to be conversative for rounding
EPS_SCALE = 0.73
TORQUE_MEAS_TOLERANCE = 1 # toyota safety adds one to be conservative for rounding
EPS_SCALE = 73
def setUp(self):
self.packer = CANPackerPanda("toyota_nodsu_pt_generated")
self.safety = libpandasafety_py.libpandasafety
self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, 73)
self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE)
self.safety.init_tests()
def _torque_meas_msg(self, torque):
values = {"STEER_TORQUE_EPS": (torque/self.EPS_SCALE)}
values = {"STEER_TORQUE_EPS": (torque / self.EPS_SCALE) * 100.}
return self.packer.make_can_msg_panda("STEER_TORQUE_SENSOR", 0, values)
def _torque_msg(self, torque):
@@ -61,8 +62,8 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest,
values = {"STEER_REQUEST": req, "STEER_REQUEST_2": req2, "STEER_ANGLE_CMD": angle_cmd}
return self.packer.make_can_msg_panda("STEERING_LTA", 0, values)
def _accel_msg(self, accel):
values = {"ACCEL_CMD": accel}
def _accel_msg(self, accel, cancel_req=0):
values = {"ACCEL_CMD": accel, "CANCEL_REQ": cancel_req}
return self.packer.make_can_msg_panda("ACC_CONTROL", 0, values)
def _speed_msg(self, speed):
@@ -99,7 +100,7 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest,
msg = common.package_can_msg([0x283, 0, bytes(dat), 0])
self.assertEqual(not bad, self._tx(msg))
def test_accel_actuation_limits(self):
def test_accel_actuation_limits(self, stock_longitudinal=False):
limits = ((MIN_ACCEL, MAX_ACCEL, ALTERNATIVE_EXPERIENCE.DEFAULT),
(MIN_ACCEL, MAX_ACCEL, ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX))
@@ -108,7 +109,9 @@ class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest,
for controls_allowed in [True, False]:
self.safety.set_controls_allowed(controls_allowed)
self.safety.set_alternative_experience(alternative_experience)
if controls_allowed:
if stock_longitudinal:
should_tx = False
elif controls_allowed:
should_tx = int(min_accel * 1000) <= int(accel * 1000) <= int(max_accel * 1000)
else:
should_tx = np.isclose(accel, 0, atol=0.0001)
@@ -155,7 +158,7 @@ class TestToyotaAltBrakeSafety(TestToyotaSafety):
def setUp(self):
self.packer = CANPackerPanda("toyota_new_mc_pt_generated")
self.safety = libpandasafety_py.libpandasafety
self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, 73 | Panda.FLAG_TOYOTA_ALT_BRAKE)
self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE | Panda.FLAG_TOYOTA_ALT_BRAKE)
self.safety.init_tests()
def _user_brake_msg(self, brake):
@@ -167,5 +170,27 @@ class TestToyotaAltBrakeSafety(TestToyotaSafety):
pass
class TestToyotaStockLongitudinal(TestToyotaSafety):
def setUp(self):
self.packer = CANPackerPanda("toyota_nodsu_pt_generated")
self.safety = libpandasafety_py.libpandasafety
self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, self.EPS_SCALE | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL)
self.safety.init_tests()
def test_accel_actuation_limits(self, stock_longitudinal=True):
super().test_accel_actuation_limits(stock_longitudinal=stock_longitudinal)
def test_acc_cancel(self):
"""
Regardless of controls allowed, never allow ACC_CONTROL if cancel bit isn't set
"""
for controls_allowed in [True, False]:
self.safety.set_controls_allowed(controls_allowed)
for accel in np.arange(MIN_ACCEL - 1, MAX_ACCEL + 1, 0.1):
self.assertFalse(self._tx(self._accel_msg(accel)))
should_tx = np.isclose(accel, 0, atol=0.0001)
self.assertEqual(should_tx, self._tx(self._accel_msg(accel, cancel_req=1)))
if __name__ == "__main__":
unittest.main()