Merge pull request #177 from xx979xx/safety_debug

Panda Safety debug
xx979xx · Oct 30, 2020 · 025c3fe · 025c3fe
2 parents 09bd519 + 11825a6
commit 025c3fe
Show file tree

Hide file tree

Showing 5 changed files with 103 additions and 19 deletions.
diff --git a/panda/board/drivers/usb.h b/panda/board/drivers/usb.h
@@ -842,7 +842,7 @@ void usb_irqhandler(void) {
       // TODO: why was this here? fires when TX buffers when we can't clear NAK
       // USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U;
       // USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
-    } else if ((USBx_OUTEP(3)->DOEPINT) != 0) {
+    } else if ((USBx_OUTEP(3)->DOEPINT) != 0 && (USBx_OUTEP(3)->DOEPINT) != 0x10) {
       puts("OUTEP3 error ");
       puth(USBx_OUTEP(3)->DOEPINT);
       puts("\n");
@@ -1051,4 +1051,4 @@ void usb_init(void) {
 
   // enable the IRQ
   NVIC_EnableIRQ(OTG_FS_IRQn);
-}
+}
diff --git a/panda/board/safety.h b/panda/board/safety.h
@@ -138,6 +138,7 @@ void safety_tick(const safety_hooks *hooks) {
       hooks->addr_check[i].lagging = lagging;
       if (lagging) {
         controls_allowed = 0;
+        puts("  CAN msg ("); puth(hooks->addr_check[i].msg[hooks->addr_check[i].index].addr); puts(") lags: controls not allowed\n");
       }
     }
   }
@@ -158,6 +159,9 @@ bool is_msg_valid(AddrCheckStruct addr_list[], int index) {
     if ((!addr_list[index].valid_checksum) || (addr_list[index].wrong_counters >= MAX_WRONG_COUNTERS)) {
       valid = false;
       controls_allowed = 0;
+      int addrr = addr_list[index].msg[addr_list[index].index].addr;
+      if (!addr_list[index].valid_checksum){puts("  CAN msg ("); puth(addrr); puts(") checksum invalid: controls not allowed\n");}
+      else {puts("  CAN msg ("); puth(addrr); puts(") wrong counter: controls not allowed\n");}
     }
   }
   return valid;
@@ -205,12 +209,14 @@ void generic_rx_checks(bool stock_ecu_detected) {
   // exit controls on rising edge of gas press
   if (gas_pressed && !gas_pressed_prev && !(unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS)) {
     controls_allowed = 0;
+    puts("  gas pressed w/ long control: controls not allowed"); puts("\n");
   }
   gas_pressed_prev = gas_pressed;
 
   // exit controls on rising edge of brake press
   if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
     controls_allowed = 0;
+    puts("  brake pressed w/ long control: controls not allowed"); puts("\n");
   }
   brake_pressed_prev = brake_pressed;
 

diff --git a/panda/board/safety/safety_defaults.h b/panda/board/safety/safety_defaults.h
@@ -14,10 +14,10 @@ int default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   int addr = GET_ADDR(to_push);
 
   if (addr == 832) {
-    if (bus == 0 && HKG_forward_bus2) {HKG_forward_bus2 = false; HKG_LKAS_bus0_cnt = 10;}
+    if (bus == 0 && HKG_forward_bus2) {HKG_forward_bus2 = false; HKG_LKAS_bus0_cnt = 10; puts("  LKAS on bus0: forwarding disabled\n");}
     if (bus == 2) {
-      if (HKG_LKAS_bus0_cnt > 0) {HKG_LKAS_bus0_cnt--;} else if (!HKG_forward_bus2) {HKG_forward_bus2 = true;}
-      if (HKG_Lcan_bus1_cnt > 0) {HKG_Lcan_bus1_cnt--;} else if (HKG_LCAN_on_bus1) {HKG_LCAN_on_bus1 = false;}
+      if (HKG_LKAS_bus0_cnt > 0) {HKG_LKAS_bus0_cnt--;} else if (!HKG_forward_bus2) {HKG_forward_bus2 = true; puts("  LKAS on bus2 & not on bus0: forwarding enabled\n");}
+      if (HKG_Lcan_bus1_cnt > 0) {HKG_Lcan_bus1_cnt--;} else if (HKG_LCAN_on_bus1) {HKG_LCAN_on_bus1 = false; puts("  Lcan not on bus1\n");}
       if (HKG_obd_int_cnt > 1) {HKG_obd_int_cnt--;}
     }
   }
@@ -27,18 +27,20 @@ int default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     if (HKG_forward_bus1 || !HKG_LCAN_on_bus1) {
       HKG_LCAN_on_bus1 = true;
       HKG_forward_bus1 = false;
+      puts("  LCAN on bus1: forwarding disabled\n");
     }
   }
   // check if we have a MDPS or SCC on Bus1
   if (bus == 1 && (addr == 593 || addr == 897 || addr == 1057) && !HKG_LCAN_on_bus1) {
     if (HKG_forward_bus1 != true) {
       HKG_forward_bus1 = true;
-      if (HKG_obd_int_cnt > 0) {HKG_forward_obd = true;}
+      puts("  MDPS or SCC on bus1: forwarding enabled\n");
+      if (board_has_obd() && HKG_obd_int_cnt > 0) {HKG_forward_obd = true; puts("  MDPS or SCC on OBD2 CAN: setting can mode obd\n");}
     }
   }
   // set CAN2 mode to normal if int_cnt expaired
   if (HKG_obd_int_cnt == 1) {
-    if (!HKG_forward_obd) {current_board->set_can_mode(CAN_MODE_NORMAL);}
+    if (board_has_obd() && !HKG_forward_obd) {current_board->set_can_mode(CAN_MODE_NORMAL); puts("  OBD2 CAN empty: setting can mode normal\n");}
     HKG_obd_int_cnt = 0;
   }
 
@@ -72,6 +74,7 @@ static void nooutput_init(int16_t param) {
   relay_malfunction_reset();
   if (board_has_obd() && (HKG_forward_obd || HKG_obd_int_cnt > 0)) {
     current_board->set_can_mode(CAN_MODE_OBD_CAN2);
+    puts("  MDPS or SCC on OBD2 CAN: setting can mode obd\n");
   }
 }
 
@@ -177,6 +180,7 @@ static void alloutput_init(int16_t param) {
   relay_malfunction_reset();
   if (board_has_obd() && HKG_forward_obd) {
     current_board->set_can_mode(CAN_MODE_OBD_CAN2);
+    puts("  MDPS or SCC on OBD2 CAN: setting can mode obd\n");
   }
 }
 

diff --git a/panda/board/safety/safety_hyundai_community.h b/panda/board/safety/safety_hyundai_community.h
@@ -40,32 +40,39 @@ static int hyundai_community_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
                             hyundai_get_checksum, hyundai_compute_checksum,
                             hyundai_get_counter);
 
+  if (!valid){
+    puts("  CAN RX invalid: "); puth(addr); puts("\n");
+  }
   if (bus == 1 && HKG_LCAN_on_bus1) {valid = false;}
   // check if we have a LCAN on Bus1
   if (bus == 1 && (addr == 1296 || addr == 524)) {
     HKG_Lcan_bus1_cnt = 100;
-    if (HKG_forward_bus1 || !HKG_LCAN_on_bus1) { HKG_LCAN_on_bus1 = true; HKG_forward_bus1 = false;}
+    if (HKG_forward_bus1 || !HKG_LCAN_on_bus1) {
+      HKG_LCAN_on_bus1 = true;
+      HKG_forward_bus1 = false;
+      puts("  LCAN on bus1: forwarding disabled\n");
+    }
   }
   // check if LKAS on Bus0
   if (addr == 832) {
-    if (bus == 0 && HKG_forward_bus2) {HKG_forward_bus2 = false; HKG_LKAS_bus0_cnt = 10;}
+    if (bus == 0 && HKG_forward_bus2) {HKG_forward_bus2 = false; HKG_LKAS_bus0_cnt = 10; puts("  LKAS on bus0: forwarding disabled\n");}
     if (bus == 2) {
-      if (HKG_LKAS_bus0_cnt > 0) {HKG_LKAS_bus0_cnt--;} else if (!HKG_forward_bus2) {HKG_forward_bus2 = true;}
-      if (HKG_Lcan_bus1_cnt > 0) {HKG_Lcan_bus1_cnt--;} else if (HKG_LCAN_on_bus1) {HKG_LCAN_on_bus1 = false;}
+      if (HKG_LKAS_bus0_cnt > 0) {HKG_LKAS_bus0_cnt--;} else if (!HKG_forward_bus2) {HKG_forward_bus2 = true; puts("  LKAS on bus2 & not on bus0: forwarding enabled\n");}
+      if (HKG_Lcan_bus1_cnt > 0) {HKG_Lcan_bus1_cnt--;} else if (HKG_LCAN_on_bus1) {HKG_LCAN_on_bus1 = false; puts("  Lcan not on bus1\n");}
     }
   }
   // check MDPS on Bus
   if ((addr == 593 || addr == 897) && HKG_mdps_bus != bus) {
     if (bus != 1 || !HKG_LCAN_on_bus1) {
       HKG_mdps_bus = bus;
-      if (bus == 1 && !HKG_forward_bus1) {HKG_forward_bus1 = true;}
+      if (bus == 1 && !HKG_forward_bus1) {HKG_forward_bus1 = true; puts("  MDPS on bus1: forwarding enabled\n");}
     }
   }
   // check SCC on Bus
   if ((addr == 1056 || addr == 1057) && HKG_scc_bus != bus) {
     if (bus != 1 || !HKG_LCAN_on_bus1) {
       HKG_scc_bus = bus;
-      if (bus == 1 && !HKG_forward_bus1) {HKG_forward_bus1 = true;}
+      if (bus == 1 && !HKG_forward_bus1) {HKG_forward_bus1 = true;puts("  SCC on bus1: forwarding enabled\n");}
     }
   }
 
@@ -83,9 +90,11 @@ static int hyundai_community_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3;
       if (cruise_engaged && !cruise_engaged_prev) {
         controls_allowed = 1;
+        puts("  SCC w/ long control: controls allowed"); puts("\n");
       }
       if (!cruise_engaged) {
         controls_allowed = 0;
+        if (controls_allowed) {puts("  SCC w/ long control: controls not allowed"); puts("\n");}
       }
       cruise_engaged_prev = cruise_engaged;
     }
@@ -94,8 +103,10 @@ static int hyundai_community_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       int cruise_engaged = GET_BYTES_04(to_push) & 0x1; // ACC main_on signal
       if (cruise_engaged && !cruise_engaged_prev) {
         controls_allowed = 1;
+        puts("  SCC w/o long control: controls allowed"); puts("\n");
       }
       if (!cruise_engaged) {
+        if (controls_allowed) {puts("  SCC w/o long control: controls not allowed"); puts("\n");}
         controls_allowed = 0;
       }
       cruise_engaged_prev = cruise_engaged;
@@ -106,8 +117,10 @@ static int hyundai_community_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       int cruise_engaged = (GET_BYTES_04(to_push) >> 25 & 0x1); // ACC main_on signal
       if (cruise_engaged && !cruise_engaged_prev) {
         controls_allowed = 1;
+        puts("  non-SCC w/ long control: controls allowed"); puts("\n");
       }
       if (!cruise_engaged) {
+        if (controls_allowed) {puts("  non-SCC w/ long control: controls not allowed"); puts("\n");}
         controls_allowed = 0;
       }
       cruise_engaged_prev = cruise_engaged;
@@ -119,18 +132,20 @@ static int hyundai_community_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       // enable on both accel and decel buttons falling edge
       if (!cruise_button && (cruise_engaged_prev == 1 || cruise_engaged_prev == 2)) {
         controls_allowed = 1;
+        puts("  non-SCC w/o long control: controls allowed"); puts("\n");
       }
       // disable on cancel rising edge
       if (cruise_button == 4) {
+        if (controls_allowed) {puts("  non-SCC w/o long control: controls not allowed"); puts("\n");}
         controls_allowed = 0;
       }
       cruise_engaged_prev = cruise_button;
     }
     // exit controls on rising edge of gas press for cars with long control
-    if (addr == 608 && OP_SCC_live && bus == 0) {
+    if (addr == 608 && OP_SCC_live && bus == 0) { // EMS16
       gas_pressed = (GET_BYTE(to_push, 7) >> 6) != 0;
     }
-    if (addr == 881 && OP_SCC_live && bus == 0) {
+    if (addr == 881 && OP_SCC_live && bus == 0) { // E_EMS11
       gas_pressed = (((GET_BYTE(to_push, 4) & 0x7F) << 1) | GET_BYTE(to_push, 3) >> 7) > 5;
     }
     // sample wheel speed, averaging opposite corners
@@ -158,10 +173,12 @@ static int hyundai_community_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
   if (!msg_allowed(to_send, HYUNDAI_COMMUNITY_TX_MSGS, sizeof(HYUNDAI_COMMUNITY_TX_MSGS)/sizeof(HYUNDAI_COMMUNITY_TX_MSGS[0]))) {
     tx = 0;
+    puts("  CAN TX not allowed: "); puth(addr); puts(", "); puth(bus); puts("\n");
   }
 
   if (relay_malfunction) {
     tx = 0;
+    puts("  CAN TX not allowed LKAS on bus0"); puts("\n");
   }
 
   // LKA STEER: safety check
@@ -174,18 +191,24 @@ static int hyundai_community_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
     if (controls_allowed) {
 
       // *** global torque limit check ***
-      violation |= max_limit_check(desired_torque, HYUNDAI_MAX_STEER, -HYUNDAI_MAX_STEER);
+      bool torque_check = 0;
+      violation |= torque_check = max_limit_check(desired_torque, HYUNDAI_MAX_STEER, -HYUNDAI_MAX_STEER);
+      if (torque_check) {puts("  LKAS TX not allowed: torque limit check failed!"); puts("\n");}
 
       // *** torque rate limit check ***
-      violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver,
+      bool torque_rate_check = 0;
+      violation |= torque_rate_check = driver_limit_check(desired_torque, desired_torque_last, &torque_driver,
         HYUNDAI_MAX_STEER, HYUNDAI_MAX_RATE_UP, HYUNDAI_MAX_RATE_DOWN,
         HYUNDAI_DRIVER_TORQUE_ALLOWANCE, HYUNDAI_DRIVER_TORQUE_FACTOR);
+      if (torque_rate_check) {puts("  LKAS TX not allowed: torque rate limit check failed!"); puts("\n");}
 
       // used next time
       desired_torque_last = desired_torque;
 
       // *** torque real time rate limit check ***
-      violation |= rt_rate_limit_check(desired_torque, rt_torque_last, HYUNDAI_MAX_RT_DELTA);
+      bool torque_rt_check = 0;
+      violation |= torque_rt_check = rt_rate_limit_check(desired_torque, rt_torque_last, HYUNDAI_MAX_RT_DELTA);
+      if (torque_rt_check) {puts("  LKAS TX not allowed: torque real time rate limit check failed!"); puts("\n");}
 
       // every RT_INTERVAL set the new limits
       uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last);
@@ -198,6 +221,7 @@ static int hyundai_community_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
     // no torque if controls is not allowed
     if (!controls_allowed && (desired_torque != 0)) {
       violation = 1;
+      puts("  LKAS torque not allowed: controls not allowed!"); puts("\n");
     }
 
     // reset to 0 if either controls is not allowed or there's a violation
@@ -304,7 +328,8 @@ static void hyundai_community_init(int16_t param) {
 
   if (board_has_obd() && HKG_forward_obd) {
     current_board->set_can_mode(CAN_MODE_OBD_CAN2);
-    }
+    puts("  MDPS or SCC on OBD2 CAN: setting can mode obd\n");
+  }
 }
 
 const safety_hooks hyundai_community_hooks = {

diff --git a/panda/tests/debug_console.py b/panda/tests/debug_console.py
@@ -0,0 +1,49 @@
+#!/usr/bin/env python3
+
+import os
+import sys
+import time
+import select
+
+sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), ".."))
+from panda import Panda  # noqa: E402
+
+setcolor = ["\033[1;32;40m", "\033[1;31;40m"]
+unsetcolor = "\033[00m"
+
+if __name__ == "__main__":
+  while True:
+    try:
+      port_number = int(os.getenv("PORT", "0"))
+      claim = os.getenv("CLAIM") is not None
+
+      serials = Panda.list()
+      if os.getenv("SERIAL"):
+        serials = [x for x in serials if x == os.getenv("SERIAL")]
+
+      pandas = list([Panda(x, claim=claim) for x in serials])
+
+      if not len(pandas):
+        sys.exit("no pandas found")
+
+      if os.getenv("BAUD") is not None:
+        for panda in pandas:
+          panda.set_uart_baud(port_number, int(os.getenv("BAUD")))  # type: ignore
+
+      while True:
+        for i, panda in enumerate(pandas):
+          while True:
+            ret = panda.serial_read(port_number)
+            if len(ret) > 0:
+              sys.stdout.write(setcolor[i] + ret.decode('ascii') + unsetcolor)
+              sys.stdout.flush()
+            else:
+              break
+          if select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []):
+            ln = sys.stdin.readline()
+            if claim:
+              panda.serial_write(port_number, ln)
+          time.sleep(0.01)
+    except Exception:
+      print("panda disconnected!")
+      time.sleep(0.5)