diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 7cbeafcb96f65d..57f86b8ab0bfd7 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -1,32 +1,32 @@ -struct sample_t torque_meas; // last 3 motor torques produced by the eps +struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps // global torque limit -const int MAX_TORQUE = 1500; // max torque cmd allowed ever +const int TOYOTA_MAX_TORQUE = 1500; // max torque cmd allowed ever // rate based torque limit + stay within actually applied // packet is sent at 100hz, so this limit is 1000/sec -const int MAX_RATE_UP = 10; // ramp up slow -const int MAX_RATE_DOWN = 25; // ramp down fast -const int MAX_TORQUE_ERROR = 350; // max torque cmd in excess of torque motor +const int TOYOTA_MAX_RATE_UP = 10; // ramp up slow +const int TOYOTA_MAX_RATE_DOWN = 25; // ramp down fast +const int TOYOTA_MAX_TORQUE_ERROR = 350; // max torque cmd in excess of torque motor // real time torque limit to prevent controls spamming // the real time limit is 1500/sec -const int MAX_RT_DELTA = 375; // max delta torque allowed for real time checks -const int RT_INTERVAL = 250000; // 250ms between real time checks +const int TOYOTA_MAX_RT_DELTA = 375; // max delta torque allowed for real time checks +const int TOYOTA_RT_INTERVAL = 250000; // 250ms between real time checks // longitudinal limits -const int MAX_ACCEL = 1500; // 1.5 m/s2 -const int MIN_ACCEL = -3000; // 3.0 m/s2 +const int TOYOTA_MAX_ACCEL = 1500; // 1.5 m/s2 +const int TOYOTA_MIN_ACCEL = -3000; // 3.0 m/s2 // global actuation limit state -int actuation_limits = 1; // by default steer limits are imposed -int dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file +int toyota_actuation_limits = 1; // by default steer limits are imposed +int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file // state of torque limits -int desired_torque_last = 0; // last desired steer torque -int rt_torque_last = 0; // last desired torque for real time check -uint32_t ts_last = 0; -int cruise_engaged_last = 0; // cruise state +int toyota_desired_torque_last = 0; // last desired steer torque +int toyota_rt_torque_last = 0; // last desired torque for real time check +uint32_t toyota_ts_last = 0; +int toyota_cruise_engaged_last = 0; // cruise state static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { @@ -36,25 +36,25 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { torque_meas_new = to_signed(torque_meas_new, 16); // scale by dbc_factor - torque_meas_new = (torque_meas_new * dbc_eps_torque_factor) / 100; + torque_meas_new = (torque_meas_new * toyota_dbc_eps_torque_factor) / 100; // increase torque_meas by 1 to be conservative on rounding torque_meas_new += (torque_meas_new > 0 ? 1 : -1); // update array of sample - update_sample(&torque_meas, torque_meas_new); + update_sample(&toyota_torque_meas, torque_meas_new); } // enter controls on rising edge of ACC, 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 (cruise_engaged && !toyota_cruise_engaged_last) { controls_allowed = 1; } else if (!cruise_engaged) { controls_allowed = 0; } - cruise_engaged_last = cruise_engaged; + toyota_cruise_engaged_last = cruise_engaged; } } @@ -70,8 +70,8 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { if ((to_send->RIR>>21) == 0x343) { 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) { - int violation = max_limit_check(desired_accel, MAX_ACCEL, MIN_ACCEL); + if (controls_allowed && toyota_actuation_limits) { + int violation = max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL); if (violation) return 0; } else if (!controls_allowed && (desired_accel != 0)) { return 0; @@ -87,25 +87,26 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { uint32_t ts = TIM2->CNT; // only check if controls are allowed and actuation_limits are imposed - if (controls_allowed && actuation_limits) { + if (controls_allowed && toyota_actuation_limits) { // *** global torque limit check *** - violation |= max_limit_check(desired_torque, MAX_TORQUE, -MAX_TORQUE); + violation |= max_limit_check(desired_torque, TOYOTA_MAX_TORQUE, -TOYOTA_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); + violation |= dist_to_meas_check(desired_torque, toyota_desired_torque_last, + &toyota_torque_meas, TOYOTA_MAX_RATE_UP, TOYOTA_MAX_RATE_DOWN, TOYOTA_MAX_TORQUE_ERROR); // used next time - desired_torque_last = desired_torque; + toyota_desired_torque_last = desired_torque; // *** torque real time rate limit check *** - violation |= rt_rate_limit_check(desired_torque, rt_torque_last, MAX_RT_DELTA); + violation |= rt_rate_limit_check(desired_torque, toyota_rt_torque_last, TOYOTA_MAX_RT_DELTA); // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last); - if (ts_elapsed > RT_INTERVAL) { - rt_torque_last = desired_torque; - ts_last = ts; + uint32_t ts_elapsed = get_ts_elapsed(ts, toyota_ts_last); + if (ts_elapsed > TOYOTA_RT_INTERVAL) { + toyota_rt_torque_last = desired_torque; + toyota_ts_last = ts; } } @@ -116,9 +117,9 @@ static int toyota_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) { - desired_torque_last = 0; - rt_torque_last = 0; - ts_last = ts; + toyota_desired_torque_last = 0; + toyota_rt_torque_last = 0; + toyota_ts_last = ts; } if (violation) { @@ -138,8 +139,8 @@ static int toyota_tx_lin_hook(int lin_num, uint8_t *data, int len) { static void toyota_init(int16_t param) { controls_allowed = 0; - actuation_limits = 1; - dbc_eps_torque_factor = param; + toyota_actuation_limits = 1; + toyota_dbc_eps_torque_factor = param; } static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { @@ -157,8 +158,8 @@ const safety_hooks toyota_hooks = { static void toyota_nolimits_init(int16_t param) { controls_allowed = 0; - actuation_limits = 0; - dbc_eps_torque_factor = param; + toyota_actuation_limits = 0; + toyota_dbc_eps_torque_factor = param; } const safety_hooks toyota_nolimits_hooks = { diff --git a/board/safety/safety_toyota_ipas.h b/board/safety/safety_toyota_ipas.h index 35b7e2b5974a64..dc265fa03e55c7 100644 --- a/board/safety/safety_toyota_ipas.h +++ b/board/safety/safety_toyota_ipas.h @@ -2,7 +2,7 @@ // TODO: refactor to repeat less code // IPAS override -const int32_t IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value +const int32_t TOYOTA_IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value struct lookup_t { float x[3]; @@ -92,7 +92,7 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // every RT_INTERVAL or when controls are turned on, set the new limits uint32_t ts_elapsed = get_ts_elapsed(ts, ts_angle_last); - if ((ts_elapsed > RT_INTERVAL) || (controls_allowed && !controls_allowed_last)) { + if ((ts_elapsed > TOYOTA_RT_INTERVAL) || (controls_allowed && !controls_allowed_last)) { rt_angle_last = angle_meas_new; ts_angle_last = ts; } @@ -118,8 +118,8 @@ static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // exit controls on high steering override - if (angle_control && ((torque_driver.min > IPAS_OVERRIDE_THRESHOLD) || - (torque_driver.max < -IPAS_OVERRIDE_THRESHOLD) || + if (angle_control && ((torque_driver.min > TOYOTA_IPAS_OVERRIDE_THRESHOLD) || + (torque_driver.max < -TOYOTA_IPAS_OVERRIDE_THRESHOLD) || (ipas_state==5))) { controls_allowed = 0; } diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index 8840c66f315dd1..0af95ae47a4e3d 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -38,13 +38,13 @@ int get_controls_allowed(void); void init_tests_toyota(void); void set_timer(int t); -void set_torque_meas(int min, int max); +void set_toyota_torque_meas(int min, int max); void set_cadillac_torque_driver(int min, int max); void set_gm_torque_driver(int min, int max); -void set_rt_torque_last(int t); -void set_desired_torque_last(int t); -int get_torque_meas_min(void); -int get_torque_meas_max(void); +void set_toyota_rt_torque_last(int t); +void set_toyota_desired_torque_last(int t); +int get_toyota_torque_meas_min(void); +int get_toyota_torque_meas_max(void); void init_tests_honda(void); int get_ego_speed(void); diff --git a/tests/safety/test.c b/tests/safety/test.c index f0f2af506da58b..3d4288116e4a47 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -22,7 +22,7 @@ typedef struct uint32_t CNT; } TIM_TypeDef; -struct sample_t torque_meas; +struct sample_t toyota_torque_meas; struct sample_t cadillac_torque_driver; struct sample_t gm_torque_driver; @@ -60,9 +60,9 @@ void set_timer(int t){ timer.CNT = t; } -void set_torque_meas(int min, int max){ - torque_meas.min = min; - torque_meas.max = max; +void set_toyota_torque_meas(int min, int max){ + toyota_torque_meas.min = min; + toyota_torque_meas.max = max; } void set_cadillac_torque_driver(int min, int max){ @@ -75,16 +75,16 @@ void set_gm_torque_driver(int min, int max){ gm_torque_driver.max = max; } -int get_torque_meas_min(void){ - return torque_meas.min; +int get_toyota_torque_meas_min(void){ + return toyota_torque_meas.min; } -int get_torque_meas_max(void){ - return torque_meas.max; +int get_toyota_torque_meas_max(void){ + return toyota_torque_meas.max; } -void set_rt_torque_last(int t){ - rt_torque_last = t; +void set_toyota_rt_torque_last(int t){ + toyota_rt_torque_last = t; } void set_cadillac_rt_torque_last(int t){ @@ -95,8 +95,8 @@ void set_gm_rt_torque_last(int t){ gm_rt_torque_last = t; } -void set_desired_torque_last(int t){ - desired_torque_last = t; +void set_toyota_desired_torque_last(int t){ + toyota_desired_torque_last = t; } void set_cadillac_desired_torque_last(int t){ @@ -129,11 +129,11 @@ void set_bosch_hardware(bool c){ } void init_tests_toyota(void){ - torque_meas.min = 0; - torque_meas.max = 0; - desired_torque_last = 0; - rt_torque_last = 0; - ts_last = 0; + toyota_torque_meas.min = 0; + toyota_torque_meas.max = 0; + toyota_desired_torque_last = 0; + toyota_rt_torque_last = 0; + toyota_ts_last = 0; set_timer(0); } diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py index ad86be63beb64b..b4d23af65df321 100644 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -41,9 +41,9 @@ def setUp(cls): cls.safety.init_tests_toyota() def _set_prev_torque(self, t): - self.safety.set_desired_torque_last(t) - self.safety.set_rt_torque_last(t) - self.safety.set_torque_meas(t, t) + self.safety.set_toyota_desired_torque_last(t) + self.safety.set_toyota_rt_torque_last(t) + self.safety.set_toyota_torque_meas(t, t) def _torque_meas_msg(self, torque): to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') @@ -158,9 +158,9 @@ def test_torque_absolute_limits(self): for controls_allowed in [True, False]: for torque in np.arange(-MAX_TORQUE - 1000, MAX_TORQUE + 1000, MAX_RATE_UP): self.safety.set_controls_allowed(controls_allowed) - self.safety.set_rt_torque_last(torque) - self.safety.set_torque_meas(torque, torque) - self.safety.set_desired_torque_last(torque - MAX_RATE_UP) + self.safety.set_toyota_rt_torque_last(torque) + self.safety.set_toyota_torque_meas(torque, torque) + self.safety.set_toyota_desired_torque_last(torque - MAX_RATE_UP) if controls_allowed: send = (-MAX_TORQUE <= torque <= MAX_TORQUE) @@ -181,14 +181,14 @@ def test_non_realtime_limit_up(self): def test_non_realtime_limit_down(self): self.safety.set_controls_allowed(True) - self.safety.set_rt_torque_last(1000) - self.safety.set_torque_meas(500, 500) - self.safety.set_desired_torque_last(1000) + self.safety.set_toyota_rt_torque_last(1000) + self.safety.set_toyota_torque_meas(500, 500) + self.safety.set_toyota_desired_torque_last(1000) self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN))) - self.safety.set_rt_torque_last(1000) - self.safety.set_torque_meas(500, 500) - self.safety.set_desired_torque_last(1000) + self.safety.set_toyota_rt_torque_last(1000) + self.safety.set_toyota_torque_meas(500, 500) + self.safety.set_toyota_desired_torque_last(1000) self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN + 1))) def test_exceed_torque_sensor(self): @@ -210,14 +210,14 @@ def test_realtime_limit_up(self): self._set_prev_torque(0) for t in np.arange(0, 380, 10): t *= sign - self.safety.set_torque_meas(t, t) + self.safety.set_toyota_torque_meas(t, t) self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t))) self.assertFalse(self.safety.toyota_tx_hook(self._torque_msg(sign * 380))) self._set_prev_torque(0) for t in np.arange(0, 370, 10): t *= sign - self.safety.set_torque_meas(t, t) + self.safety.set_toyota_torque_meas(t, t) self.assertTrue(self.safety.toyota_tx_hook(self._torque_msg(t))) # Increase timer to update rt_torque_last @@ -233,16 +233,16 @@ def test_torque_measurements(self): self.safety.toyota_rx_hook(self._torque_meas_msg(0)) self.safety.toyota_rx_hook(self._torque_meas_msg(0)) - self.assertEqual(-51, self.safety.get_torque_meas_min()) - self.assertEqual(51, self.safety.get_torque_meas_max()) + self.assertEqual(-51, self.safety.get_toyota_torque_meas_min()) + self.assertEqual(51, self.safety.get_toyota_torque_meas_max()) self.safety.toyota_rx_hook(self._torque_meas_msg(0)) - self.assertEqual(-1, self.safety.get_torque_meas_max()) - self.assertEqual(-51, self.safety.get_torque_meas_min()) + self.assertEqual(-1, self.safety.get_toyota_torque_meas_max()) + self.assertEqual(-51, self.safety.get_toyota_torque_meas_min()) self.safety.toyota_rx_hook(self._torque_meas_msg(0)) - self.assertEqual(-1, self.safety.get_torque_meas_max()) - self.assertEqual(-1, self.safety.get_torque_meas_min()) + self.assertEqual(-1, self.safety.get_toyota_torque_meas_max()) + self.assertEqual(-1, self.safety.get_toyota_torque_meas_min()) def test_ipas_override(self):