Skip to content

Commit

Permalink
Revert "added steer override check when IPAS is in control (#106)"
Browse files Browse the repository at this point in the history 
This reverts commit 7d21acb.
  • Loading branch information
@rbiasini
rbiasini committed Apr 11, 2018
1 parent ea1c1dc commit 02c1ddf
Show file tree
Hide file tree
Showing 3 changed files with 6 additions and 127 deletions.
2 changes: 1 addition & 1 deletion VERSION
View file
Original file line number Diff line number Diff line change
@@ -1 +1 @@
v1.1.1
v1.1.0
56 changes: 5 additions & 51 deletions board/safety/safety_toyota.h
View file
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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;
}
}

Expand All @@ -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;
Expand Down
75 changes: 0 additions & 75 deletions tests/safety/test_toyota.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,6 @@

MAX_TORQUE_ERROR = 350

IPAS_OVERRIDE_THRESHOLD = 200

def twos_comp(val, bits):
if val >= 0:
return val
Expand Down Expand Up @@ -44,18 +42,6 @@ def _torque_meas_msg(self, torque):
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
Expand All @@ -64,19 +50,6 @@ def _torque_msg(self, torque):
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
Expand Down Expand Up @@ -207,54 +180,6 @@ def test_torque_measurements(self):
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()

0 comments on commit 02c1ddf

Please sign in to comment.