retropilot
/
panda
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Revert "added steer override check when IPAS is in control (#106)" 

This reverts commit 7d21acbccc.
master
Riccardo 2018-04-11 14:00:25 -07:00
parent ea1c1dca48
commit 02c1ddf5fc
3 changed files with 6 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
VERSION
View File

@ -1 +1 @@
v1.1.1
v1.1.0

56
board/safety/safety_toyota.h
View File

@ -1,15 +1,6 @@
int cruise_engaged_last = 0; // cruise state
int ipas_state = 0;
// track the torque measured for limiting
int16_t torque_meas[3] = {0, 0, 0}; // last 3 motor torques produced by the eps
int16_t torque_meas_min = 0, torque_meas_max = 0;
int16_t torque_driver[3] = {0, 0, 0}; // last 3 driver steering torque
int16_t torque_driver_min = 0, torque_driver_max = 0;
// IPAS override
const int32_t IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value
int angle_control = 0; // 1 if direct angle control packets are seen
// global torque limit
const int32_t MAX_TORQUE = 1500; // max torque cmd allowed ever
@ -39,10 +30,8 @@ int16_t rt_torque_last = 0; // last desired torque for real time chec
uint32_t ts_last = 0;
static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// EPS torque sensor
// get eps motor torque (0.66 factor in dbc)
if ((to_push->RIR>>21) == 0x260) {
// get eps motor torque (see dbc_eps_torque_factor in dbc)
int16_t torque_meas_new_16 = (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF));
// increase torque_meas by 1 to be conservative on rounding
@ -60,46 +49,16 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
if (torque_meas[i] < torque_meas_min) torque_meas_min = torque_meas[i];
if (torque_meas[i] > torque_meas_max) torque_meas_max = torque_meas[i];
}
// get driver steering torque
int16_t torque_driver_new = (((to_push->RDLR) & 0xFF00) | ((to_push->RDLR >> 16) & 0xFF));
// shift the array
for (int i = sizeof(torque_driver)/sizeof(torque_driver[0]) - 1; i > 0; i--) {
torque_driver[i] = torque_driver[i-1];
}
torque_driver[0] = torque_driver_new;
// get the minimum and maximum driver torque over the last 3 frames
torque_driver_min = torque_driver_max = torque_driver[0];
for (int i = 1; i < sizeof(torque_driver)/sizeof(torque_driver[0]); i++) {
if (torque_driver[i] < torque_driver_min) torque_driver_min = torque_driver[i];
if (torque_driver[i] > torque_driver_max) torque_driver_max = torque_driver[i];
}
}
// enter controls on rising edge of ACC, exit controls on ACC off
// exit controls on ACC off
if ((to_push->RIR>>21) == 0x1D2) {
// 4 bits: 55-52
int cruise_engaged = to_push->RDHR & 0xF00000;
if (cruise_engaged && (!cruise_engaged_last)) {
if (to_push->RDHR & 0xF00000) {
controls_allowed = 1;
} else if (!cruise_engaged) {
} else {
controls_allowed = 0;
}
cruise_engaged_last = cruise_engaged;
}
// get ipas state
if ((to_push->RIR>>21) == 0x262) {
ipas_state = (to_push->RDLR & 0xf);
}
// exit controls on high steering override
if (angle_control && ((torque_driver_min > IPAS_OVERRIDE_THRESHOLD) ||
(torque_driver_max < -IPAS_OVERRIDE_THRESHOLD) ||
(ipas_state==5))) {
controls_allowed = 0;
}
}
@ -120,12 +79,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
}
}
// STEER ANGLE
if ((to_send->RIR>>21) == 0x266) {
angle_control = 1;
}
// STEER TORQUE: safety check on bytes 2-3
// STEER: safety check on bytes 2-3
if ((to_send->RIR>>21) == 0x2E4) {
int16_t desired_torque = (to_send->RDLR & 0xFF00) | ((to_send->RDLR >> 16) & 0xFF);
int16_t violation = 0;

75
tests/safety/test_toyota.py
View File

@ -15,8 +15,6 @@ RT_INTERVAL = 250000
MAX_TORQUE_ERROR = 350
IPAS_OVERRIDE_THRESHOLD = 200
def twos_comp(val, bits):
if val >= 0:
return val
@ -44,18 +42,6 @@ class TestToyotaSafety(unittest.TestCase):
to_send[0].RDHR = t | ((t & 0xFF) << 16)
return to_send
def _torque_driver_msg(self, torque):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x260 << 21
t = twos_comp(torque, 16)
to_send[0].RDLR = t | ((t & 0xFF) << 16)
return to_send
def _torque_driver_msg_array(self, torque):
for i in range(3):
self.safety.toyota_rx_hook(self._torque_driver_msg(torque))
def _torque_msg(self, torque):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x2E4 << 21
@ -64,19 +50,6 @@ class TestToyotaSafety(unittest.TestCase):
to_send[0].RDLR = t | ((t & 0xFF) << 16)
return to_send
def _ipas_state_msg(self, state):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x262 << 21
to_send[0].RDLR = state & 0xF
return to_send
def _ipas_control_msg(self):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x266 << 21
return to_send
def _accel_msg(self, accel):
to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
to_send[0].RIR = 0x343 << 21
@ -207,54 +180,6 @@ class TestToyotaSafety(unittest.TestCase):
self.assertEqual(-1, self.safety.get_torque_meas_max())
self.assertEqual(-1, self.safety.get_torque_meas_min())
def test_ipas_override(self):
## angle control is not active
self.safety.set_controls_allowed(1)
# 3 consecutive msgs where driver exceeds threshold but angle_control isn't active
self.safety.set_controls_allowed(1)
self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1)
self.assertTrue(self.safety.get_controls_allowed())
self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1)
self.assertTrue(self.safety.get_controls_allowed())
# ipas state is override
self.safety.toyota_rx_hook(self._ipas_state_msg(5))
self.assertTrue(self.safety.get_controls_allowed())
## now angle control is active
self.safety.toyota_tx_hook(self._ipas_control_msg())
self.safety.toyota_rx_hook(self._ipas_state_msg(0))
# 3 consecutive msgs where driver does exceed threshold
self.safety.set_controls_allowed(1)
self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD + 1)
self.assertFalse(self.safety.get_controls_allowed())
self.safety.set_controls_allowed(1)
self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD - 1)
self.assertFalse(self.safety.get_controls_allowed())
# ipas state is override and torque isn't overriding any more
self.safety.set_controls_allowed(1)
self._torque_driver_msg_array(0)
self.safety.toyota_rx_hook(self._ipas_state_msg(5))
self.assertFalse(self.safety.get_controls_allowed())
# 3 consecutive msgs where driver does not exceed threshold and
# ipas state is not override
self.safety.set_controls_allowed(1)
self.safety.toyota_rx_hook(self._ipas_state_msg(0))
self.assertTrue(self.safety.get_controls_allowed())
self._torque_driver_msg_array(IPAS_OVERRIDE_THRESHOLD)
self.assertTrue(self.safety.get_controls_allowed())
self._torque_driver_msg_array(-IPAS_OVERRIDE_THRESHOLD)
self.assertTrue(self.safety.get_controls_allowed())
if __name__ == "__main__":
unittest.main()