diff --git a/board/safety/safety_cadillac.h b/board/safety/safety_cadillac.h index aedd49719342a9..a0e13d220fbe42 100644 --- a/board/safety/safety_cadillac.h +++ b/board/safety/safety_cadillac.h @@ -11,11 +11,18 @@ const int CADILLAC_DRIVER_TORQUE_FACTOR = 4; int cadillac_ign = 0; int cadillac_cruise_engaged_last = 0; int cadillac_rt_torque_last = 0; -int cadillac_desired_torque_last = 0; +int cadillac_desired_torque_last[4] = {0}; // 4 torque messages uint32_t cadillac_ts_last = 0; struct sample_t cadillac_torque_driver; // last 3 driver torques measured +int cadillac_get_torque_idx(uint32_t addr) { + if (addr==0x151) return 0; + else if (addr==0x152) return 1; + else if (addr==0x153) return 2; + else return 3; +} + static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int bus_number = (to_push->RDTR >> 4) & 0xFF; uint32_t addr = to_push->RIR >> 21; @@ -53,6 +60,7 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int desired_torque = ((to_send->RDLR & 0x3f) << 8) + ((to_send->RDLR & 0xff00) >> 8); int violation = 0; uint32_t ts = TIM2->CNT; + int idx = cadillac_get_torque_idx(addr); desired_torque = to_signed(desired_torque, 14); if (controls_allowed) { @@ -63,8 +71,8 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } // *** torque rate limit check *** - int highest_allowed_torque = max(cadillac_desired_torque_last, 0) + CADILLAC_MAX_RATE_UP; - int lowest_allowed_torque = min(cadillac_desired_torque_last, 0) - CADILLAC_MAX_RATE_UP; + int highest_allowed_torque = max(cadillac_desired_torque_last[idx], 0) + CADILLAC_MAX_RATE_UP; + int lowest_allowed_torque = min(cadillac_desired_torque_last[idx], 0) - CADILLAC_MAX_RATE_UP; int driver_torque_max_limit = CADILLAC_STEER_MAX + (CADILLAC_DRIVER_TORQUE_ALLOWANCE + cadillac_torque_driver.max) * @@ -75,10 +83,10 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // if we've exceeded the applied torque, we must start moving toward 0 highest_allowed_torque = min(highest_allowed_torque, - max(cadillac_desired_torque_last - CADILLAC_MAX_RATE_DOWN, + max(cadillac_desired_torque_last[idx] - CADILLAC_MAX_RATE_DOWN, max(driver_torque_max_limit, 0))); lowest_allowed_torque = max(lowest_allowed_torque, - min(cadillac_desired_torque_last + CADILLAC_MAX_RATE_DOWN, + min(cadillac_desired_torque_last[idx] + CADILLAC_MAX_RATE_DOWN, min(driver_torque_min_limit, 0))); // check for violation @@ -87,7 +95,7 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { } //// used next time - cadillac_desired_torque_last = desired_torque; + cadillac_desired_torque_last[idx] = desired_torque; // *** torque real time rate limit check *** int highest_rt_torque = max(cadillac_rt_torque_last, 0) + CADILLAC_MAX_RT_DELTA; @@ -114,7 +122,7 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // reset to 0 if either controls is not allowed or there's a violation if (violation || !controls_allowed) { - cadillac_desired_torque_last = 0; + cadillac_desired_torque_last[idx] = 0; cadillac_rt_torque_last = 0; cadillac_ts_last = ts; } diff --git a/tests/safety/test.c b/tests/safety/test.c index 353b4fab57dec2..6d387de14d7cad 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -90,7 +90,7 @@ void set_desired_torque_last(int t){ } void set_cadillac_desired_torque_last(int t){ - cadillac_desired_torque_last = t; + for (int i = 0; i < 4; i++) cadillac_desired_torque_last[i] = t; } int get_ego_speed(void){ @@ -117,7 +117,7 @@ void init_tests_toyota(void){ void init_tests_cadillac(void){ cadillac_torque_driver.min = 0; cadillac_torque_driver.max = 0; - cadillac_desired_torque_last = 0; + for (int i = 0; i < 4; i++) cadillac_desired_torque_last[i] = 0; cadillac_rt_torque_last = 0; cadillac_ts_last = 0; set_timer(0);