diff --git a/VERSION b/VERSION index efdb8b1801..32e7334ebd 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -v1.1.2 \ No newline at end of file +v1.1.3 \ No newline at end of file diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 7cbeafcb96..d51a12a441 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -1,32 +1,34 @@ -struct sample_t torque_meas; // last 3 motor torques produced by the eps +int toyota_no_dsu_car = 0; // ch-r and camry don't have the DSU +int toyota_giraffe_switch_1 = 0; // is giraffe switch 1 high? // 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 +struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { @@ -36,26 +38,38 @@ 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; } + + int bus = (to_push->RDTR >> 4) & 0xF; + // 0x680 is a radar msg only found in dsu-less cars + if ((to_push->RIR>>21) == 0x680 && (bus == 1)) { + toyota_no_dsu_car = 1; + } + + // 0x2E4 is lkas cmd. If it is on bus 0, then giraffe switch 1 is high + if ((to_push->RIR>>21) == 0x2E4 && (bus == 0)) { + toyota_giraffe_switch_1 = 1; + } + } static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { @@ -70,8 +84,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 +101,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 +131,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,11 +153,19 @@ 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_giraffe_switch_1 = 0; + toyota_dbc_eps_torque_factor = param; } static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { + + // forward cam to radar and viceversa if car is dsu-less, except lkas cmd and hud + if ((bus_num == 0 || bus_num == 2) && toyota_no_dsu_car && !toyota_giraffe_switch_1) { + int addr = to_fwd->RIR>>21; + bool is_lkas_msg = (addr == 0x2E4 || addr == 0x412) && bus_num == 2; + return is_lkas_msg? -1 : (uint8_t)(~bus_num & 0x2); + } return -1; } @@ -157,8 +180,9 @@ 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_giraffe_switch_1 = 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 35b7e2b597..dc265fa03e 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/examples/tesla_tester.py b/examples/tesla_tester.py index b2cbb4d789..99d8d92854 100644 --- a/examples/tesla_tester.py +++ b/examples/tesla_tester.py @@ -29,11 +29,11 @@ def tesla_tester(): # BDY 0x248 is the MCU_commands message, which includes folding mirrors, opening the trunk, frunk, setting the cars lock state and more. For our test, we will edit the 3rd byte, which is MCU_lockRequest. 0x01 will lock, 0x02 will unlock: print("Unlocking Tesla...") - p.can_send(0x248, "\x00\x00\x02\x00\x00\x00\x00\x00", bus_num) + p.can_send(0x248, "\x00\x00\x02\x00\x00\x00\x00\x00", body_bus_num) #Or, we can set the first byte, MCU_frontHoodCommand + MCU_liftgateSwitch, to 0x01 to pop the frunk, or 0x04 to open/close the trunk (0x05 should open both) print("Opening Frunk...") - p.can_send(0x248, "\x01\x00\x00\x00\x00\x00\x00\x00", bus_num) + p.can_send(0x248, "\x01\x00\x00\x00\x00\x00\x00\x00", body_bus_num) #Back to safety... print("Disabling output on Panda...") @@ -41,7 +41,6 @@ def tesla_tester(): print("Reading VIN from 0x568. This is painfully slow and can take up to 3 minutes (1 minute per message; 3 messages needed for full VIN)...") - cnt = 0 vin = {} while True: #Read the VIN @@ -53,11 +52,10 @@ def tesla_tester(): vin_string = binascii.hexlify(dat)[2:] #rest of the string is the actual VIN data vin[vin_index] = vin_string.decode("hex") print("Got VIN index " + str(vin_index) + " data " + vin[vin_index]) - cnt += 1 #if we have all 3 parts of the VIN, print it and break out of our while loop - if cnt == 3: + if 0 in vin and 1 in vin and 2 in vin: print("VIN: " + vin[0] + vin[1] + vin[2][:3]) break if __name__ == "__main__": - tesla_tester() \ No newline at end of file + tesla_tester() diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index 8840c66f31..0af95ae47a 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -38,13 +38,13 @@ void reset_angle_control(void); 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 f0f2af506d..3d4288116e 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 ad86be63be..b4d23af65d 100644 --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -41,9 +41,9 @@ class TestToyotaSafety(unittest.TestCase): 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 @@ class TestToyotaSafety(unittest.TestCase): 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 @@ class TestToyotaSafety(unittest.TestCase): 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 @@ class TestToyotaSafety(unittest.TestCase): 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 @@ class TestToyotaSafety(unittest.TestCase): 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):