Skip to content

safety: recover Toyota LTA after torque oscillation #3686

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
danielpolimac wants to merge 1 commit into
base: master
Choose a base branch
from
+18 −2
Open

safety: recover Toyota LTA after torque oscillation #3686

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions opendbc/safety/modes/toyota.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -296,12 +296,14 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
}

// check if we should wind down torque
int driver_torque = SAFETY_MIN(SAFETY_ABS(torque_driver.min), SAFETY_ABS(torque_driver.max));
// Use the current and 60ms-old samples. A min/max over the full window
// can keep a stale oscillation above the wind-down threshold.
int driver_torque = SAFETY_MIN(SAFETY_ABS(torque_driver.values[0]), SAFETY_ABS(torque_driver.values[3]));
if ((driver_torque > TOYOTA_LTA_MAX_DRIVER_TORQUE) && (torque_wind_down != 0)) {
tx = false;
}

int eps_torque = SAFETY_MIN(SAFETY_ABS(torque_meas.min), SAFETY_ABS(torque_meas.max));
int eps_torque = SAFETY_MIN(SAFETY_ABS(torque_meas.values[0]), SAFETY_ABS(torque_meas.values[3]));
if ((eps_torque > TOYOTA_LTA_MAX_MEAS_TORQUE) && (torque_wind_down != 0)) {
tx = false;
}
Expand Down
14 changes: 14 additions & 0 deletions opendbc/safety/tests/test_toyota.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -243,6 +243,20 @@ def test_lta_steer_cmd(self):
self.assertEqual(should_tx, self._tx(self._lta_msg(1, 1, angle, 100)))
self.assertTrue(self._tx(self._lta_msg(1, 1, angle, 0))) # should tx if we wind down torque

def test_lta_torque_wind_down_recovers_after_oscillation(self):
"""Recent zero torque must clear a previous oscillating EPS torque."""
self.safety.set_controls_allowed(True)
self._reset_angle_measurement(0)
self._set_prev_desired_angle(0)

high_torque = self.MAX_MEAS_TORQUE + 100
for torque in (-high_torque, high_torque) * 3:
self._rx(self._torque_meas_msg(torque, 0))
self.assertFalse(self._tx(self._lta_msg(1, 1, 0, 100)))

self._rx(self._torque_meas_msg(0, 0))
self.assertTrue(self._tx(self._lta_msg(1, 1, 0, 100)))

def test_angle_measurements(self):
"""
* Tests angle meas quality flag dictates whether angle measurement is parsed, and if rx is valid
Expand Down
Loading