diff --git a/board/safety/safety_mazda.h b/board/safety/safety_mazda.h index 60d8b2bda9b16a..043f337c7017ce 100644 --- a/board/safety/safety_mazda.h +++ b/board/safety/safety_mazda.h @@ -51,7 +51,7 @@ void mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int cruise_engaged = GET_BYTE(to_push, 0) & 8; if (cruise_engaged != 0) { if (!mazda_cruise_engaged_last) { - controls_allowed = 1; + controls_allowed = 1; } } else { @@ -96,42 +96,42 @@ static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { if (controls_allowed) { - // *** global torque limit check *** - violation |= max_limit_check(desired_torque, MAZDA_MAX_STEER, -MAZDA_MAX_STEER); - - // *** torque rate limit check *** - int desired_torque_last = mazda_desired_torque_last; - violation |= driver_limit_check(desired_torque, desired_torque_last, &mazda_torque_driver, - MAZDA_MAX_STEER, MAZDA_MAX_RATE_UP, MAZDA_MAX_RATE_DOWN, - MAZDA_DRIVER_TORQUE_ALLOWANCE, MAZDA_DRIVER_TORQUE_FACTOR); - // used next time - mazda_desired_torque_last = desired_torque; - - // *** torque real time rate limit check *** - violation |= rt_rate_limit_check(desired_torque, mazda_rt_torque_last, MAZDA_MAX_RT_DELTA); - - // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, mazda_ts_last); - if (ts_elapsed > ((uint32_t) MAZDA_RT_INTERVAL)) { - mazda_rt_torque_last = desired_torque; - mazda_ts_last = ts; - } + // *** global torque limit check *** + violation |= max_limit_check(desired_torque, MAZDA_MAX_STEER, -MAZDA_MAX_STEER); + + // *** torque rate limit check *** + int desired_torque_last = mazda_desired_torque_last; + violation |= driver_limit_check(desired_torque, desired_torque_last, &mazda_torque_driver, + MAZDA_MAX_STEER, MAZDA_MAX_RATE_UP, MAZDA_MAX_RATE_DOWN, + MAZDA_DRIVER_TORQUE_ALLOWANCE, MAZDA_DRIVER_TORQUE_FACTOR); + // used next time + mazda_desired_torque_last = desired_torque; + + // *** torque real time rate limit check *** + violation |= rt_rate_limit_check(desired_torque, mazda_rt_torque_last, MAZDA_MAX_RT_DELTA); + + // every RT_INTERVAL set the new limits + uint32_t ts_elapsed = get_ts_elapsed(ts, mazda_ts_last); + if (ts_elapsed > ((uint32_t) MAZDA_RT_INTERVAL)) { + mazda_rt_torque_last = desired_torque; + mazda_ts_last = ts; + } } // no torque if controls is not allowed if (!controls_allowed && (desired_torque != 0)) { - violation = 1; + violation = 1; } // reset to 0 if either controls is not allowed or there's a violation if (violation || !controls_allowed) { - mazda_desired_torque_last = 0; - mazda_rt_torque_last = 0; - mazda_ts_last = ts; + mazda_desired_torque_last = 0; + mazda_rt_torque_last = 0; + mazda_ts_last = ts; } if (violation) { - tx = 0; + tx = 0; } } } @@ -149,7 +149,7 @@ static int mazda_fwd_hook(int bus, CAN_FIFOMailBox_TypeDef *to_fwd) { // drop stock CAM_LKAS and CAM_LANEINFI if OP is sending them if (!((addr == MAZDA_LKAS) && mazda_op_lkas_detected) && !((addr == MAZDA_LANEINFO) && mazda_op_laneinfo_detected)) { - bus_fwd = MAZDA_MAIN; + bus_fwd = MAZDA_MAIN; } } else {