Merge panda subtree

panda/VERSION

@@ -1 +1 @@
-v1.1.2
+v1.1.3

panda/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 = {

panda/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;
   }

panda/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()
+  tesla_tester()

panda/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);

panda/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);
 }
 

panda/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):