diff --git a/panda/board/safety.h b/panda/board/safety.h index 411d757a9b..4b5a840848 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -12,7 +12,7 @@ int safety_ignition_hook(); uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last); int to_signed(int d, int bits); void update_sample(struct sample_t *sample, int sample_new); -int max_limit_check(int val, const int MAX); +int max_limit_check(int val, const int MAX, const int MIN); int dist_to_meas_check(int val, int val_last, struct sample_t *val_meas, const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR); int driver_limit_check(int val, int val_last, struct sample_t *val_driver, @@ -149,8 +149,8 @@ void update_sample(struct sample_t *sample, int sample_new) { } } -int max_limit_check(int val, const int MAX) { - return (val > MAX) | (val < -MAX); +int max_limit_check(int val, const int MAX, const int MIN) { + return (val > MAX) || (val < MIN); } // check that commanded value isn't too far from measured @@ -158,8 +158,8 @@ int dist_to_meas_check(int val, int val_last, struct sample_t *val_meas, const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR) { // *** val rate limit check *** - int16_t highest_allowed_val = max(val_last, 0) + MAX_RATE_UP; - int16_t lowest_allowed_val = min(val_last, 0) - MAX_RATE_UP; + int highest_allowed_val = max(val_last, 0) + MAX_RATE_UP; + int lowest_allowed_val = min(val_last, 0) - MAX_RATE_UP; // if we've exceeded the meas val, we must start moving toward 0 highest_allowed_val = min(highest_allowed_val, max(val_last - MAX_RATE_DOWN, max(val_meas->max, 0) + MAX_ERROR)); @@ -195,8 +195,8 @@ int driver_limit_check(int val, int val_last, struct sample_t *val_driver, int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) { // *** torque real time rate limit check *** - int16_t highest_val = max(val_last, 0) + MAX_RT_DELTA; - int16_t lowest_val = min(val_last, 0) - MAX_RT_DELTA; + int highest_val = max(val_last, 0) + MAX_RT_DELTA; + int lowest_val = min(val_last, 0) - MAX_RT_DELTA; // check for violation return (val < lowest_val) || (val > highest_val); diff --git a/panda/board/safety/safety_cadillac.h b/panda/board/safety/safety_cadillac.h index dfc15d0a35..86549d96eb 100644 --- a/panda/board/safety/safety_cadillac.h +++ b/panda/board/safety/safety_cadillac.h @@ -70,7 +70,7 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { if (controls_allowed) { // *** global torque limit check *** - violation |= max_limit_check(desired_torque, CADILLAC_MAX_STEER); + violation |= max_limit_check(desired_torque, CADILLAC_MAX_STEER, -CADILLAC_MAX_STEER); // *** torque rate limit check *** int desired_torque_last = cadillac_desired_torque_last[idx]; diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index e70332c1eb..2a6e8af067 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -163,7 +163,7 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { if (current_controls_allowed) { // *** global torque limit check *** - violation |= max_limit_check(desired_torque, GM_MAX_STEER); + violation |= max_limit_check(desired_torque, GM_MAX_STEER, -GM_MAX_STEER); // *** torque rate limit check *** violation |= driver_limit_check(desired_torque, gm_desired_torque_last, &gm_torque_driver, diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index da4bd388dd..7cbeafcb96 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -71,9 +71,8 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int desired_accel = ((to_send->RDLR & 0xFF) << 8) | ((to_send->RDLR >> 8) & 0xFF); desired_accel = to_signed(desired_accel, 16); if (controls_allowed && actuation_limits) { - if ((desired_accel > MAX_ACCEL) || (desired_accel < MIN_ACCEL)) { - return 0; - } + int violation = max_limit_check(desired_accel, MAX_ACCEL, MIN_ACCEL); + if (violation) return 0; } else if (!controls_allowed && (desired_accel != 0)) { return 0; } @@ -91,7 +90,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { if (controls_allowed && actuation_limits) { // *** global torque limit check *** - violation |= max_limit_check(desired_torque, MAX_TORQUE); + violation |= max_limit_check(desired_torque, MAX_TORQUE, -MAX_TORQUE); // *** torque rate limit check *** violation |= dist_to_meas_check(desired_torque, desired_torque_last, &torque_meas, MAX_RATE_UP, MAX_RATE_DOWN, MAX_TORQUE_ERROR); diff --git a/panda/python/isotp.py b/panda/python/isotp.py index 74720b75d9..d68aa4d70e 100644 --- a/panda/python/isotp.py +++ b/panda/python/isotp.py @@ -44,7 +44,7 @@ def isotp_recv_subaddr(panda, addr, bus, sendaddr, subaddr): idx = 1 for mm in recv(panda, (tlen-len(dat) + 5)/6, addr, bus): assert ord(mm[0]) == subaddr - assert ord(mm[1]) == (0x20 | idx) + assert ord(mm[1]) == (0x20 | (idx&0xF)) dat += mm[2:] idx += 1 elif ord(msg[1])&0xf0 == 0x00: @@ -117,7 +117,7 @@ def isotp_recv(panda, addr, bus=0, sendaddr=None, subaddr=None): idx = 1 for mm in recv(panda, (tlen-len(dat) + 6)/7, addr, bus): - assert ord(mm[0]) == (0x20 | idx) + assert ord(mm[0]) == (0x20 | (idx&0xF)) dat += mm[1:] idx += 1 elif ord(msg[0])&0xf0 == 0x00: