Chrysler: disengage on gas press (#442)
* add gas cancellation for chrysler * added regr test too * also add bus checks to chrysler. SHould have been there alreadymaster
parent
2ebbe3616b
commit
b2ffaae60e
|
@ -4,18 +4,23 @@ const uint32_t CHRYSLER_RT_INTERVAL = 250000; // 250ms between real time checks
|
|||
const int CHRYSLER_MAX_RATE_UP = 3;
|
||||
const int CHRYSLER_MAX_RATE_DOWN = 3;
|
||||
const int CHRYSLER_MAX_TORQUE_ERROR = 80; // max torque cmd in excess of torque motor
|
||||
const int CHRYSLER_GAS_THRSLD = 30; // 7% more than 2m/s
|
||||
const AddrBus CHRYSLER_TX_MSGS[] = {{571, 0}, {658, 0}, {678, 0}};
|
||||
|
||||
// TODO: do checksum and counter checks
|
||||
AddrCheckStruct chrysler_rx_checks[] = {
|
||||
{.addr = {544}, .bus = 0, .expected_timestep = 10000U},
|
||||
{.addr = {514}, .bus = 0, .expected_timestep = 10000U},
|
||||
{.addr = {500}, .bus = 0, .expected_timestep = 20000U},
|
||||
{.addr = {308}, .bus = 0, .expected_timestep = 20000U}, // verify ts
|
||||
};
|
||||
const int CHRYSLER_RX_CHECK_LEN = sizeof(chrysler_rx_checks) / sizeof(chrysler_rx_checks[0]);
|
||||
|
||||
int chrysler_rt_torque_last = 0;
|
||||
int chrysler_desired_torque_last = 0;
|
||||
int chrysler_cruise_engaged_last = 0;
|
||||
bool chrysler_gas_prev = false;
|
||||
int chrysler_speed = 0;
|
||||
uint32_t chrysler_ts_last = 0;
|
||||
struct sample_t chrysler_torque_meas; // last few torques measured
|
||||
|
||||
|
@ -29,7 +34,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
int addr = GET_ADDR(to_push);
|
||||
|
||||
// Measured eps torque
|
||||
if (addr == 544) {
|
||||
if ((addr == 544) && (bus == 0)) {
|
||||
int torque_meas_new = ((GET_BYTE(to_push, 4) & 0x7U) << 8) + GET_BYTE(to_push, 5) - 1024U;
|
||||
|
||||
// update array of samples
|
||||
|
@ -37,7 +42,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
}
|
||||
|
||||
// enter controls on rising edge of ACC, exit controls on ACC off
|
||||
if (addr == 0x1F4) {
|
||||
if ((addr == 500) && (bus == 0)) {
|
||||
int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38) >> 3) == 7;
|
||||
if (cruise_engaged && !chrysler_cruise_engaged_last) {
|
||||
controls_allowed = 1;
|
||||
|
@ -48,7 +53,21 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
chrysler_cruise_engaged_last = cruise_engaged;
|
||||
}
|
||||
|
||||
// TODO: add gas pressed check
|
||||
// update speed
|
||||
if ((addr == 514) && (bus == 0)) {
|
||||
int speed_l = (GET_BYTE(to_push, 0) << 4) + (GET_BYTE(to_push, 1) >> 4);
|
||||
int speed_r = (GET_BYTE(to_push, 2) << 4) + (GET_BYTE(to_push, 3) >> 4);
|
||||
chrysler_speed = (speed_l + speed_r) / 2;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of gas press
|
||||
if ((addr == 308) && (bus == 0)) {
|
||||
bool gas = (GET_BYTE(to_push, 5) & 0x7F) != 0;
|
||||
if (gas && !chrysler_gas_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
chrysler_gas_prev = gas;
|
||||
}
|
||||
|
||||
// check if stock camera ECU is on bus 0
|
||||
if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x292)) {
|
||||
|
|
|
@ -312,6 +312,8 @@ void init_tests_hyundai(void){
|
|||
|
||||
void init_tests_chrysler(void){
|
||||
init_tests();
|
||||
chrysler_gas_prev = false;
|
||||
chrysler_speed = 0;
|
||||
chrysler_torque_meas.min = 0;
|
||||
chrysler_torque_meas.max = 0;
|
||||
chrysler_desired_torque_last = 0;
|
||||
|
|
|
@ -28,6 +28,18 @@ class TestChryslerSafety(unittest.TestCase):
|
|||
to_send[0].RDLR = buttons
|
||||
return to_send
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
speed = int(speed / 0.071028)
|
||||
to_send = make_msg(0, 514, 4)
|
||||
to_send[0].RDLR = ((speed & 0xFF0) >> 4) + ((speed & 0xF) << 12) + \
|
||||
((speed & 0xFF0) << 12) + ((speed & 0xF) << 28)
|
||||
return to_send
|
||||
|
||||
def _gas_msg(self, gas):
|
||||
to_send = make_msg(0, 308)
|
||||
to_send[0].RDHR = (gas & 0x7F) << 8
|
||||
return to_send
|
||||
|
||||
def _set_prev_torque(self, t):
|
||||
self.safety.set_chrysler_desired_torque_last(t)
|
||||
self.safety.set_chrysler_rt_torque_last(t)
|
||||
|
@ -80,6 +92,16 @@ class TestChryslerSafety(unittest.TestCase):
|
|||
self.safety.safety_rx_hook(to_push)
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_gas_disable(self):
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._speed_msg(2.2))
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(self._gas_msg(0))
|
||||
self.safety.safety_rx_hook(self._speed_msg(2.3))
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_non_realtime_limit_up(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
|
|
Loading…
Reference in New Issue